Skip to content

Commit a41f251

Browse files
dellaertdellaert
committed
Wrapper and notebook
1 parent 393ee24 commit a41f251

File tree

3 files changed

+142
-2
lines changed

3 files changed

+142
-2
lines changed

‎gtsam/navigation/doc/InvariantEKF.ipynb‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "markdown",
5+
"id": "83ec2b32",
6+
"metadata": {},
7+
"source": [
8+
"# InvariantEKF User Guide\n",
9+
"\n",
10+
"This short guide summarizes the `InvariantEKF` API (and the related `LieGroupEKF`) and points to concrete examples in the repository.\n",
11+
"\n",
12+
"## What is the InvariantEKF?\n",
13+
"- A left-invariant EKF on a Lie group `G`.\n",
14+
"- State transition can be given by right multiplication (`predict(U, Q)`) or by a left–right pair (`predict(W, U, Q)`) that executes `X_{k+1} = W · X_k · U`."
15+
]
16+
},
17+
{
18+
"cell_type": "markdown",
19+
"id": "5381b579",
20+
"metadata": {},
21+
"source": [
22+
"## Python usage sketch"
23+
]
24+
},
25+
{
26+
"cell_type": "code",
27+
"execution_count": 1,
28+
"id": "76d8bece",
29+
"metadata": {},
30+
"outputs": [],
31+
"source": [
32+
"import numpy as np\n",
33+
"import gtsam\n",
34+
"\n",
35+
"# State type\n",
36+
"G = gtsam.Pose2\n",
37+
"\n",
38+
"# Initialize filter\n",
39+
"X0 = G.Identity()\n",
40+
"P0 = np.eye(3) * 0.1\n",
41+
"ekf = gtsam.InvariantEKFPose2(X0, P0)\n",
42+
"\n",
43+
"# Right-increment predict with discrete Q\n",
44+
"U = G(1.0, 0.0, 0.1)\n",
45+
"Qd = np.eye(3) * 0.01\n",
46+
"ekf.predict(U, Qd)\n",
47+
"\n",
48+
"# Left–right predict: X <- W * X * U\n",
49+
"W = G(0.1, 0.0, 0.0)\n",
50+
"ekf.predict(W, U, Qd)\n",
51+
"\n",
52+
"# In python, update using ManifoldEKF updateWithVector\n",
53+
"X = ekf.state()\n",
54+
"prediction = np.array([X.x(), X.y()])\n",
55+
"H = np.array([[1, 0, 0], [0, 1, 0]])\n",
56+
"z = np.array([1.0, 0.0])\n",
57+
"R = np.eye(2) * 0.01\n",
58+
"ekf.updateWithVector(prediction, H, z, R)"
59+
]
60+
},
61+
{
62+
"cell_type": "markdown",
63+
"id": "796ed3be",
64+
"metadata": {},
65+
"source": [
66+
"## Usage sketch (C++)\n",
67+
"```cpp\n",
68+
"using namespace gtsam;\n",
69+
"using G = Pose2;\n",
70+
"Matrix3 P0 = I_3x3 * 0.1;\n",
71+
"InvariantEKF<G> ekf(G(), P0);\n",
72+
"\n",
73+
"G U(1.0, 0.0, 0.1);\n",
74+
"Matrix3 Qd = I_3x3 * 0.01;\n",
75+
"ekf.predict(U, Qd); // right increment\n",
76+
"G W(0.1, 0.0, 0.0);\n",
77+
"ekf.predict(W, U, Qd); // left–right increment\n",
78+
"\n",
79+
"auto h = [](const G& X, OptionalJacobian<2,3> H) {\n",
80+
" if (H) *H << 1, 0, 0, 0, 1, 0;\n",
81+
" return Vector2(X.x(), X.y());\n",
82+
"};\n",
83+
"Vector2 z; z << 1.0, 0.0;\n",
84+
"Matrix2 R = Matrix2::Identity() * 0.01;\n",
85+
"ekf.update(h, z, R);\n",
86+
"```"
87+
]
88+
},
89+
{
90+
"cell_type": "markdown",
91+
"id": "55098c65",
92+
"metadata": {},
93+
"source": [
94+
"## Notes\n",
95+
"- `predict(W, U, Q)` treats `Q` as **discrete** (no `dt`).\n",
96+
"- `predict(u, dt, Q)` uses continuous-time `Q` and scales by `dt` internally.\n",
97+
"- IMU-specific EKFs (`NavStateImuEKF`, `Gal3ImuEKF`) reuse these interfaces; see the notebooks above for end-to-end flows."
98+
]
99+
},
100+
{
101+
"cell_type": "markdown",
102+
"id": "7a292821",
103+
"metadata": {},
104+
"source": [
105+
"## Where it is used today\n",
106+
"- C++ tests: [gtsam/navigation/tests/testInvariantEKF.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/tests/testInvariantEKF.cpp) exercises `predict(U, Q)` and the `dt` overload.\n",
107+
"- C++ examples: [examples/IEKF_NavstateExample.cpp](https://github.com/borglab/gtsam/blob/develop/examples/IEKF_NavstateExample.cpp) (NavState), [examples/GEKF_Rot3Example.cpp](https://github.com/borglab/gtsam/blob/develop/examples/GEKF_Rot3Example.cpp) (LieGroupEKF on SO(3)).\n",
108+
"- Docs: [gtsam/navigation/doc/InvariantEKF.md](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/doc/InvariantEKF.md) (math/background).\n",
109+
"- Python notebooks with IMU-specialized EKFs built on the same interface:\n",
110+
" - [python/gtsam/examples/NavStateImuASVExample.ipynb](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/NavStateImuASVExample.ipynb)\n",
111+
" - [python/gtsam/examples/Gal3ImuExample.ipynb](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/Gal3ImuExample.ipynb)\n",
112+
" - [python/gtsam/examples/Gal3ImuASVExample.ipynb](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/Gal3ImuASVExample.ipynb)\n",
113+
"\n",
114+
"If you add new examples that use `predict(W, U, Q)`, please link them here for discoverability."
115+
]
116+
}
117+
],
118+
"metadata": {
119+
"kernelspec": {
120+
"display_name": "py312",
121+
"language": "python",
122+
"name": "python3"
123+
},
124+
"language_info": {
125+
"codemirror_mode": {
126+
"name": "ipython",
127+
"version": 3
128+
},
129+
"file_extension": ".py",
130+
"mimetype": "text/x-python",
131+
"name": "python",
132+
"nbconvert_exporter": "python",
133+
"pygments_lexer": "ipython3",
134+
"version": "3.12.6"
135+
}
136+
},
137+
"nbformat": 4,
138+
"nbformat_minor": 5
139+
}

