(* Content-type: application/vnd.wolfram.mathematica *) (*** Wolfram Notebook File ***) (* http://www.wolfram.com/nb *) (* CreatedBy='Mathematica 8.0' *) (*CacheID: 234*) (* Internal cache information: NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 157, 7] NotebookDataLength[ 18933, 598] NotebookOptionsPosition[ 18110, 565] NotebookOutlinePosition[ 18464, 581] CellTagsIndexPosition[ 18421, 578] WindowFrame->Normal*) (* Beginning of Notebook Content *) Notebook[{ Cell[TextData[{ "The \[OpenCurlyQuote]right trivialised\[CloseCurlyQuote] tangent of the \ exponential map, ", Cell[BoxData[ FormBox["dexpR", TraditionalForm]], FormatType->"TraditionalForm"], ", according to Iserles05an, formula 2.42, pg. 32 can be written as\n\t", Cell[BoxData[ FormBox[GridBox[{ {"\t"}, { RowBox[{ RowBox[{ RowBox[{"g", "'"}], SuperscriptBox["g", RowBox[{"-", "1"}]]}], "=", RowBox[{ SubscriptBox["dexpR", "\[Omega]"], "(", RowBox[{"\[Omega]", "'"}], ")"}]}]} }], TraditionalForm]], FormatType->"TraditionalForm"], "\nwhere ", Cell[BoxData[ FormBox[ RowBox[{"g", "=", RowBox[{"exp", "(", "\[Omega]", ")"}]}], TraditionalForm]], FormatType->"TraditionalForm"], ", and ", Cell[BoxData[ FormBox[ RowBox[{ RowBox[{"g", "'"}], "=", RowBox[{ RowBox[{"exp", "'"}], RowBox[{"(", "\[Omega]", ")"}]}]}], TraditionalForm]], FormatType->"TraditionalForm"], ".\nCompare this to the left Jacobian matrix ", Cell[BoxData[ FormBox[ SubscriptBox["J", "l"], TraditionalForm]], FormatType->"TraditionalForm"], " in Chirikjian11book2, pg. 26, we see that ", Cell[BoxData[ FormBox["dexpR", TraditionalForm]], FormatType->"TraditionalForm"], " and ", Cell[BoxData[ FormBox[ SubscriptBox["J", "l"], TraditionalForm]], FormatType->"TraditionalForm"], " are the same.\n\nHence, be careful, Iserles\[CloseCurlyQuote]s \ \[OpenCurlyQuote]", StyleBox["right", FontWeight->"Bold"], " trivialised\[CloseCurlyQuote] tangent of the exponential map ", Cell[BoxData[ FormBox["dexpR", TraditionalForm]], FormatType->"TraditionalForm"], " is in fact Chirikjian\[CloseCurlyQuote]s ", StyleBox["left", FontWeight->"Bold"], " Jacobian matrix ", Cell[BoxData[ FormBox[ SubscriptBox["J", "l"], TraditionalForm]], FormatType->"TraditionalForm"], "!!!\n\nWe want to compute the s \[OpenCurlyQuote]", StyleBox["left", FontWeight->"Bold"], " trivialised\[CloseCurlyQuote] tangent of the exponential map, ", Cell[BoxData[ FormBox["dexpL", TraditionalForm]], FormatType->"TraditionalForm"], ", for SE2, hence, we need to use Chirikjian\[CloseCurlyQuote]s ", StyleBox["right", FontWeight->"Bold"], " Jacobian matrix ", Cell[BoxData[ FormBox[ SubscriptBox["J", "r"], TraditionalForm]], FormatType->"TraditionalForm"], " formula in Chirikjian11book2, pg. 36." }], "Text", CellChangeTimes->{{3.6279967389044943`*^9, 3.6279968865058002`*^9}, { 3.6279969695759087`*^9, 3.6279974871811028`*^9}, 3.62799757389325*^9}], Cell[BoxData[{ RowBox[{"Clear", "[", "J", "]"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"J", "[", "\[Alpha]_", "]"}], ":=", RowBox[{"{", RowBox[{ RowBox[{"{", RowBox[{ RowBox[{ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", RowBox[{ RowBox[{"(", RowBox[{"1", "-", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], "/", "\[Alpha]"}], ",", " ", RowBox[{ RowBox[{"(", RowBox[{ RowBox[{"\[Alpha]", " ", SubscriptBox["v", "1"]}], "-", SubscriptBox["v", "2"], "+", RowBox[{ SubscriptBox["v", "2"], RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-", RowBox[{ SubscriptBox["v", "1"], RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/", SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",", RowBox[{"{", RowBox[{ RowBox[{ RowBox[{"-", RowBox[{"(", RowBox[{"1", "-", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}], "/", "\[Alpha]"}], ",", RowBox[{ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", " ", RowBox[{ RowBox[{"(", RowBox[{ SubscriptBox["v", "1"], "+", RowBox[{"\[Alpha]", " ", SubscriptBox["v", "2"]}], "-", RowBox[{ SubscriptBox["v", "1"], RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-", RowBox[{ SubscriptBox["v", "2"], RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/", SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",", RowBox[{"{", RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], "Input", CellChangeTimes->{{3.627993817228732*^9, 3.6279939547434673`*^9}, { 3.627993986274671*^9, 3.6279940386007967`*^9}, {3.627995391081044*^9, 3.627995412846286*^9}, 3.6279954452391644`*^9, {3.627995531089571*^9, 3.6279955341932592`*^9}, {3.627996429604282*^9, 3.62799643077184*^9}}], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ RowBox[{ RowBox[{"Jinv", "[", "\[Alpha]_", "]"}], "=", RowBox[{"Inverse", "[", RowBox[{"J", "[", "\[Alpha]", "]"}], "]"}]}], "\[IndentingNewLine]"}]], "Input", CellChangeTimes->{ 3.627995475343099*^9, {3.627995548533964*^9, 3.627995559455151*^9}, { 3.627996438504909*^9, 3.6279964431657553`*^9}}], Cell[BoxData[ RowBox[{"{", RowBox[{ RowBox[{"{", RowBox[{ FractionBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], RowBox[{"\[Alpha]", " ", RowBox[{"(", RowBox[{ FractionBox["1", SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",", FractionBox[ RowBox[{ RowBox[{"-", FractionBox["1", "\[Alpha]"]}], "+", FractionBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}], RowBox[{ FractionBox["1", SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]]}]], ",", FractionBox[ RowBox[{ FractionBox[ SubscriptBox["v", "1"], SuperscriptBox["\[Alpha]", "3"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", SubscriptBox["v", "1"]}], SuperscriptBox["\[Alpha]", "3"]], "+", FractionBox[ RowBox[{ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ", SubscriptBox["v", "1"]}], SuperscriptBox["\[Alpha]", "3"]], "-", FractionBox[ RowBox[{ RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ", SubscriptBox["v", "1"]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ RowBox[{ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ", SubscriptBox["v", "1"]}], SuperscriptBox["\[Alpha]", "3"]], "+", FractionBox[ SubscriptBox["v", "2"], SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{ RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", SubscriptBox["v", "2"]}], SuperscriptBox["\[Alpha]", "2"]]}], RowBox[{ FractionBox["1", SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",", RowBox[{"{", RowBox[{ FractionBox[ RowBox[{ FractionBox["1", "\[Alpha]"], "-", FractionBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}], RowBox[{ FractionBox["1", SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]]}]], ",", FractionBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], RowBox[{"\[Alpha]", " ", RowBox[{"(", RowBox[{ FractionBox["1", SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",", FractionBox[ RowBox[{ RowBox[{"-", FractionBox[ SubscriptBox["v", "1"], SuperscriptBox["\[Alpha]", "2"]]}], "+", FractionBox[ RowBox[{ RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", SubscriptBox["v", "1"]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SubscriptBox["v", "2"], SuperscriptBox["\[Alpha]", "3"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", SubscriptBox["v", "2"]}], SuperscriptBox["\[Alpha]", "3"]], "+", FractionBox[ RowBox[{ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ", SubscriptBox["v", "2"]}], SuperscriptBox["\[Alpha]", "3"]], "-", FractionBox[ RowBox[{ RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ", SubscriptBox["v", "2"]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ RowBox[{ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ", SubscriptBox["v", "2"]}], SuperscriptBox["\[Alpha]", "3"]]}], RowBox[{ FractionBox["1", SuperscriptBox["\[Alpha]", "2"]], "-", FractionBox[ RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]], "+", FractionBox[ SuperscriptBox[ RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",", RowBox[{"{", RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]], "Output", CellChangeTimes->{ 3.627995560030972*^9, {3.627996412919798*^9, 3.627996444306521*^9}}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ RowBox[{ RowBox[{"Jinv", "[", "\[Alpha]", "]"}], "//", "Simplify"}], "//", "MatrixForm"}]], "Input", CellChangeTimes->{{3.627993835637863*^9, 3.627993839233502*^9}, { 3.627994046108457*^9, 3.627994058781851*^9}, {3.627995546842499*^9, 3.6279955664940767`*^9}}], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ { RowBox[{ FractionBox["1", "2"], " ", "\[Alpha]", " ", RowBox[{"Cot", "[", FractionBox["\[Alpha]", "2"], "]"}]}], RowBox[{"-", FractionBox["\[Alpha]", "2"]}], FractionBox[ RowBox[{ RowBox[{ RowBox[{"(", RowBox[{ RowBox[{"-", "2"}], "+", RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+", RowBox[{"\[Alpha]", " ", RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", SubscriptBox["v", "1"]}], "+", RowBox[{"\[Alpha]", " ", RowBox[{"(", RowBox[{ RowBox[{"-", "1"}], "+", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], " ", SubscriptBox["v", "2"]}]}], RowBox[{"2", " ", "\[Alpha]", " ", RowBox[{"(", RowBox[{ RowBox[{"-", "1"}], "+", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]}, { FractionBox["\[Alpha]", "2"], RowBox[{ FractionBox["1", "2"], " ", "\[Alpha]", " ", RowBox[{"Cot", "[", FractionBox["\[Alpha]", "2"], "]"}]}], FractionBox[ RowBox[{ RowBox[{ RowBox[{"(", RowBox[{"\[Alpha]", "-", RowBox[{"\[Alpha]", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", SubscriptBox["v", "1"]}], "+", RowBox[{ RowBox[{"(", RowBox[{ RowBox[{"-", "2"}], "+", RowBox[{"2", " ", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+", RowBox[{"\[Alpha]", " ", RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", SubscriptBox["v", "2"]}]}], RowBox[{"2", " ", "\[Alpha]", " ", RowBox[{"(", RowBox[{ RowBox[{"-", "1"}], "+", RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]}, {"0", "0", "1"} }, GridBoxAlignment->{ "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, "RowsIndexed" -> {}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.7]}, Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { Offset[0.2], { Offset[0.4]}, Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", CellChangeTimes->{ 3.627993840513033*^9, {3.62799404156531*^9, 3.6279940592345743`*^9}, 3.627995567356995*^9, 3.627996415136314*^9, 3.6279964490074778`*^9}] }, Open ]], Cell[TextData[{ "In case ", Cell[BoxData[ FormBox[ RowBox[{"\[Alpha]", "=", "0"}], TraditionalForm]], FormatType->"TraditionalForm"], ", we compute the limits of ", Cell[BoxData[ FormBox[ SubscriptBox["J", "r"], TraditionalForm]], FormatType->"TraditionalForm"], " and ", Cell[BoxData[ FormBox[ SuperscriptBox[ SubscriptBox["J", "r"], RowBox[{"-", "1"}]], TraditionalForm]], FormatType->"TraditionalForm"], " as follows" }], "Text", CellChangeTimes->{{3.627997495449997*^9, 3.627997524522543*^9}}], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ RowBox[{ RowBox[{"Limit", "[", RowBox[{ RowBox[{"Jinv", "[", "\[Alpha]", "]"}], ",", RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//", "MatrixForm"}]], "Input", CellChangeTimes->{{3.627995572179821*^9, 3.627995606373824*^9}}], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", FractionBox[ SubscriptBox["v", "2"], "2"]}, {"0", "1", RowBox[{"-", FractionBox[ SubscriptBox["v", "1"], "2"]}]}, {"0", "0", "1"} }, GridBoxAlignment->{ "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, "RowsIndexed" -> {}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.7]}, Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { Offset[0.2], { Offset[0.4]}, Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", CellChangeTimes->{{3.627995585954463*^9, 3.627995606858135*^9}, 3.6279964178087473`*^9, 3.6279964634008904`*^9}] }, Open ]], Cell[CellGroupData[{ Cell[BoxData[ RowBox[{ RowBox[{ RowBox[{"Limit", "[", RowBox[{ RowBox[{"J", "[", "\[Alpha]", "]"}], ",", RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//", "MatrixForm"}]], "Input", CellChangeTimes->{{3.6279956527343893`*^9, 3.627995660697241*^9}}], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", GridBox[{ {"1", "0", RowBox[{"-", FractionBox[ SubscriptBox["v", "2"], "2"]}]}, {"0", "1", FractionBox[ SubscriptBox["v", "1"], "2"]}, {"0", "0", "1"} }, GridBoxAlignment->{ "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, "RowsIndexed" -> {}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.7]}, Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { Offset[0.2], { Offset[0.4]}, Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", CellChangeTimes->{{3.627995653969245*^9, 3.627995661346282*^9}, 3.627996419383007*^9, 3.627996465705708*^9}] }, Open ]], Cell[BoxData[""], "Input", CellChangeTimes->{{3.627995694633294*^9, 3.627995695945466*^9}}] }, WindowSize->{654, 852}, WindowMargins->{{Automatic, 27}, {Automatic, 0}}, FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \ 2011)", StyleDefinitions->"Default.nb" ] (* End of Notebook Content *) (* Internal cache information *) (*CellTagsOutline CellTagsIndex->{} *) (*CellTagsIndex CellTagsIndex->{} *) (*NotebookFileOutline Notebook[{ Cell[557, 20, 2591, 84, 197, "Text"], Cell[3151, 106, 2022, 56, 68, "Input"], Cell[CellGroupData[{ Cell[5198, 166, 343, 9, 43, "Input"], Cell[5544, 177, 6519, 190, 290, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[12100, 372, 298, 7, 27, "Input"], Cell[12401, 381, 2665, 76, 99, "Output"] }, Open ]], Cell[15081, 460, 535, 20, 29, "Text"], Cell[CellGroupData[{ Cell[15641, 484, 297, 8, 27, "Input"], Cell[15941, 494, 863, 25, 91, "Output"] }, Open ]], Cell[CellGroupData[{ Cell[16841, 524, 296, 8, 27, "Input"], Cell[17140, 534, 859, 25, 91, "Output"] }, Open ]], Cell[18014, 562, 92, 1, 27, "Input"] } ] *) (* End of internal cache information *)