Tracking on Homogeneous Manifolds

dedicated to Andrew Ladd, who had motivated me to study Lie theory

by Jan Ph. Hakenberg, January 2008, Automation Technology Laboratory of the Helsinki University of Technology

select Kernel→Evaluation→"Evaluate Notebook" in the menu

Implementation of the Campbell-Baker-Hausdorff series

<< Statistics`NormalDistribution`
Off[General::spell]; 
Off[General::spell1];

\(\(Τ★[v_, n_] := 
        Array[v\_##1 &, n];\)\)
𝓃[x__] := 
        Union[0 == #1 & /@ 
            Simplify[Flatten[{x}]]];
Randn := Random[NormalDistribution[0, 1]];

EXP := MatrixExp;
LOG::convergence = "low or none";
LOG[x_] := 
      Module[
{X = x - IdentityMatrix[Length[x]], Xp, Xl, 
          n}, 
Xl = Array[0 &, Dimensions[x]]; Xp = X; 
        n = 1; 
While[(10^-12) < Norm[Xp] < 100 && 
            n < 100, 
Xl = 
            Xl + (-1)^(n + 1) Xp/n; Xp = Xp . X; 
          n = n + 1;]; 
If[
          n == 100 || 100 < Norm[Xp], 
Message[
            LOG::convergence], 
Chop[Xl]]]

\(\(Μd[X_List, r_Integer, n_Integer] := 
        Transpose[Array[D[X, x\_#1] &, {n}], 
          Flatten[{r + 1, Range[r]}]];\)\)
Μd[X_List, r_Integer] := Μd[X, r, 
          Length[X]];
\(\(Μd[X_, n_Integer] := 
        Array[D[X, x\_#1] &, n];\)\)

adCBH[ad_, p_, X_, Y_] := 
        Module[
{glb = 
              Prepend[Array[0 &, {p, Length[ad]}], X], AX = ad . X, 
            AY = ad . Y, m}, 
adCBHin[v_, d_, qp_, s_] := 
            Module[{n = Length[qp], 
                k = Length[qp]/2}, 
glb[[
                  d]] += 
                v (-1)^k/(k + 1)/(1 + 
                          Plus @@ qp[[
                              Range[1, Length[qp], 
                                2]]])/
                    Times @@ (qp!); 
If[
                d ≤ p && 
                  Length[𝓃[0, v]] > 1, 
If[
                  n > 0, 
If[s, 
                    adCBHin[AY . v, d + 1, 
                      qp + IdentityMatrix[n][[
                          n - 1]], 
                      True]]; 
adCBHin[AX . v, d + 1, 
                    qp + IdentityMatrix[n][[
                        n]], 
                    False];
]; 
adCBHin[
                  AY . v, d + 1, Join[qp, {1, 0}], 
                  True]; 
adCBHin[AX . v, d + 1, 
                  Join[qp, {0, 1}], 
                  False];
];
]; 
Table[
            adCBHin[MatrixPower[AX, m] . Y/m!, m + 1, {}, True], {m, 0, 
              p}]; 
Simplify[glb]];

adCBH[ad_, p_] := 
        adCBH[ad, 
          p, Τ★[x, 
            Length[ad]], Τ★[y, 
            Length[ad]]];

adCBHdepth = 2;
adCBH[ad_, X_, Y_] := 
        Plus @@ adCBH[ad, adCBHdepth, X, Y];

Drawing utilities

MYDIR = "c:/jan/latex/tracking/";

jGFxPoints[P_, c_: { .2,  .2, 1}] := 
        Graphics[
          Flatten[{RGBColor @@ c, PointSize[ .02], 
              Point /@ P[[
                  All, {1, 
                    2}]]}]];
jGFxPoints3D[P_, c_: { .2,  .2, 1}] := 
        Graphics3D[
          Flatten[{RGBColor @@ c, PointSize[ .02], 
              Point /@ P[[
                  All, {1, 2, 
                    3}]]}]];
jGFxRectangle[A_, c_: { .2,  .2, 1}]:=Graphics[Flatten[{RGBColor @@ c,Rectangle@@(Transpose[A[[{1, 2}, 
                          3]]+.15(({{-1, 1}, {-1, 1}}))])}]];

jGFxFrame[A_, c_: { .2,  .2, 1}] := 
        Module[{p = 
              A[[{1, 2}, 
                3]]}, 
Graphics[{RGBColor @@ c, Thickness[0.01], 
              Line[{p, 
                  p + A[[{1, 2}, 1]]}], 
              Line[{p, 
                  p + A[[{1, 2}, 
                      2]]}]}]];
jGFxFrame3D[A_, c_: { .2,  .2, 1}] := 
        Module[{p = 
              A[[{1, 2, 3}, 
                4]]}, 
Graphics3D[{RGBColor @@ c, Thickness[0.01], 
              Line[{p, 
                  p + A[[{1, 2, 3}, 
                      1]]}], 
              Line[{p, 
                  p + A[[{1, 2, 3}, 
                      2]]}], 
              Line[{p, 
                  p + A[[{1, 2, 3}, 
                      3]]}]}]];

\(\(jTrail[pnt_] := 
        Table[g /. {u\_1 -> U[[1]], 
              u\_2 -> U[[2]], 
              m\&^\_1 -> pnt[[1]], 
              m\&^\_2 -> 
                pnt[[2]]}, {t, 0, 
            1,  .05}];\)\)
\(\(jTrail3D[pnt_] := 
        Table[g /. {u\_1 -> U[[1]], 
              u\_2 -> U[[2]], 
              u\_3 -> U[[3]], 
              u\_4 -> U[[4]], 
              u\_5 -> U[[5]], 
              u\_6 -> U[[6]], 
              m\&^\_1 -> pnt[[1]], 
              m\&^\_2 -> pnt[[2]], 
              m\&^\_3 -> 
                pnt[[3]]}, {t, 0, 
            1,  .05}];\)\)

Examples

Train-like robot

adCBHdepth = 2;

ℝ2 = {{{0, 0}, {0, 0}}, {{0, 0}, {0, 
              0}}};
X={(({{0, 0, 1}, {0, 0, 0}, {0, 0, 0}})),(({{0, 0, 0}, {0, 0, 1}, {0, 0, 0}}))};
\(g = 
      adCBH[ℝ2, 
        t\ {u\_1, 0}, {m\&^\_1, m\&^\_2}]\)

U = {1, 0};
Q = 
      Array[{Random[Real, {-4, 4}], Random[Real, {-1.5, 1.5}], 
            1} &, {10}]
A = EXP[U . X];
P = A . (#1 + 0.1 {Randn, Randn, 0}) & /@ Q

jGFx = 
        Show[jGFxPoints[P, { .2,  .2, 1}], jGFxPoints[Q, {1,  .2,  .2}], 
          Graphics[
            Line /@ (jTrail /@ Q)], 
jGFxRectangle[
            IdentityMatrix[3], {1,  .2,  .2}], 
          jGFxRectangle[A, { .2,  .2, 1}], Graphics[Line[jTrail[{0, 0}]]], 
          PlotRange -> {{-5, 5}, {-2, 2}}, Axes -> True, 
          AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2])}, 
          AspectRatio -> Automatic];

F = 
      Table[adCBH[ℝ2, {(Subscript[x, 1]), 0}, 
              Q[[k, {1, 2}]]] - 
            P[[k, {1, 2}]], {k, 
            Length[P]}] // Chop

f=Mean[((({{1, 0}, {0, 1}})).#1.#1&/@F)];
df = Μd[f, 
        1]
Hf = Μd[df, 1]

Plot[{df[[1]], 
          2}, {(Subscript[x, 1]), -1, 3}];

NestList[({(Subscript[x, 1])} - 
              Inverse[Hf] . 
                df) /. {(Subscript[x, 1]) -> #1[[1]]} &, {0}, 3]

Differential-drive robot

adCBHdepth = 2;

E2 = {{{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, {{0, 0, 1}, {0, 0, 
              0}, {-1, 0, 0}}, {{0, -1, 0}, {1, 0, 0}, {0, 0, 
              0}}};
X = {{{0, -1, 0}, {1, 0, 0}, {0, 0, 0}}, {{0, 0, 1}, {0, 0, 
              0}, {0, 0, 0}}, {{0, 0, 0}, {0, 0, 1}, {0, 0, 
              0}}};
\(g = \(adCBH[E2, 
          adCBH[E2, 
            t\ {u\_1, u\_2, 0}, {0, m\&^\_1, m\&^\_2}], {\(-t\)\ u\_1, 0, 
            0}]\)[[{2, 3}]]\)

U = { .5, 1. , 0};
Q = 
      Array[{Random[Real, {-5, 4}], Random[Real, {-1, 1}], 
            1} &, {8}]
A = EXP[U . X];
P = A . (#1 + 0.1 {Randn, Randn, 0}) & /@ Q

jGFx = 
        Show[jGFxPoints[P, { .2,  .2, 1}], 
          jGFxPoints[Q, {1,  .2,  .2}], 
jGFxFrame[
            IdentityMatrix[3], {1,  .2,  .2}], 
Graphics[
            Table[Line[{P[[k, {1, 2}]], 
                  Q[[k, {1, 2}]]}], {k, 
                Length[Q]}]], 
PlotRange -> {{-5, 
                5}, {-2.5, 2.5}}, Axes -> True, 
          AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2])}, 
          AspectRatio -> Automatic];
jGFx = 
        Show[jGFxPoints[P, { .2,  .2, 1}], jGFxPoints[Q, {1,  .2,  .2}], 
          Graphics[Line /@ (jTrail /@ Q)], 
jGFxFrame[
            IdentityMatrix[3], {1,  .2,  .2}], 
          jGFxFrame[A, { .2,  .2, 1}], 
Graphics[
            Line[jTrail[{0, 0}]]], 
          PlotRange -> {{-5, 5}, {-2.5, 2.5}}, 
          Axes -> True, AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2])}, 
          AspectRatio -> Automatic];

adCBHdepth = 3;
F = 
        Table[adCBH[E2, 
                  adCBH[E2, {(Subscript[x, 1]), (Subscript[x, 2]), 0}, {0, 
                      Q[[k, 1]], 
                      Q[[k, 
                        2]]}], {-(Subscript[x, 1]), 0, 
                    0}][[{2, 
                  3}]] - {P[[k, 
                  1]], 
                P[[k, 2]]}, {k, 
              Length[P]}] // Chop;

f=Mean[((({{1, 0}, {0, 1}})).#1.#1&/@F)];
df = Μd[f, 
          2];
Hf = Μd[df, 
          1];

NestList[({(Subscript[x, 1]), (Subscript[x, 2])} - 
              Inverse[Hf] . 
                df) /. {(Subscript[x, 1]) -> #1[[1]], 
            (Subscript[x, 2]) -> #1[[2]]} &, {0, 
        0}, 5]

Plot3D[
        df[[1]], {(Subscript[x, 1]), - .5,  .5}, {(Subscript[x, 2]), 0, 1}];
Plot3D[
        df[[2]], {(Subscript[x, 1]), - .5,  .5}, {(Subscript[x, 2]), 0, 1}];
Plot3D[Det[Hf], {(Subscript[x, 1]), - .5,  .5}, {(Subscript[x, 2]), 0, 1}];

Robot in Euclidean space

adCBHdepth = 2;

E3 = {{{0, 0, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 1, 0, 
              0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 
              0, 0}}, {{0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {-1, 0, 0, 
              0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 
              0, 0}}, {{0, -1, 0, 0, 0, 0}, {1, 0, 0, 0, 0, 0}, {0, 0, 0, 
              0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 
              0, 0}}, {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, -1}, {0, 0, 0, 
              0, 1, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 1, 
              0, 0, 0, 0}}, {{0, 0, 0, 0, 0, 1}, {0, 0, 0, 0, 0, 0}, {0, 0, 
              0, -1, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, 0, 0, 
              0}, {-1, 0, 0, 0, 0, 0}}, {{0, 0, 0, 0, -1, 0}, {0, 0, 
              0, 1, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {1, 
              0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}};
X={(({{0, 0, 0, 0}, {0, 0, -1, 0}, {0, 1, 0, 0}, {0, 0, 0, 0}})),(({{0, 0, 1, 0}, {0, 0, 0, 0}, {-1, 0, 0, 0}, {0, 0, 0, 0}})),(({{0, -1, 0, 0}, {1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}})),(({{0, 0, 0, 1}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}})),(({{0, 0, 0, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}, {0, 0, 0, 0}})),(({{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}, {0, 0, 0, 0}}))};

mobility U = ⟨(Subscript[x, 1]), (Subscript[x, 2]), 
          (Subscript[x, 3])⟩

adCBHdepth = 3;
\(g = \(adCBH[E3, 
          adCBH[E3, 
            t\ {u\_1, u\_2, u\_3, 0, 0, 0}, {0, 0, 0, m\&^\_1, m\&^\_2, 
              m\&^\_3}], \(-t\)\ {u\_1, u\_2, u\_3, 0, 0, 
              0}]\)[[{4, 5, 
          6}]]\)

U = { .5,  .2,  .7, 0, 0, 0};
rng = 1.7;
Q = 
      Array[{Random[Real, {-rng, rng}], Random[Real, {-rng, rng}], 
            Random[Real, {-rng, rng}], 
            1} &, {8}]
A = EXP[U . X];
P = A . (#1 + 0.05 {Randn, Randn, Randn, 0}) & /@ Q

jGFx = 
        Show[jGFxPoints3D[P, { .2,  .2, 1}], jGFxPoints3D[Q, {1,  .2,  .2}], 
          Graphics3D[Line /@ (jTrail3D /@ Q)], 
          Graphics3D[
            Line /@ (jTrail3D /@ 
                  IdentityMatrix[4])], 
jGFxFrame3D[
            IdentityMatrix[4], {1,  .2,  .2}], 
          jGFxFrame3D[
            A, { .2,  .2, 
              1}], 
PlotRange -> {{-2, 
                2}, {-2, 2}, {-2, 2}}, Axes -> True, 
          AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2]), (Subscript[m, 3])}, 
          AspectRatio -> Automatic];

adCBHdepth = 2;
F = 
        Table[adCBH[E3, 
                  adCBH[E3, 
{(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 0, 0, 
                      0}, {0, 0, 0, 
                      Q[[k, 1]], 
                      Q[[k, 2]], 
                      Q[[k, 
                        3]]}], -{(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 0, 0, 
                      0}][[{4, 5, 
                  6}]] - {P[[k, 
                  1]], 
                P[[k, 2]], 
                P[[k, 3]]}, {k, 
              Length[P]}] // Chop;

f = 
        Mean[(IdentityMatrix[3] . #1 . #1 & /@ 
              F)];
df = Μd[f, 3];
Hf = Μd[df, 1];

NestList[({(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3])} - 
              Inverse[Hf] . 
                df) /. {(Subscript[x, 1]) -> #1[[1]], 
            (Subscript[x, 2]) -> #1[[2]], 
            (Subscript[x, 3]) -> #1[[3]]} &, {0, 
        0, 0}, 4]

mobility U = ⟨(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 
          (Subscript[x, 4])⟩

adCBHdepth = 2;
\(g = \(adCBH[E3, 
          adCBH[E3, 
            t\ {u\_1, u\_2, u\_3, u\_4, 0, 0}, {0, 0, 0, m\&^\_1, m\&^\_2, 
              m\&^\_3}], \(-t\)\ {u\_1, u\_2, u\_3, 0, 0, 
              0}]\)[[{4, 5, 
          6}]]\)

U = {- .3,  .5,  .2,  .7, 0, 0};
rng = 1.7;
Q = 
      Array[{Random[Real, {-rng, rng}], Random[Real, {-rng, rng}], 
            Random[Real, {-rng, rng}], 
            1} &, {10}]
A = EXP[U . X];
P = A . (#1 + 0.05 {Randn, Randn, Randn, 0}) & /@ Q

jGFx = 
        Show[jGFxPoints3D[P, { .2,  .2, 1}], 
          jGFxPoints3D[Q, {1,  .2,  .2}], 
Graphics3D[
            Table[Line[{P[[
                    k, {1, 2, 3}]], 
                  Q[[k, {1, 2, 
                      3}]]}], {k, 
                Length[Q]}]], 
jGFxFrame3D[
            IdentityMatrix[
              4], {1,  .2,  .2}], 
PlotRange -> {{-2, 2}, {-2, 2}, {-2, 2}}, Axes -> True, 
          AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2]), (Subscript[m, 3])}, 
          AspectRatio -> Automatic];
jGFx = 
        Show[jGFxPoints3D[P, { .2,  .2, 1}], jGFxPoints3D[Q, {1,  .2,  .2}], 
          Graphics3D[Line /@ (jTrail3D /@ Q)], 
          Graphics3D[
            Line /@ (jTrail3D /@ 
                  IdentityMatrix[4])], 
jGFxFrame3D[
            IdentityMatrix[4], {1,  .2,  .2}], 
          jGFxFrame3D[
            A, { .2,  .2, 
              1}], 
PlotRange -> {{-2, 
                2}, {-2, 2}, {-2, 2}}, Axes -> True, 
          AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2]), (Subscript[m, 3])}, 
          AspectRatio -> Automatic];

F = 
        Table[adCBH[E3, 
                  adCBH[E3, 
{(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 0, 0, 
                      0}, {0, 0, 0, 
                      Q[[k, 1]], 
                      Q[[k, 2]], 
                      Q[[k, 
                        3]]}], -{(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 0, 0, 
                      0}][[{4, 5, 
                  6}]] - {P[[k, 
                  1]], 
                P[[k, 2]], 
                P[[k, 3]]}, {k, 
              Length[P]}] // Chop;

f = 
        Mean[(IdentityMatrix[3] . #1 . #1 & /@ 
              F)];
df = Μd[f, 3];
Hf = Μd[df, 1];

NestList[({(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3])} - 
              Inverse[Hf] . 
                df) /. {(Subscript[x, 1]) -> #1[[1]], 
            (Subscript[x, 2]) -> #1[[2]], 
            (Subscript[x, 3]) -> #1[[3]]} &, {0, 
        0, 0}, 4]

mobility U = ⟨(Subscript[x, 2]), (Subscript[x, 3]), (Subscript[x, 4]), 
          (Subscript[x, 5])⟩

adCBHdepth = 2;
\(g = \(adCBH[E3, 
          adCBH[E3, 
            t\ {0, u\_2, u\_3, u\_4, u\_5, 0}, {0, 0, 0, m\&^\_1, m\&^\_2, 
              m\&^\_3}], \(-t\)\ {0, u\_2, u\_3, 0, 0, 
              0}]\)[[{4, 5, 
          6}]]\)

U = {0, - .5,  .5,  .5,  .3, 0};
rng = 1.7;
Q = 
      Array[{Random[Real, {-rng, rng}], Random[Real, {-rng, rng}], 
            Random[Real, {-rng, rng}], 
            1} &, {10}]
A = EXP[U . X];
P = A . (#1 + 0.0 {Randn, Randn, Randn, 0}) & /@ Q

jGFx = 
        Show[jGFxPoints3D[P, { .2,  .2, 1}], jGFxPoints3D[Q, {1,  .2,  .2}], 
          Graphics3D[Line /@ (jTrail3D /@ Q)], 
          Graphics3D[
            Line /@ (jTrail3D /@ 
                  IdentityMatrix[4])], 
jGFxFrame3D[
            IdentityMatrix[4], {1,  .2,  .2}], 
          jGFxFrame3D[
            A, { .2,  .2, 
              1}], 
PlotRange -> {{-2, 
                2}, {-2, 2}, {-2, 2}}, Axes -> True, 
          AxesLabel -> {(Subscript[m, 1]), (Subscript[m, 2]), (Subscript[m, 3])}, 
          AspectRatio -> Automatic];

F = 
        Table[adCBH[E3, 
                  adCBH[E3, 
{(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 0, 0, 
                      0}, {0, 0, 0, 
                      Q[[k, 1]], 
                      Q[[k, 2]], 
                      Q[[k, 
                        3]]}], -{(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3]), 0, 0, 
                      0}][[{4, 5, 
                  6}]] - {P[[k, 
                  1]], 
                P[[k, 2]], 
                P[[k, 3]]}, {k, 
              Length[P]}] // Chop;

f = 
        Mean[(IdentityMatrix[3] . #1 . #1 & /@ 
              F)];
df = Μd[f, 3];
Hf = Μd[df, 1];

NestList[({(Subscript[x, 1]), (Subscript[x, 2]), (Subscript[x, 3])} - 
              Inverse[Hf] . 
                df) /. {(Subscript[x, 1]) -> #1[[1]], 
            (Subscript[x, 2]) -> #1[[2]], 
            (Subscript[x, 3]) -> #1[[3]]} &, {0, 
        0, 0}, 4]