‎gtsam/navigation/navigation.i‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -639,6 +639,7 @@ virtual class InvariantEKF : gtsam::LeftLinearEKF<G> {
639639

640640
// Left-invariant predict APIs
641641
void predict(const G& U, gtsam::Matrix Q);
642+
void predict(const G& W, const G& U, const gtsam::Matrix& Q);
642643
void predict(const gtsam::Vector& u, double dt, gtsam::Matrix Q);
643644
};
644645

‎gtsam/navigation/navigation.md‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ The `navigation` module in GTSAM provides specialized tools for inertial navigat
1212

1313
- **[ManifoldEKF](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldEKF.h)**: Implements an EKF for states that operate on a differentiable manifold.
1414
- **[LieGroupEKF](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/LieGroupEKF.h)**: Implements an EKF for states that operate on a Lie group with state dependent dynamics.
15-
- **[InvariantEKF](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/InvariantEKF.h)**: Implements an EKF for states that operate on a Lie group with group composition (state independent) dynamics.
15+
- **[InvariantEKF](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/InvariantEKF.h)**: Implements an EKF for states that operate on a Lie group with group composition (state independent) dynamics. See the [InvariantEKF user guide](doc/InvariantEKF.ipynb).
1616

1717
### Attitude Estimation
1818

@@ -211,4 +211,4 @@ The key components are:
211211
- If true, `TangentPreintegration` is used. This does the integration on the tangent space of the NavState manifold.
212212
- If you wish to use any preintegration type other than the default, you must template your PIMs and factors on the desired preintegration type using the template-supporting classes `PreintegratedImuMeasurementsT`, `ImuFactorT`, `ImuFactor2T`, `PreintegratedCombinedMeasurementsT`, or `CombinedImuFactorT`.
213213
- Using the combined IMU factor is not recommended. Typically biases evolve slowly, and hence a separate, lower frequency Markov chain on the bias is more appropriate.
214-
- For short-duration experiments it is even recommended to use a single constant bias. Bias estimation is notoriously hard to tune/debug, and also acts as a "sink" for any modeling errors. Hence, starting with a constant bias is a good idea to get the rest of the pipeline working.
214+
- For short-duration experiments it is even recommended to use a single constant bias. Bias estimation is notoriously hard to tune/debug, and also acts as a "sink" for any modeling errors. Hence, starting with a constant bias is a good idea to get the rest of the pipeline working.

0 commit comments

Comments
 (0)