Add liekf example for se2_localization

[Chris7462]

This PR provides an example of the left IEKF method based on position measurements (GPS-like). The example was adapted from here with some corrections in the update step.

[Chris7462]

Since the expectation $h(X)=t$, $t$ is the translation, this is equivalent to $X$ act on 0. The Jacobian of $H$ is derived as follows:

$$\begin{align} H & = \frac{\partial h}{\partial X}\\\ & = \lim_{\tau\rightarrow 0} \frac{h(\tau\oplus X)\ominus h(X)}{\tau}\\\ & = \lim_{\tau\rightarrow 0} \frac{(Exp(\tau)X)\cdot 0-X\cdot 0}{\tau}\\\ & = \lim_{\tau\rightarrow 0}\frac{\left(\begin{bmatrix} I+\phi^{\wedge} & \rho\\\ 0 & 1 \end{bmatrix}\begin{bmatrix} R & t\\\ 0 & 1 \end{bmatrix}\right) \cdot 0-\begin{bmatrix} R & t\\\ 0 & 1 \end{bmatrix}\cdot 0}{\tau}\\\ & = \lim_{\tau\rightarrow 0}\frac{\begin{bmatrix} (I+\phi^{\wedge})R & (I+\phi^{\wedge})t+\rho\\\ 0 & 1 \end{bmatrix}\cdot 0-\begin{bmatrix} R & t\\\ 0 & 1 \end{bmatrix}\cdot 0}{\tau}\\\ & = \lim_{\tau\rightarrow 0}\frac{(I+\phi^{\wedge})t+\rho-t}{\tau}\\\ & = \lim_{\tau\rightarrow 0}\frac{\phi^{\wedge}t+\rho}{\tau}\\\ & = \frac{\partial(\phi^{\wedge}t+\rho)}{\partial \tau}\Bigg|_{\tau=0}\\\ & = \left[\left.\frac{\partial(\phi^{\wedge}t+\rho)}{\partial\rho}\right|_{\rho=0},\left.\frac{\partial(\phi^{\wedge}t+\rho)}{\partial\phi}\right|_{\phi=0}\right]\\\ & = [I, 1^{\wedge}t] \end{align}$$

[joansola]

This comment I guess will not appear in the final commits.

[Chris7462]

Yes, this comment will not appear in the commits.

[artivis]

This matrix isn't right. See e.g. chapter "4 Simplified car example" in "The Invariant Extended Kalman filter as a stable observer", A. Barrau, S. Bonnabel.

[artivis]

This measurement model is of the form y = x.b + v and is thus left invariant. If I'm not mistaken, H should then be derived with the right-plus (which correspond to left invariance):

$$\begin{align} H & = \frac{\partial h}{\partial X}\\\ & = \lim_{\tau\rightarrow 0} \frac{h(X\oplus\tau)\ominus h(X)}{\tau}\\\ & = ... \end{align}$$

If you work the math you should find something along the lines of:

$$H = \begin{bmatrix} -{\bf I} & {\bf 0} \end{bmatrix}$$

[joansola]

Ok this makes more sense

[joansola]

Then you need the Jacobian of X.inv(y-h(X)), and not of h(X). OK, to find such Jacobian, just use the rules for right-jacobians in the paper, you certainly will find [-I, 0] I guess. Just need to know if you need to differentiate wrt. X, or wrt. \hat X -- I am unsure at the moment.

[Chris7462]

I get the desired result from the Jacobian of $X^{-1}\cdot h(X)$ w.r.t $X$, where $h(X)=X\cdot 0 =t$

$$\begin{align} J^{X^{-1}\cdot h(X)}_{X} & = J^{X^{-1}\cdot t}_{X^{-1}}J^{X^{-1}}_{X}\\\ & = \begin{bmatrix} R^{T} & R^{T}[1]_{x}t \end{bmatrix}\begin{bmatrix} -R & [1]_{x}t\\\ 0 & -1 \end{bmatrix}\\\ & = \begin{bmatrix} -\mathbf{I} & \mathbf{0} \end{bmatrix} \end{align}$$

Am I doing this correctly? Does this make sense?

[joansola]

looks good

[Chris7462]

After reading the papers by Axel Barrau and Silvere Bonnabel (2017, 2018) and Jonathan Arsenault (2019), I believe I finally understand the concepts. Let me use this example to summarize my findings and compare the results with the manif paper.

Example: SE(2) with GPS-like measurement

Recap the setting:

We consider the robot pose $\chi$ in SE(2) and the GPS measurement $y$ is in $\mathbb{R}^{2}$,

$$\mathbf{\chi} = \begin{bmatrix} \mathbf{R} & \mathbf{t}\\\ \mathbf{0}^{T} & 1 \end{bmatrix}$$

The control signal $\mathbf{u}$ is in se(2) and is corrupted by additive Gaussian noise $\mathbf{\varepsilon}$, which has a mean of 0 and a covariance of $\mathbf{Q}$. Upon receiving a control $\mathbf{u}$, the robot pose is updated as follows:

$$\begin{align} \chi_{t} & = f(\chi_{t-1}, \mathbf{u}_{t},\mathbf{\varepsilon}_{t})\\\ & = \chi_{t-1}*\text{Exp}(\mathbf{u}_{t}+\mathbf{\varepsilon}_{t}). \end{align}$$

The GPS measurement $\mathbf{y}$ is in Cartesian form for simplicity, and the noise is denoted $\mathbf{\delta}$, which has a mean of 0 and a covariance of $\mathbf{R}$.

$$\begin{align} \mathbf{y}_{t} & = h(\chi_{t}, b) + \mathbf{\delta}_{t}\\\ & = \chi_{t} \cdot b+\mathbf{\delta}_{t} \end{align}$$

where $b=0$ for rigid motion action in this case.

Let $\bar{\chi}$ be the estimate of $\chi$. Therefore, we have $\bar{\chi}_{t}=f(\bar{\chi}_{t-1}, \mathbf{u}_{t}, 0)$. Now, we can begin working on the math and comparing the differences

Predict step:

Axel and Silvere's approach:

They define the left-invariant error as $\eta_{t}=\chi^{-1}_{t}\bar{\chi}_{t}$.

$$\begin{align} \eta_{t}^{L} & = \chi^{-1}_{t}\bar{\chi}_{t}\\\ & = \left[\chi_{t-1}*\text{Exp}(\mathbf{u}_{t}+\mathbf{\varepsilon}_{t})\right]^{-1}\left[\bar{\chi}_{t-1}*\text{Exp}(\mathbf{u}_{t})\right]\\\ & = \text{Exp}(-\varepsilon_{t})\text{Exp}(\mathbf{u}_{t})^{-1}\eta_{t-1}^{L}\text{Exp}(\mathbf{u}_{t}) \end{align}$$

One can find a $\xi_{t}^{L}$ such that $\eta_{t}^{L} = \text{Exp}(\xi_{t}^{L})$. This allows us to rewrite the above equation as follows:

$$\begin{align} \Rightarrow \text{Exp}(\xi_{t}^{L}) &= \text{Exp}(-\varepsilon_{t})\text{Exp}(\mathbf{u}_{t})^{-1}\text{Exp}(\xi_{t}^{L})\text{Exp}(\mathbf{u}_{t})\\\ & = \text{Exp}(-\varepsilon_{t})\text{Exp}(\text{Ad}_{\text{Exp}(\mathbf{u}_{t})}^{-1}\xi_{t-1}^{L})\\\ \Rightarrow \xi_{t}^{L} &= \text{Ad}_{\text{Exp}(\mathbf{u}_{t})}^{-1}\xi_{t-1}^{L}-\varepsilon_{t}\\\ \Rightarrow \xi_{t}^{L} &= F_{t}\xi_{t-1}^{L}+W_{t}\varepsilon_{t} \end{align}$$

The Jacobians are given by

$$F_{t} = \text{Ad}_{\text{Exp}(\mathbf{u}_{t})}^{-1} \text{\hspace{3ex}and\hspace{3ex}} W_{t}=-\mathbf{I}.$$

Thus, the predicted state and error covariance are computed as follows:

$$\begin{align} \check{\chi}_{t} & = \bar{\chi}_{t}=\bar{\chi}_{t-1}\oplus \mathbf{u}_{t}\\\ \check{\Sigma}_{t} & = F_{t}\hat{\Sigma}_{t-1}F_{t}^{T}+W_{t}Q_{t}W_{t}^{T} \end{align}$$

manif approach:

Their approach involves defining the error on the tangent space and applying the error-state extended Kalman filter on the Lie Group. To facilitate this, they introduce the concepts of right-plus and right-minus operators as follows:

$$\begin{align} \text{right-}\oplus: & \hspace{1ex}\mathcal{Y}=\mathcal{X}\oplus {}^{\mathcal{x}}\tau =\mathcal{X}*\text{Exp}({}^{\mathcal{x}}\tau)\\\ \text{right-}\ominus: & \hspace{1ex} {}^{\mathcal{x}}\tau=\mathcal{Y}\ominus\mathcal{X}=\text{Log}(\mathcal{X}^{-1}*\mathcal{Y}) \end{align}$$

Next, using Taylor expansion of $\chi_{t-1}$ around $\bar{\chi}_{t-1}$, we have:

$$\begin{align} \chi_{t} &= f(\chi_{t-1},\mathbf{u}_{t}, \varepsilon)\\\ & \approx f(\bar{\chi}_{t-1},\mathbf{u}_{t},0)\oplus \left.\frac{\partial f}{\partial \chi}\right|_{\chi_{t-1}=\bar{\chi}_{t-1}}(\chi_{t-1}\ominus\bar{\chi}_{t-1})\oplus\left.\frac{\partial f}{\partial \varepsilon}\right|_{\varepsilon_{t}=0}(\varepsilon_{t}-0)\\\ & = \bar{\chi_{t}}\oplus F_{t}(\chi_{t-1}\ominus\bar{\chi}_{t-1})\oplus W_{t}\varepsilon_{t} \end{align}$$

Let ${}^{\mathcal{x}}\xi=\chi\ominus\bar{\chi}=\text{Log}(\bar{\chi}^{-1}*\chi).$ From this definition, we have $\text{Exp}({}^{\mathcal{x}}\xi)=\bar{\chi}^{-1}*\chi$. We note that ${}^{\mathcal{x}}\xi = -\xi^{L}$.

We can now rewrite the earlier equation as:

$${}^{\mathcal{x}}\xi_{t}=F_{t} {}^{\mathcal{x}}\xi_{t-1}+W_{t}\varepsilon_{t}$$

where the Jacobians are defined as follows:

For $F_{t}$:

$$\begin{align} F_{t} & = \left.\frac{\partial f}{\partial \chi}\right|_{\chi_{t-1}=\bar{\chi}_{t-1}}\\\ & = \lim_{\chi_{t-1}\rightarrow\bar{\chi}_{t-1}}\frac{f(\chi_{t-1},\mathbf{u}_{t},0)\ominus f(\bar{\chi}_{t-1},\mathbf{u}_{t},0)}{\chi_{t-1}\ominus\bar{\chi}_{t-1}}\\\ & = \lim_{{}^{\mathcal{x}}\xi_{t-1}\rightarrow 0}\frac{f(\chi_{t-1}\oplus{}^{\mathcal{x}}\xi_{t-1},\mathbf{u}_{t},0)\ominus f(\chi_{t-1},\mathbf{u}_{t},0)}{{}^{\mathcal{x}}\xi_{t-1}}\\\ & = \frac{Df(\chi_{t-1},\mathbf{u}_{t},0)}{D\chi_{t-1}}\\\ & = J_{\chi_{t-1}}^{f(\chi_{t-1},\mathbf{u}_{t},0)}\\\ & = J_{\chi_{t-1}}^{\chi_{t-1}\oplus\mathbf{u}_{t}}\\\ & = J_{\chi_{t-1}}^{\chi_{t-1}*\text{Exp}(\mathbf{u}_{t})}\\\ & = \text{Ad}_{\text{Exp}(\mathbf{u}_{t})}^{-1} \end{align}$$

For $W_{t}$:

$$\begin{align} W_{t} & = \left.\frac{\partial f}{\partial \varepsilon}\right|_{\varepsilon_{t}=0}\\\ & = \lim_{\varepsilon_{t}\rightarrow 0}\frac{f(\bar{\chi}_{t-1},\mathbf{u}_{t},\varepsilon_{t})\ominus f(\bar{\chi}_{t-1},\mathbf{u}_{t},0)}{\varepsilon_{t}}\\\ & = J^{f(\bar{\chi}_{t-1},\mathbf{u}_{t},\varepsilon_{t})}_{\varepsilon_{t}}\\\ & = J^{(\bar{\chi}_{t-1}\oplus\mathbf{u}_{t})\oplus\varepsilon_{t}}_{\varepsilon_{t}}\\\ & = J_{r}(\varepsilon_{t})\\\ & \approx \mathbf{I} \end{align}$$

Note that in the manif approach, the $W_{t}$ is the negation of $W_{t}$ in Axel and Silvere's approach. This difference arises from the definition of the error state ${}^{\mathcal{x}}\xi$, where $\text{Exp}({}^{\mathcal{x}}\xi)=\bar{\chi}^{-1}\chi$. If you substitute ${}^{\mathcal{x}}\xi$ with $-\xi^{L}$ in the error propogation equation, you will get the same result of $W_{t}$ as in Axel and Silvere's approach.

Therefore, the predicted state and error covariance are

$$\begin{align} \check{\chi}_{t} & = \bar{\chi}_{t}=\bar{\chi}_{t-1}\oplus \mathbf{u}_{t}\\\ \check{\Sigma}_{t} & =F_{t}\hat{\Sigma}_{t-1}F_{t}^{T}+W_{t}Q_{t}W_{t}^{T} \end{align}$$

which match exactly with the results in Axel and Silvere's approach.

Update step:

Axel and Silvere's approach:

$$\begin{align} y_{t} &= h(\chi_{t},b)+\delta_{t},\hspace{1ex}\text{with } b=0\\\ &=\chi_{t}\cdot b+\delta_{t}\\\ \Rightarrow \begin{bmatrix} y_{t}\\\ 0 \end{bmatrix} & = \chi_{t}\begin{bmatrix} b\\\ 1 \end{bmatrix}+\begin{bmatrix} \delta_{t}\\\ 0 \end{bmatrix} \end{align}$$ $$\begin{align} \bar{y}_{t} &=h(\bar{x}_{t},b)\\\ & = \bar{x}_{t}\cdot b \end{align}$$

Now, define the innovation $z_{t}$ such that

$$\begin{align} \begin{bmatrix} z_{t}\\\ 0 \end{bmatrix} &= \bar{\chi}_{t}^{-1}\left(\begin{bmatrix} y_{t}\\\ 0 \end{bmatrix}-\begin{bmatrix} \bar{y}_{t}\\\ 0 \end{bmatrix}\right)\\\ &= \bar{\chi}_{t}^{-1}\left( \chi_{t} \begin{bmatrix} b\\\ 1 \end{bmatrix}+ \begin{bmatrix} \delta_{t}\\\ 0 \end{bmatrix}-\bar{\chi}_{t}\begin{bmatrix} b\\\ 1 \end{bmatrix} \right)\\\ & = \eta_{t}^{L^{-1}} \begin{bmatrix} b\\\ 1 \end{bmatrix}+\bar{\chi}_{t}^{-1} \begin{bmatrix} \delta_{t}\\\ 0 \end{bmatrix}- \begin{bmatrix} b\\\ 1 \end{bmatrix}\\\ & = \text{Exp}(\xi_{t}^{L})^{-1} \begin{bmatrix} b\\\ 1 \end{bmatrix}+\bar{\chi}_{t}^{-1} \begin{bmatrix} \delta_{t}\\\ 0 \end{bmatrix}- \begin{bmatrix} b\\\ 1 \end{bmatrix}\\\ & = (\mathbf{I}-\xi_{t}^{L^{\wedge}}) \begin{bmatrix} b\\\ 1 \end{bmatrix}+\bar{\chi}_{t}^{-1} \begin{bmatrix} \delta_{t}\\\ 0 \end{bmatrix}- \begin{bmatrix} b\\\ 1 \end{bmatrix}\\\ & = -\xi_{t}^{L^{\wedge}} \begin{bmatrix} b\\\ 1 \end{bmatrix}+\bar{\chi}_{t}^{-1} \begin{bmatrix} \delta_{t}\\\ 0 \end{bmatrix}\\\ \Rightarrow z_{t} & = H_{t}\xi_{t}^{L}+V_{t}\delta_{t} \end{align}$$

where

$$H_{t} = \begin{bmatrix} -\mathbf{I} & 0 \end{bmatrix}\hspace{3ex}\text{and}\hspace{3ex}V_{t}=\bar{R}^{T}_{t}$$

You will obtain this result by removing the last row of zeros in these matries.

In addition, we have $\text{Exp}(\xi_{t}^{L})=\eta_{t}^{L}=\chi_{t}^{-1}\bar{\chi}_{t}$, which implies that $\chi_{t}=\bar{\chi}_{t}\text{Exp}(-\xi_{t}^{L})$. This suggest the update of $\chi_{t}$ is

$$\hat{\chi}_{t}=\check{\chi}_{t}\text{Exp}(-K_{t}z_{t}),$$

where $K_{t}=\check{\Sigma}_{t}H_{t}S_{t}^{-1}$ and $S_{t}=H_{t}\check{\Sigma}_{t}H_{t}^{T}+V_{t}R_{t}V_{t}^{T}$.

Thus, the updated state and error covariance are

$$\begin{align} \hat{\chi}_{t} & = \check{\chi}_{t}\text{Exp}(-K_{t}z_{t})\\\ \hat{\Sigma}_{t} & = (\mathbf{I}-K_{t}H_{t})\check{\Sigma}_{t} \end{align}$$

manif approach:

Define the innovation $z_{t}$ such that

$$\begin{align} z_{t} & = \bar{\chi}_{t}^{-1}\cdot (y_{t}-\bar{y}_{t})\\\ & = \bar{\chi}_{t}^{-1}\cdot y_{t}-b \end{align}$$

Now let $h(\chi)=(\bar{\chi}_{t}^{-1}\chi_{t})\cdot b$ and note that

$$\begin{align} \bar{\chi}_{t}^{-1}\cdot y_{t} &= \bar{\chi}_{t}^{-1}(\chi\cdot b+\delta_{t})\\\ & = \bar{\chi}_{t}^{-1}\chi_{t}\cdot b+\bar{\chi}_{t}^{-1}\cdot \delta_{t}\\\ & = h(\bar{\chi}_{t})+\left.\frac{\partial h}{\partial \chi}\right|_{\chi_{t}=\bar{\chi}_{t}}(\chi_{t}\ominus\bar{\chi}_{t})+\bar{\chi}_{t}^{-1}\cdot \delta_{t}\\\ & = b+H_{t}{}^{\mathcal{x}}\xi_{t}+\bar{\chi}_{t}^{-1}\cdot \delta_{t}\\\ \end{align}$$

So, we can rewrite the innovation as

$$z_{t} = H_{t}{}^{\mathcal{x}}\xi_{t}+V_{t}\delta_{t}\\$$

where

$$\begin{align} H_{t} &=\left.\frac{\partial h}{\partial \chi}\right|_{\chi_{t}=\bar{\chi}_{t}}\\\ & = \lim_{\chi_{t}\rightarrow \bar{\chi}_{t}}\frac{h(\chi_{t})-h(\bar{\chi}_{t})}{\chi_{t}\ominus \bar{\chi}_{t}}\\\ & = \lim_{{}^{\mathcal{x}}\xi_{t}\rightarrow 0}\frac{\text{Exp}({}^{\mathcal{x}}\xi_{t})\cdot b-b}{{}^{\mathcal{x}}\xi_{t}}\\\ & = \lim_{{}^{\mathcal{x}}\xi_{t}\rightarrow 0}\frac{(\mathbf{I}+{}^{\mathcal{x}}\xi_{t}^{\wedge})\cdot b-b}{{}^{\mathcal{x}}\xi_{t}}\\\ & = \lim_{{}^{\mathcal{x}}\xi_{t}\rightarrow 0}\frac{\phi^{\wedge}b+\rho}{{}^{\mathcal{x}}\xi_{t}}\\\ & = \begin{bmatrix} \left.\frac{\partial \phi^{\wedge}b+\rho}{\partial\rho}\right|_{\rho=0} & \left.\frac{\partial \phi^{\wedge}b+\rho}{\partial\phi}\right|_{\phi=0} & \end{bmatrix}\\\ & = \begin{bmatrix} \mathbf{I} & 0 \end{bmatrix} \end{align}$$

and $V_{t}=\bar{R}_{t}^{T}$. Again, the $H_{t}$ in manif approach is negation of $H_{t}$ in Axel and Silvere's approach. This difference arises due to the definition of the error ${}^{\mathcal{x}}\xi_{t}$.

Furthermore,, we have $\text{Exp}({}^{\mathcal{x}}\xi_{t})=\bar{\chi}_{t}^{-1}\chi_{t}$, which implies that $\chi_{t}=\bar{\chi}_{t}\text{Exp}({}^{\mathcal{x}}\xi_{t})$. This suggest the update of $\chi_{t}$ is

$$\hat{\chi}_{t}=\check{\chi}_{t}\text{Exp}(K_{t}z_{t}),$$

where $K_{t}=\check{\Sigma}_{t}H_{t}S_{t}^{-1}$ and $S_{t}=H_{t}\check{\Sigma}_{t}H_{t}^{T}+V_{t}R_{t}V_{t}^{T}$.

Thus, the updated state and error covariance are given by:

$$\begin{align} \hat{\chi}_{t} & = \check{\chi}_{t}\text{Exp}(K_{t}z_{t})\\\ \hat{\Sigma}_{t} & = (\mathbf{I}-K_{t}H_{t})\check{\Sigma}_{t} \end{align}$$

Summary:

	Axel and Silvere	Manif
error	$\eta^{L}=\chi^{-1}\bar{\chi}$ $\text{Exp}(\xi^{L})=\chi^{-1}\bar{\chi}$	${}^{\mathcal{x}}\xi=\chi\ominus\bar{\chi}$ $\text{Exp}({}^{\mathcal{x}}\xi)=\bar{\chi}^{-1}\chi$
Predict	$\check{\chi}_{t}=\hat{\chi}_{t-1}\oplus \mathbf{u}_{t}$ $\check{\Sigma}_{t}=F_{t}\hat{\Sigma}_{t-1}F_{t}^{T}+W_{t}Q_{t}W_{t}^{T}$	$\check{\chi}_{t}=\hat{\chi}_{t-1}\oplus \mathbf{u}_{t}$ $\check{\Sigma}_{t}=F_{t}\hat{\Sigma}_{t-1}F_{t}^{T}+W_{t}Q_{t}W_{t}^{T}$
Update	$z_{t}=\bar{\chi}_{t}^{-1}\cdot(y_{t}-\bar{y}_{t})$ $S_{t}=H_{t}\check{\Sigma}_{t}H_{t}^{T}+V_{t}R_{t}V_{t}^{T}$ $K_{t}=\check{\Sigma}_{t}H_{t}^{T}S_{t}^{-1}$ $\hat{\chi}_{t}=\check{\chi}_{t}\text{Exp}(-K_{t}z_{t}).$ $\hat{\Sigma}_{t}=(\mathbf{I}-K_{t}H_{t})\check{\Sigma}_{t}$	$z_{t}=\bar{\chi}_{t}^{-1}\cdot(y_{t}-\bar{y}_{t})$ $S_{t}=H_{t}\check{\Sigma}_{t}H_{t}^{T}+V_{t}R_{t}V_{t}^{T}$ $K_{t}=\check{\Sigma}_{t}H_{t}^{T}S_{t}^{-1}$ $\hat{\chi}_{t}=\check{\chi}_{t}\text{Exp}(K_{t}z_{t}).$ $\hat{\Sigma}_{t}=(\mathbf{I}-K_{t}H_{t})\check{\Sigma}_{t}$

Noticing the sign difference in the update step, one should be very careful about how the error is defined when implementing the invariant kalman filter.

Also, in this example, since the state error in both of the predict step and the update step is left-invariant, there is no need to perform any covariance transformation from left-invariant to right-invariant or vice versa.

[joansola]

Comments about left-right invariances require answers from the author of the MR.

[joansola]

This comment I guess will not appear in the final commits.

[Chris7462]

Sorry forget to post the simulation results here. The results obtained from a 10 step simulation of this example is shown below

X STATE     :    X      Y    THETA
----------------------------------
X initial   : +0.000 +0.000 +0.000
----------------------------------
X simulated : +0.100 +0.000 +0.050
X estimated : +0.069 +0.041 -0.013
X unfilterd : +0.089 +0.091 -0.012
----------------------------------
X simulated : +0.200 -0.000 +0.100
X estimated : +0.139 -0.022 +0.094
X unfilterd : +0.131 +0.102 +0.100
----------------------------------
X simulated : +0.300 -0.000 +0.150
X estimated : +0.263 -0.010 +0.136
X unfilterd : +0.281 +0.103 +0.139
----------------------------------
X simulated : +0.400 -0.000 +0.200
X estimated : +0.416 +0.074 +0.079
X unfilterd : +0.370 +0.181 +0.090
----------------------------------
X simulated : +0.500 +0.000 +0.250
X estimated : +0.532 -0.004 +0.059
X unfilterd : +0.527 +0.166 +0.103
----------------------------------
X simulated : +0.600 -0.000 +0.300
X estimated : +0.657 +0.103 +0.073
X unfilterd : +0.684 +0.243 +0.083
----------------------------------
X simulated : +0.700 -0.000 +0.350
X estimated : +0.671 +0.025 +0.180
X unfilterd : +0.726 +0.140 +0.185
----------------------------------
X simulated : +0.800 -0.000 +0.400
X estimated : +0.712 +0.054 +0.347
X unfilterd : +0.776 +0.093 +0.330
----------------------------------
X simulated : +0.900 -0.000 +0.450
X estimated : +0.847 +0.132 +0.335
X unfilterd : +0.790 +0.141 +0.342
----------------------------------
X simulated : +1.000 +0.000 +0.500
X estimated : +1.050 +0.036 +0.482
X unfilterd : +0.968 +0.030 +0.455
----------------------------------

[joansola]

These results are non conclusive. The unfiltered solution has basically the same error as the filtered one, meaning that the filter did not do a great job.

You should try to tune the noises so that an unfiltered solution departs more from the nominal trajectory. Then we can evaluate the effect of the filter. As it is now, the filter has no effect and we cannot assess if it is working correctly or not.

You can also try. with a longer experiment to give time to the noisy unfiltered trajectory to deviate from the nominal one.

[Chris7462]

I have increased the number of steps from 10 to 50 while keeping all the noise levels the same. I observed that there are instances where the values of $\theta$ deviate from the true value. This deviation occurs because we only have position measurements to correct the translation, but no measurement for the rotation. Additionally, since there is only one measurement per step, the fusion results would likely improve if there were multiple measurements (e.g., from landmarks) at each step.

X STATE     :    X      Y    THETA
----------------------------------
X initial   : +0.000 +0.000 +0.000
----------------------------------
X simulated : +0.100 +0.000 +0.050
X estimated : +0.026 +0.002 +0.029
X unfilterd : +0.003 -0.053 +0.028
----------------------------------
X simulated : +0.200 -0.000 +0.100
X estimated : +0.184 -0.022 +0.002
X unfilterd : +0.157 -0.023 +0.010
----------------------------------
X simulated : +0.300 -0.000 +0.150
X estimated : +0.373 -0.045 -0.006
X unfilterd : +0.340 -0.007 +0.015
----------------------------------
X simulated : +0.400 -0.000 +0.200
X estimated : +0.346 +0.014 +0.151
X unfilterd : +0.357 -0.048 +0.156
----------------------------------
X simulated : +0.500 +0.000 +0.250
X estimated : +0.486 +0.059 +0.116
X unfilterd : +0.410 -0.014 +0.125
----------------------------------
X simulated : +0.600 -0.000 +0.300
X estimated : +0.605 +0.130 +0.169
X unfilterd : +0.596 +0.088 +0.170
----------------------------------
X simulated : +0.700 -0.000 +0.350
X estimated : +0.680 +0.102 +0.129
X unfilterd : +0.754 +0.073 +0.138
----------------------------------
X simulated : +0.800 -0.000 +0.400
X estimated : +0.749 +0.144 +0.096
X unfilterd : +0.921 +0.179 +0.127
----------------------------------
X simulated : +0.900 -0.000 +0.450
X estimated : +0.884 +0.179 +0.206
X unfilterd : +0.999 +0.206 +0.270
----------------------------------
X simulated : +1.000 +0.000 +0.500
X estimated : +1.076 +0.226 +0.181
X unfilterd : +1.192 +0.277 +0.269
----------------------------------
X simulated : +1.100 +0.000 +0.550
X estimated : +1.048 +0.216 +0.241
X unfilterd : +1.218 +0.204 +0.304
----------------------------------
X simulated : +1.200 +0.000 +0.600
X estimated : +1.158 +0.081 +0.407
X unfilterd : +1.265 +0.048 +0.440
----------------------------------
X simulated : +1.300 +0.000 +0.650
X estimated : +1.352 +0.045 +0.532
X unfilterd : +1.433 -0.005 +0.536
----------------------------------
X simulated : +1.400 +0.000 +0.700
X estimated : +1.400 +0.103 +0.599
X unfilterd : +1.477 +0.025 +0.584
----------------------------------
X simulated : +1.500 +0.000 +0.750
X estimated : +1.549 +0.118 +0.618
X unfilterd : +1.635 +0.071 +0.678
----------------------------------
X simulated : +1.600 -0.000 +0.800
X estimated : +1.640 +0.224 +0.582
X unfilterd : +1.717 +0.185 +0.663
----------------------------------
X simulated : +1.700 -0.000 +0.850
X estimated : +1.673 +0.282 +0.502
X unfilterd : +1.873 +0.358 +0.618
----------------------------------
X simulated : +1.800 -0.000 +0.900
X estimated : +1.798 +0.323 +0.448
X unfilterd : +2.051 +0.439 +0.595
----------------------------------
X simulated : +1.900 -0.000 +0.950
X estimated : +1.832 +0.340 +0.566
X unfilterd : +2.160 +0.467 +0.634
----------------------------------
X simulated : +2.000 -0.000 +1.000
X estimated : +1.925 +0.358 +0.568
X unfilterd : +2.350 +0.530 +0.645
----------------------------------
X simulated : +2.100 -0.000 +1.050
X estimated : +2.027 +0.239 +0.710
X unfilterd : +2.422 +0.362 +0.768
----------------------------------
X simulated : +2.200 -0.000 +1.100
X estimated : +2.077 +0.315 +0.700
X unfilterd : +2.479 +0.413 +0.723
----------------------------------
X simulated : +2.300 -0.000 +1.150
X estimated : +2.261 +0.428 +0.752
X unfilterd : +2.619 +0.525 +0.676
----------------------------------
X simulated : +2.400 +0.000 +1.200
X estimated : +2.241 +0.484 +0.805
X unfilterd : +2.618 +0.547 +0.706
----------------------------------
X simulated : +2.500 -0.000 +1.250
X estimated : +2.337 +0.521 +0.754
X unfilterd : +2.703 +0.597 +0.728
----------------------------------
X simulated : +2.600 +0.000 +1.300
X estimated : +2.444 +0.499 +0.900
X unfilterd : +2.879 +0.655 +0.793
----------------------------------
X simulated : +2.700 +0.000 +1.350
X estimated : +2.563 +0.381 +1.007
X unfilterd : +3.058 +0.536 +0.887
----------------------------------
X simulated : +2.800 -0.000 +1.400
X estimated : +2.696 +0.474 +1.008
X unfilterd : +3.221 +0.608 +0.930
----------------------------------
X simulated : +2.900 +0.000 +1.450
X estimated : +2.818 +0.420 +1.142
X unfilterd : +3.371 +0.543 +1.037
----------------------------------
X simulated : +3.000 -0.000 +1.500
X estimated : +2.896 +0.437 +1.252
X unfilterd : +3.491 +0.601 +1.063
----------------------------------
X simulated : +3.100 -0.000 +1.550
X estimated : +2.956 +0.370 +1.309
X unfilterd : +3.536 +0.505 +1.129
----------------------------------
X simulated : +3.200 -0.000 +1.600
X estimated : +3.071 +0.365 +1.382
X unfilterd : +3.676 +0.499 +1.155
----------------------------------
X simulated : +3.300 +0.000 +1.650
X estimated : +3.185 +0.296 +1.476
X unfilterd : +3.850 +0.413 +1.279
----------------------------------
X simulated : +3.400 +0.000 +1.700
X estimated : +3.273 +0.404 +1.455
X unfilterd : +4.011 +0.584 +1.243
----------------------------------
X simulated : +3.500 +0.000 +1.750
X estimated : +3.365 +0.413 +1.514
X unfilterd : +4.098 +0.637 +1.200
----------------------------------
X simulated : +3.600 +0.000 +1.800
X estimated : +3.440 +0.523 +1.565
X unfilterd : +4.186 +0.718 +1.226
----------------------------------
X simulated : +3.700 +0.000 +1.850
X estimated : +3.571 +0.423 +1.591
X unfilterd : +4.312 +0.588 +1.267
----------------------------------
X simulated : +3.800 +0.000 +1.900
X estimated : +3.764 +0.248 +1.782
X unfilterd : +4.572 +0.443 +1.384
----------------------------------
X simulated : +3.900 +0.000 +1.950
X estimated : +3.873 +0.150 +1.876
X unfilterd : +4.715 +0.375 +1.406
----------------------------------
X simulated : +4.000 +0.000 +2.000
X estimated : +4.084 -0.035 +2.011
X unfilterd : +4.952 +0.111 +1.538
----------------------------------
X simulated : +4.100 +0.000 +2.050
X estimated : +4.182 -0.313 +2.182
X unfilterd : +5.121 -0.179 +1.640
----------------------------------
X simulated : +4.200 +0.000 +2.100
X estimated : +4.209 +0.050 +2.065
X unfilterd : +5.107 +0.052 +1.601
----------------------------------
X simulated : +4.300 -0.000 +2.150
X estimated : +4.507 -0.212 +2.267
X unfilterd : +5.423 -0.216 +1.738
----------------------------------
X simulated : +4.400 -0.000 +2.200
X estimated : +4.561 -0.232 +2.325
X unfilterd : +5.517 -0.181 +1.744
----------------------------------
X simulated : +4.500 -0.000 +2.250
X estimated : +4.691 -0.208 +2.394
X unfilterd : +5.686 -0.275 +1.839
----------------------------------
X simulated : +4.600 -0.000 +2.300
X estimated : +4.628 -0.058 +2.364
X unfilterd : +5.645 -0.128 +1.821
----------------------------------
X simulated : +4.700 -0.000 +2.350
X estimated : +4.779 -0.054 +2.395
X unfilterd : +5.850 -0.201 +1.895
----------------------------------
X simulated : +4.800 -0.000 +2.400
X estimated : +4.943 +0.011 +2.395
X unfilterd : +6.088 -0.237 +1.941
----------------------------------
X simulated : +4.900 -0.000 +2.450
X estimated : +5.012 -0.322 +2.584
X unfilterd : +6.193 -0.582 +2.088
----------------------------------
X simulated : +5.000 -0.000 +2.500
X estimated : +5.116 -0.055 +2.570
X unfilterd : +6.315 -0.607 +2.160
----------------------------------

[joansola]

You can increase the process noise, e.g. by a factor 10 (cov by a factor 100), and/or reduce the measurement noise, by the same factor. This way you emphasize the effect of the GNSS corrections.

With the longer simu, things start to be visible. But I think more evidence is needed. Try one or both of the ideas above. Keep the length the same as in the long run you did now.

[Chris7462]

Thank you. I increased the control noise by a factor of 10 and reduced the measurement noise by a factor of 10. Here are the results from the new simulation. I appreciate your insight.

X STATE     :    X      Y    THETA
----------------------------------
X initial   : +0.000 +0.000 +0.000
----------------------------------
X simulated : +0.100 +0.000 +0.050
X estimated : +0.098 +0.002 -0.050
X unfilterd : +0.367 -0.929 -0.050
----------------------------------
X simulated : +0.200 -0.000 +0.100
X estimated : +0.193 -0.002 +0.057
X unfilterd : -0.468 -1.441 +0.056
----------------------------------
X simulated : +0.300 -0.000 +0.150
X estimated : +0.304 -0.001 +0.172
X unfilterd : -0.754 -2.151 +0.168
----------------------------------
X simulated : +0.400 -0.000 +0.200
X estimated : +0.410 -0.016 +0.297
X unfilterd : +0.236 -2.397 +0.293
----------------------------------
X simulated : +0.500 +0.000 +0.250
X estimated : +0.498 -0.009 +0.298
X unfilterd : -0.095 -2.089 +0.295
----------------------------------
X simulated : +0.600 -0.000 +0.300
X estimated : +0.608 +0.013 +0.259
X unfilterd : +0.685 -2.124 +0.256
----------------------------------
X simulated : +0.700 -0.000 +0.350
X estimated : +0.705 +0.014 +0.334
X unfilterd : +1.052 -2.031 +0.330
----------------------------------
X simulated : +0.800 -0.000 +0.400
X estimated : +0.804 +0.037 +0.322
X unfilterd : +0.095 -1.695 +0.322
----------------------------------
X simulated : +0.900 -0.000 +0.450
X estimated : +0.885 +0.079 +0.291
X unfilterd : +0.055 -2.475 +0.286
----------------------------------
X simulated : +1.000 +0.000 +0.500
X estimated : +0.985 +0.101 +0.276
X unfilterd : +0.676 -3.075 +0.265
----------------------------------
X simulated : +1.100 +0.000 +0.550
X estimated : +1.080 +0.094 +0.375
X unfilterd : +1.393 -2.216 +0.367
----------------------------------
X simulated : +1.200 +0.000 +0.600
X estimated : +1.187 +0.084 +0.473
X unfilterd : +1.388 -3.270 +0.457
----------------------------------
X simulated : +1.300 +0.000 +0.650
X estimated : +1.296 +0.090 +0.518
X unfilterd : +1.171 -4.091 +0.495
----------------------------------
X simulated : +1.400 +0.000 +0.700
X estimated : +1.388 +0.091 +0.572
X unfilterd : +0.755 -3.348 +0.556
----------------------------------
X simulated : +1.500 +0.000 +0.750
X estimated : +1.487 +0.046 +0.677
X unfilterd : +0.390 -2.469 +0.670
----------------------------------
X simulated : +1.600 -0.000 +0.800
X estimated : +1.594 +0.090 +0.691
X unfilterd : +0.933 -2.790 +0.678
----------------------------------
X simulated : +1.700 -0.000 +0.850
X estimated : +1.686 +0.069 +0.781
X unfilterd : +0.496 -2.430 +0.772
----------------------------------
X simulated : +1.800 -0.000 +0.900
X estimated : +1.811 -0.010 +0.920
X unfilterd : +1.335 -1.981 +0.913
----------------------------------
X simulated : +1.900 -0.000 +0.950
X estimated : +1.885 +0.068 +0.889
X unfilterd : +1.685 -0.704 +0.890
----------------------------------
X simulated : +2.000 -0.000 +1.000
X estimated : +1.996 +0.004 +0.994
X unfilterd : +2.414 +0.038 +1.000
----------------------------------
X simulated : +2.100 -0.000 +1.050
X estimated : +2.081 +0.061 +0.992
X unfilterd : +1.997 +0.987 +1.007
----------------------------------
X simulated : +2.200 -0.000 +1.100
X estimated : +2.202 +0.003 +1.097
X unfilterd : +1.539 +1.705 +1.122
----------------------------------
X simulated : +2.300 -0.000 +1.150
X estimated : +2.294 -0.014 +1.156
X unfilterd : +2.442 +1.166 +1.174
----------------------------------
X simulated : +2.400 +0.000 +1.200
X estimated : +2.407 -0.080 +1.261
X unfilterd : +2.669 +0.834 +1.276
----------------------------------
X simulated : +2.500 -0.000 +1.250
X estimated : +2.501 +0.025 +1.225
X unfilterd : +3.474 +0.875 +1.235
----------------------------------
X simulated : +2.600 +0.000 +1.300
X estimated : +2.576 +0.094 +1.224
X unfilterd : +3.883 +2.068 +1.240
----------------------------------
X simulated : +2.700 +0.000 +1.350
X estimated : +2.673 +0.126 +1.251
X unfilterd : +4.508 +1.730 +1.261
----------------------------------
X simulated : +2.800 -0.000 +1.400
X estimated : +2.765 +0.116 +1.306
X unfilterd : +5.640 +1.867 +1.310
----------------------------------
X simulated : +2.900 +0.000 +1.450
X estimated : +2.838 +0.249 +1.276
X unfilterd : +5.401 +1.544 +1.279
----------------------------------
X simulated : +3.000 -0.000 +1.500
X estimated : +2.889 +0.351 +1.264
X unfilterd : +5.492 +0.933 +1.262
----------------------------------
X simulated : +3.100 -0.000 +1.550
X estimated : +3.023 +0.256 +1.383
X unfilterd : +4.822 +0.900 +1.390
----------------------------------
X simulated : +3.200 -0.000 +1.600
X estimated : +3.140 +0.228 +1.457
X unfilterd : +5.297 +0.270 +1.456
----------------------------------
X simulated : +3.300 +0.000 +1.650
X estimated : +3.187 +0.322 +1.446
X unfilterd : +6.335 +0.728 +1.439
----------------------------------
X simulated : +3.400 +0.000 +1.700
X estimated : +3.294 +0.329 +1.506
X unfilterd : +7.229 +0.448 +1.489
----------------------------------
X simulated : +3.500 +0.000 +1.750
X estimated : +3.366 +0.372 +1.527
X unfilterd : +7.401 +0.402 +1.509
----------------------------------
X simulated : +3.600 +0.000 +1.800
X estimated : +3.451 +0.426 +1.556
X unfilterd : +6.330 +0.437 +1.551
----------------------------------
X simulated : +3.700 +0.000 +1.850
X estimated : +3.594 +0.326 +1.666
X unfilterd : +5.787 -0.419 +1.664
----------------------------------
X simulated : +3.800 +0.000 +1.900
X estimated : +3.597 +0.496 +1.627
X unfilterd : +6.022 -0.249 +1.622
----------------------------------
X simulated : +3.900 +0.000 +1.950
X estimated : +3.698 +0.487 +1.685
X unfilterd : +7.601 -0.199 +1.668
----------------------------------
X simulated : +4.000 +0.000 +2.000
X estimated : +3.784 +0.493 +1.741
X unfilterd : +7.422 +0.483 +1.729
----------------------------------
X simulated : +4.100 +0.000 +2.050
X estimated : +3.888 +0.502 +1.788
X unfilterd : +7.245 +0.664 +1.781
----------------------------------
X simulated : +4.200 +0.000 +2.100
X estimated : +3.900 +0.654 +1.769
X unfilterd : +6.774 +1.942 +1.769
----------------------------------
X simulated : +4.300 -0.000 +2.150
X estimated : +4.071 +0.514 +1.893
X unfilterd : +6.983 +0.755 +1.890
----------------------------------
X simulated : +4.400 -0.000 +2.200
X estimated : +4.204 +0.458 +1.982
X unfilterd : +7.374 +0.936 +1.978
----------------------------------
X simulated : +4.500 -0.000 +2.250
X estimated : +4.333 +0.394 +2.072
X unfilterd : +7.648 +1.671 +2.071
----------------------------------
X simulated : +4.600 -0.000 +2.300
X estimated : +4.460 +0.281 +2.171
X unfilterd : +8.422 -0.089 +2.162
----------------------------------
X simulated : +4.700 -0.000 +2.350
X estimated : +4.662 +0.112 +2.304
X unfilterd : +9.800 -0.398 +2.286
----------------------------------
X simulated : +4.800 -0.000 +2.400
X estimated : +4.847 -0.142 +2.457
X unfilterd : +10.900  -2.372  +2.431
----------------------------------
X simulated : +4.900 -0.000 +2.450
X estimated : +4.961 -0.135 +2.508
X unfilterd : +10.757  -3.081  +2.481
----------------------------------
X simulated : +5.000 -0.000 +2.500
X estimated : +4.949 +0.101 +2.459
X unfilterd : +10.223  -1.784  +2.439
----------------------------------

[joansola]

The orientation should be observable through the combination of motion model (which has a sense of direction of the robot) and the GPS. As long as the angular process noise is not too large, the orientation should be observable.

You can reduce angular process noise by 10 and try over. We want to see that the unfiltered one diverges unboundedly (even if slowly slowly) while the filtered one stays close to the nominal. This is not yet visible in your results.

Also please report the tuning values of u_sigma and y_sigma.

Position estimates X and Y do converge to the nominal values, so they are good.

[Chris7462]

Could you please check if the result make sense? I have changed only the value of $\theta$ ranging from 0.05, 0.1, 0.5, to 1.0 in the control.

u_nom    << 0.1, 0.0, 0.1;            u_nom    << 0.1, 0.0, 0.1;  u_nom    << 0.1, 0.0, 0.1;  u_nom    << 0.1, 0.0, 0.1;									
u_sigmas << 1.0, 1.0, 0.05;           u_sigmas << 1.0, 1.0, 0.1;  u_sigmas << 1.0, 1.0, 0.5;  u_sigmas << 1.0, 1.0, 1.0;
y_sigmas << 0.01, 0.01;               y_sigmas << 0.01, 0.01;     y_sigmas << 0.01, 0.01;     y_sigmas << 0.01, 0.01;

X STATE     :    X      Y    THETA    :    X      Y    THETA      :    X      Y    THETA      :    X      Y    THETA
----------------------------------    ----------------------      ----------------------      ----------------------
X initial   : +0.000 +0.000 +0.000    : +0.000 +0.000 +0.000      : +0.000 +0.000 +0.000      : +0.000 +0.000 +0.000
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.100 +0.000 +0.100    : +0.100 +0.000 +0.100      : +0.100 +0.000 +0.100      : +0.100 +0.000 +0.100
X estimated : +0.106 +0.007 +0.131    : +0.102 -0.011 +0.142      : +0.087 -0.029 +0.571      : +0.099 -0.029 +0.684
X unfilterd : +0.722 +0.566 +0.131    : +0.005 -0.897 +0.141      : +0.988 -0.755 +0.550      : +0.399 +0.035 +0.685
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.200 -0.000 +0.200    : +0.200 -0.000 +0.200      : +0.200 -0.000 +0.200      : +0.200 -0.000 +0.200
X estimated : +0.209 -0.003 +0.244    : +0.203 -0.005 +0.280      : +0.195 -0.029 +0.298      : +0.193 -0.004 +0.221
X unfilterd : +1.491 +0.786 +0.244    : -0.770 -1.866 +0.278      : +0.364 +0.167 +0.306      : +1.312 -0.338 +0.219
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.300 +0.000 +0.300    : +0.300 +0.000 +0.300      : +0.300 +0.000 +0.300      : +0.300 +0.000 +0.300
X estimated : +0.304 +0.002 +0.338    : +0.298 -0.024 +0.456      : +0.287 -0.021 +0.422      : +0.302 +0.025 +0.170
X unfilterd : +1.787 +0.444 +0.338    : -1.719 -0.916 +0.456      : -0.083 -0.190 +0.419      : +1.974 -0.104 +0.177
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.400 +0.000 +0.400    : +0.400 +0.000 +0.400      : +0.400 +0.000 +0.400      : +0.400 +0.000 +0.400
X estimated : +0.393 +0.001 +0.391    : +0.398 -0.035 +0.583      : +0.384 +0.067 -0.007      : +0.380 -0.159 +1.139
X unfilterd : +1.853 +1.090 +0.391    : -1.731 -0.409 +0.584      : -0.426 +0.405 +0.038      : +2.019 -1.326 +1.108
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.500 +0.000 +0.500    : +0.500 +0.000 +0.500      : +0.500 +0.000 +0.500      : +0.500 +0.000 +0.500
X estimated : +0.507 -0.013 +0.514    : +0.501 -0.072 +0.777      : +0.484 +0.068 +0.231      : +0.431 -0.295 +1.765
X unfilterd : +2.698 +0.496 +0.514    : -1.761 -0.697 +0.776      : +0.552 +1.096 +0.277      : +1.079 -2.486 +1.637
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.600 +0.000 +0.600    : +0.600 +0.000 +0.600      : +0.600 +0.000 +0.600      : +0.600 +0.000 +0.600
X estimated : +0.591 -0.014 +0.629    : +0.607 -0.083 +0.889      : +0.552 +0.201 -0.092      : +0.314 -0.754 +2.853
X unfilterd : +2.484 -0.129 +0.629    : -2.010 +0.034 +0.891      : -0.310 +0.385 -0.052      : -0.698 -4.126 +2.610
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.700 +0.000 +0.700    : +0.700 +0.000 +0.700      : +0.700 +0.000 +0.700      : +0.700 +0.000 +0.700
X estimated : +0.708 -0.021 +0.768    : +0.707 -0.124 +1.050      : +0.643 +0.225 +0.038      : -0.293 +1.009 -3.068
X unfilterd : +3.151 +0.868 +0.769    : -1.484 +1.577 +1.060      : -0.641 +0.119 +0.077      : -1.715 -3.281 +3.005
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.800 +0.000 +0.800    : +0.800 +0.000 +0.800      : +0.800 +0.000 +0.800      : +0.800 +0.000 +0.800
X estimated : +0.803 -0.019 +0.849    : +0.809 -0.155 +1.175      : +0.744 +0.218 +0.215      : +0.554 -0.987 +2.900
X unfilterd : +3.940 +1.192 +0.851    : -0.789 +2.623 +1.189      : -1.213 +0.049 +0.267      : -0.778 -3.930 +2.647
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +0.900 +0.000 +0.900    : +0.900 +0.000 +0.900      : +0.900 +0.000 +0.900      : +0.900 +0.000 +0.900
X estimated : +0.905 -0.037 +0.987    : +0.905 -0.165 +1.250      : +0.862 +0.154 +0.537      : +0.831 -0.649 +2.266
X unfilterd : +5.407 +0.532 +0.987    : -1.270 +2.871 +1.267      : -0.548 +0.344 +0.565      : +1.147 -3.173 +2.086
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.000 +0.000 +1.000    : +1.000 +0.000 +1.000      : +1.000 +0.000 +1.000      : +1.000 +0.000 +1.000
X estimated : +1.007 -0.052 +1.085    : +1.013 -0.219 +1.409      : +0.945 +0.241 +0.493      : +0.962 -0.407 +1.735
X unfilterd : +5.641 +1.392 +1.086    : -0.563 +3.891 +1.431      : +0.039 +0.752 +0.518      : +1.827 -1.719 +1.658
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.100 +0.000 +1.100    : +1.100 +0.000 +1.100      : +1.100 +0.000 +1.100      : +1.100 +0.000 +1.100
X estimated : +1.109 -0.038 +1.166    : +1.120 -0.211 +1.467      : +1.003 +0.337 +0.463      : +1.111 -0.403 +1.752
X unfilterd : +6.469 +1.821 +1.168    : -1.148 +4.497 +1.495      : +0.552 +1.369 +0.485      : +1.059 -1.444 +1.732
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.200 +0.000 +1.200    : +1.200 +0.000 +1.200      : +1.200 +0.000 +1.200      : +1.200 +0.000 +1.200
X estimated : +1.197 -0.062 +1.296    : +1.220 -0.190 +1.507      : +0.977 +0.581 +0.109      : +1.119 -0.804 +2.393
X unfilterd : +6.198 +2.127 +1.300    : -1.119 +5.423 +1.541      : +1.134 +1.604 +0.100      : +0.788 -3.054 +2.264
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.300 +0.000 +1.300    : +1.300 +0.000 +1.300      : +1.300 +0.000 +1.300      : +1.300 +0.000 +1.300
X estimated : +1.329 -0.099 +1.446    : +1.340 -0.185 +1.579      : +0.954 +0.753 -0.045      : +1.191 -1.280 +2.893
X unfilterd : +7.916 +1.533 +1.448    : -1.234 +4.419 +1.604      : +0.972 +1.158 -0.060      : -0.446 -2.200 +2.819
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.400 +0.000 +1.400    : +1.400 +0.000 +1.400      : +1.400 +0.000 +1.400      : +1.400 +0.000 +1.400
X estimated : +1.408 -0.090 +1.530    : +1.437 -0.221 +1.702      : +1.152 +0.536 +0.515      : -0.796 +1.578 -2.703
X unfilterd : +8.157 +1.902 +1.532    : -0.690 +5.790 +1.734      : +2.025 +0.077 +0.447      : +1.472 +0.634 -2.745
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.500 +0.000 +1.500    : +1.500 +0.000 +1.500      : +1.500 +0.000 +1.500      : +1.500 +0.000 +1.500
X estimated : +1.518 -0.084 +1.602    : +1.529 -0.179 +1.721      : +1.396 +0.305 +1.071      : -1.316 +1.580 -3.016
X unfilterd : +8.241 +2.999 +1.608    : -1.815 +5.621 +1.756      : +1.732 -1.122 +0.978      : +2.449 +1.276 -3.086
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.600 +0.000 +1.600    : +1.600 +0.000 +1.600      : +1.600 +0.000 +1.600      : +1.600 +0.000 +1.600
X estimated : +1.613 -0.069 +1.683    : +1.649 -0.208 +1.866      : +1.352 +0.580 +0.781      : -1.029 +1.663 -2.673
X unfilterd : +9.188 +3.454 +1.689    : -1.395 +5.208 +1.896      : +1.611 -0.472 +0.714      : +2.183 +1.628 -2.800
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.700 +0.000 +1.700    : +1.700 +0.000 +1.700      : +1.700 +0.000 +1.700      : +1.700 +0.000 +1.700
X estimated : +1.741 -0.105 +1.824    : +1.767 -0.261 +2.007      : +1.330 +0.762 +0.669      : +1.818 -1.023 +2.694
X unfilterd : +9.905 +2.558 +1.828    : -0.539 +5.902 +2.038      : +2.029 -0.033 +0.574      : -2.070 -1.881 +2.661
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.800 +0.000 +1.800    : +1.800 +0.000 +1.800      : +1.800 +0.000 +1.800      : +1.800 +0.000 +1.800
X estimated : +1.835 -0.145 +1.960    : +1.868 -0.309 +2.122      : +1.347 +0.824 +0.700      : -1.056 +1.739 -2.450
X unfilterd : +10.770  +1.416  +1.963 : -1.142 +5.692 +2.155      : +1.128 +0.568 +0.651      : +2.059 +0.950 -2.559
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +1.900 +0.000 +1.900    : +1.900 +0.000 +1.900      : +1.900 +0.000 +1.900      : +1.900 +0.000 +1.900
X estimated : +1.939 -0.156 +2.045    : +1.970 -0.271 +2.175      : +1.217 +1.099 +0.427      : +2.071 -1.460 +3.120
X unfilterd : +12.221  +1.377  +2.046 : -0.173 +6.663 +2.208      : +0.837 +0.750 +0.393      : -2.680 -3.166 +3.003
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.000 +0.000 +2.000    : +2.000 +0.000 +2.000      : +2.000 +0.000 +2.000      : +2.000 +0.000 +2.000
X estimated : +2.057 -0.170 +2.166    : +2.067 -0.201 +2.188      : +1.350 +1.062 +0.668      : +2.116 -0.384 +2.350
X unfilterd : +12.055  +0.006  +2.168 : +0.293 +5.679 +2.213      : +1.238 +0.644 +0.606      : -1.671 -3.108 +2.293
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.100 +0.000 +2.100    : +2.100 +0.000 +2.100      : +2.100 +0.000 +2.100      : +2.100 +0.000 +2.100
X estimated : +2.151 -0.182 +2.270    : +2.167 -0.217 +2.293      : +1.123 +1.318 +0.347      : +2.311 -0.849 +2.800
X unfilterd : +13.226  +0.200  +2.270 : -0.257 +6.112 +2.325      : +0.183 +1.011 +0.336      : -3.458 -3.378 +2.749
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.200 +0.000 +2.200    : +2.200 +0.000 +2.200      : +2.200 +0.000 +2.200      : +2.200 +0.000 +2.200
X estimated : +2.272 -0.196 +2.377    : +2.291 -0.231 +2.407      : +1.096 +1.407 +0.371      : -2.235 +1.403 -2.974
X unfilterd : +14.939  +0.086  +2.374 : -0.399 +6.854 +2.446      : +0.604 +0.897 +0.333      : +4.817 +1.861 -2.968
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.300 +0.000 +2.300    : +2.300 +0.000 +2.300      : +2.300 +0.000 +2.300      : +2.300 +0.000 +2.300
X estimated : +2.394 -0.256 +2.507    : +2.364 -0.207 +2.474      : +1.384 +1.235 +0.824      : -1.525 +1.742 -2.296
X unfilterd : +15.433  +0.187  +2.506 : +0.978 +6.423 +2.503      : +1.745 +0.889 +0.722      : +5.319 -1.007 -2.273
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.400 +0.000 +2.400    : +2.400 +0.000 +2.400      : +2.400 +0.000 +2.400      : +2.400 +0.000 +2.400
X estimated : +2.509 -0.280 +2.618    : +2.522 -0.315 +2.645      : +1.441 +1.262 +0.953      : -2.236 +1.378 -2.784
X unfilterd : +17.010  -0.169  +2.615 : +1.884 +5.339 +2.666      : +2.579 +0.694 +0.802      : +5.785 -0.389 -2.729
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.500 +0.000 +2.500    : +2.500 +0.000 +2.500      : +2.500 +0.000 +2.500      : +2.500 +0.000 +2.500
X estimated : +2.618 -0.285 +2.721    : +2.689 -0.441 +2.835      : +1.849 +0.972 +1.528      : -2.770 +1.012 -3.095
X unfilterd : +17.802  +0.106  +2.718 : +2.593 +6.539 +2.860      : +2.587 +0.110 +1.395      : +6.842 +0.412 -2.990
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.600 +0.000 +2.600    : +2.600 +0.000 +2.600      : +2.600 +0.000 +2.600      : +2.600 +0.000 +2.600
X estimated : +2.768 -0.352 +2.860    : +2.843 -0.624 +3.034      : +2.073 +0.796 +1.868      : +2.719 -0.269 +2.780
X unfilterd : +18.910  -1.005  +2.856 : +3.972 +5.797 +3.053      : +3.298 -0.853 +1.691      : -5.191 -0.907 +2.859
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.700 +0.000 +2.700    : +2.700 +0.000 +2.700      : +2.700 +0.000 +2.700      : +2.700 +0.000 +2.700
X estimated : +2.850 -0.339 +2.926    : -2.877 +0.764 -3.062      : +2.080 +0.868 +1.890      : +2.241 +0.733 +2.066
X unfilterd : +20.179  -1.848  +2.920 : -4.098 -6.092 -3.037      : +4.125 -1.099 +1.672      : -2.834 -3.271 +2.159
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.800 +0.000 +2.800    : +2.800 +0.000 +2.800      : +2.800 +0.000 +2.800      : +2.800 +0.000 +2.800
X estimated : +2.949 -0.331 +3.016    : -2.720 +0.863 -2.879      : +2.147 +0.920 +1.984      : +1.609 +1.404 +1.373
X unfilterd : +19.520  -3.545  +3.013 : -3.510 -4.825 -2.851      : +3.728 -2.212 +1.786      : -1.216 -3.829 +1.469
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +2.900 +0.000 +2.900    : +2.900 +0.000 +2.900      : +2.900 +0.000 +2.900      : +2.900 +0.000 +2.900
X estimated : +3.081 -0.368 +3.132    : -2.768 +0.730 -2.859      : +2.361 +0.769 +2.260      : +1.538 +1.499 +1.333
X unfilterd : +19.830  -3.321  +3.130 : -3.994 -5.756 -2.831      : +4.208 -2.418 +2.025      : -1.062 -4.987 +1.464
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +3.000 +0.000 +3.000    : +3.000 +0.000 +3.000      : +3.000 +0.000 +3.000      : +3.000 +0.000 +3.000
X estimated : -3.017 +0.353 -3.054    : -2.662 +0.795 -2.716      : +2.768 +0.349 +2.746      : +1.272 +1.675 +1.153
X unfilterd : -19.137  +5.370  -3.056 : -5.403 -4.678 -2.698      : +3.942 -4.575 +2.500      : -0.704 -4.868 +1.282
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : +3.100 +0.000 +3.100    : +3.100 +0.000 +3.100      : +3.100 +0.000 +3.100      : +3.100 +0.000 +3.100
X estimated : -2.946 +0.348 -2.946    : -2.657 +0.709 -2.657      : +2.582 +0.673 +2.581      : +2.113 +1.096 +2.115
X unfilterd : -18.020  +5.076  -2.946 : -4.727 -3.732 -2.634      : +5.182 -4.663 +2.297      : -1.917 -4.944 +2.130
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -3.083 -0.000 -3.083    : -3.083 -0.000 -3.083      : -3.083 -0.000 -3.083      : -3.083 -0.000 -3.083
X estimated : -2.881 +0.296 -2.886    : -2.636 +0.622 -2.613      : +2.833 +0.511 +2.839      : +2.717 +0.611 +2.735
X unfilterd : -16.227  +4.247  -2.883 : -5.132 -2.807 -2.595      : +3.984 -5.460 +2.594      : -4.905 -4.387 +2.820
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.983 -0.000 -2.983    : -2.983 -0.000 -2.983      : -2.983 -0.000 -2.983      : -2.983 -0.000 -2.983
X estimated : -2.818 +0.242 -2.803    : -2.592 +0.556 -2.556      : +2.382 +1.045 +2.460      : +3.075 +0.326 +3.075
X unfilterd : -16.239  +3.306  -2.801 : -4.543 -2.006 -2.533      : +3.868 -4.472 +2.251      : +6.885 +4.276 -3.061
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.883 -0.000 -2.883    : -2.883 -0.000 -2.883      : -2.883 -0.000 -2.883      : -2.883 -0.000 -2.883
X estimated : -2.790 +0.178 -2.749    : -2.468 +0.621 -2.388      : +2.761 +0.744 +2.852      : -2.493 +0.608 -2.401
X unfilterd : -16.468  +3.002  -2.749 : -4.427 -1.586 -2.364      : +2.085 -6.157 +2.692      : +6.918 +1.787 -2.248
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.783 -0.000 -2.783    : -2.783 -0.000 -2.783      : -2.783 -0.000 -2.783      : -2.783 -0.000 -2.783
X estimated : -2.676 +0.207 -2.637    : -2.364 +0.637 -2.246      : +2.951 +0.663 +3.053      : -1.738 +1.293 -1.514
X unfilterd : -17.312  +4.557  -2.641 : -4.687 -2.657 -2.224      : +1.513 -8.039 +2.899      : +5.889 -1.179 -1.396
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.683 -0.000 -2.683    : -2.683 -0.000 -2.683      : -2.683 -0.000 -2.683      : -2.683 -0.000 -2.683
X estimated : -2.596 +0.157 -2.561    : -2.300 +0.612 -2.171      : +2.588 +1.053 +2.816      : -1.079 +1.683 -0.702
X unfilterd : -15.711  +4.166  -2.561 : -5.721 -2.061 -2.157      : +1.186 -8.120 +2.713      : +5.009 -4.117 -0.537
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.583 -0.000 -2.583    : -2.583 -0.000 -2.583      : -2.583 -0.000 -2.583      : -2.583 -0.000 -2.583
X estimated : -2.477 +0.207 -2.408    : -2.293 +0.503 -2.157      : +2.688 +1.005 +2.962      : -1.656 +1.231 -1.314
X unfilterd : -15.579  +5.003  -2.411 : -7.129 -1.812 -2.156      : +1.352 -7.569 +2.819      : +5.240 -3.220 -1.160
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.483 -0.000 -2.483    : -2.483 -0.000 -2.483      : -2.483 -0.000 -2.483      : -2.483 -0.000 -2.483
X estimated : -2.385 +0.216 -2.315    : -2.203 +0.513 -2.021      : +2.654 +1.092 +3.011      : -0.953 +1.660 -0.399
X unfilterd : -13.615  +5.285  -2.314 : -6.435 -2.216 -2.015      : +1.172 -8.897 +2.893      : +3.533 -5.672 -0.224
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.383 -0.000 -2.383    : -2.383 -0.000 -2.383      : -2.383 -0.000 -2.383      : -2.383 -0.000 -2.383
X estimated : -2.326 +0.151 -2.267    : -2.130 +0.489 -1.937      : +2.392 +1.288 +2.903      : -0.581 +1.745 +0.112
X unfilterd : -12.351  +5.515  -2.263 : -6.695 -1.893 -1.933      : +2.762 -8.807 +2.765      : +2.414 -5.708 +0.242
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.283 -0.000 -2.283    : -2.283 -0.000 -2.283      : -2.283 -0.000 -2.283      : -2.283 -0.000 -2.283
X estimated : -2.249 +0.091 -2.198    : -2.085 +0.414 -1.889      : +2.569 +1.208 +3.120      : -0.095 +1.863 +0.763
X unfilterd : -12.025  +6.530  -2.193 : -7.359 -2.165 -1.893      : +1.850 -8.645 +2.957      : -0.111 -6.792 +0.908
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.183 +0.000 -2.183    : -2.183 +0.000 -2.183      : -2.183 +0.000 -2.183      : -2.183 +0.000 -2.183
X estimated : -2.121 +0.117 -2.075    : -1.969 +0.442 -1.736      : -2.422 -0.841 -2.850      : +0.774 +1.863 +1.747
X unfilterd : -12.080  +7.385  -2.072 : -7.640 -2.250 -1.745      : -0.491 +7.910 -3.041      : -3.353 -6.625 +1.880
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -2.083 +0.000 -2.083    : -2.083 +0.000 -2.083      : -2.083 +0.000 -2.083      : -2.083 +0.000 -2.083
X estimated : -2.015 +0.141 -1.945    : -1.890 +0.411 -1.649      : +2.255 +1.407 +3.077      : +1.116 +1.815 +2.150
X unfilterd : -10.883  +8.108  -1.940 : -7.012 -2.864 -1.654      : +1.513 -6.916 +2.876      : -5.338 -6.772 +2.358
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.983 +0.000 -1.983    : -1.983 +0.000 -1.983      : -1.983 +0.000 -1.983      : -1.983 +0.000 -1.983
X estimated : -1.938 +0.108 -1.865    : -1.834 +0.364 -1.598      : +1.613 +1.675 +2.700      : +1.525 +1.652 +2.633
X unfilterd : -11.229  +8.365  -1.864 : -6.931 -3.146 -1.604      : +2.316 -5.736 +2.496      : -6.396 -5.787 +2.805
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.883 -0.000 -1.883    : -1.883 -0.000 -1.883      : -1.883 -0.000 -1.883      : -1.883 -0.000 -1.883
X estimated : -1.867 +0.088 -1.792    : -1.767 +0.310 -1.533      : -2.082 -1.208 -2.960      : +1.454 +1.690 +2.659
X unfilterd : -11.136  +7.322  -1.795 : -6.761 -2.898 -1.537      : +2.115 -6.911 +3.075      : -6.564 -4.848 +2.767
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.783 -0.000 -1.783    : -1.783 -0.000 -1.783      : -1.783 -0.000 -1.783      : -1.783 -0.000 -1.783
X estimated : -1.748 +0.118 -1.667    : -1.699 +0.222 -1.521      : -1.919 -0.598 -2.401      : +0.611 +1.784 +2.028
X unfilterd : -8.945 +7.576 -1.665    : -5.781 -3.458 -1.518      : +0.016 +6.227 -2.652      : -3.766 -5.933 +2.197
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.683 -0.000 -1.683    : -1.683 -0.000 -1.683      : -1.683 -0.000 -1.683      : -1.683 -0.000 -1.683
X estimated : -1.646 +0.102 -1.566    : -1.603 +0.203 -1.439      : -1.777 -0.450 -2.168      : +1.163 +1.715 +2.652
X unfilterd : -8.571 +6.948 -1.567    : -4.905 -3.123 -1.427      : +0.896 +5.084 -2.445      : -6.152 -5.864 +2.895
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.583 -0.000 -1.583    : -1.583 -0.000 -1.583      : -1.583 -0.000 -1.583      : -1.583 -0.000 -1.583
X estimated : -1.551 +0.122 -1.444    : -1.534 +0.170 -1.376      : -1.662 -0.206 -1.845      : -1.590 -1.524 -3.102
X unfilterd : -9.047 +7.182 -1.450    : -5.567 -2.233 -1.366      : +1.146 +3.357 -2.169      : +7.845 +4.126 -2.842
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.483 -0.000 -1.483    : -1.483 -0.000 -1.483      : -1.483 -0.000 -1.483      : -1.483 -0.000 -1.483
X estimated : -1.448 +0.102 -1.335    : -1.453 +0.104 -1.323      : -1.582 -0.381 -1.986      : -1.499 -1.485 -3.081
X unfilterd : -8.323 +6.988 -1.342    : -5.112 -1.819 -1.305      : +0.661 +2.193 -2.353      : +7.809 +4.706 -2.749
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.383 -0.000 -1.383    : -1.383 -0.000 -1.383      : -1.383 -0.000 -1.383      : -1.383 -0.000 -1.383
X estimated : -1.348 +0.087 -1.253    : -1.352 +0.095 -1.244      : -1.439 -0.285 -1.778      : -1.458 -0.743 -2.330
X unfilterd : -7.249 +7.520 -1.256    : -4.836 -1.425 -1.223      : +1.060 +1.032 -2.178      : +7.323 +1.680 -1.931
----------------------------------    ----------------------      ----------------------      ----------------------
X simulated : -1.283 +0.000 -1.283    : -1.283 +0.000 -1.283      : -1.283 +0.000 -1.283      : -1.283 +0.000 -1.283
X estimated : -1.260 +0.051 -1.195    : -1.248 +0.139 -1.067      : -1.340 -0.269 -1.676      : -1.311 -0.004 -1.251
X unfilterd : -6.460 +7.882 -1.196    : -5.527 -1.074 -1.053      : +0.925 +1.687 -2.058      : +7.127 -2.524 -0.932
----------------------------------    ----------------------      ----------------------      ----------------------

[joansola]

So here is what happens.

You are using motion steps of 10cm forward, 0.1rad left.

Then you are using perturbations, at each step, of 1m forwards and sideways, and up to 1rad turning. the noises are much larger than the motion steps.

Under these conditions, there is no way one can guess the orientation of the vehicle from its trajectory. the motion is just too erratic.

XY converge because of GNSS.

Theta is practically unobservable.

To make theta observable, you need to make the XY noise smaller than the motion step, which is 10cm. So I recommend u_sigma 0.1, 0.1 for the XY maximum.

Then for theta, you already trien 0.05 and 0.1. they should be about right, but in the upper limit.

I will accept this PR. You can try to lower u_sigma to not be larger than u_nominal and see if you can make theta observable. You may need long trajectories to allow X_unfiltered to diverge while X_filtered stays around X_nominal.

Thanks for the effort.

[Chris7462]

Thank you! I forgot to check whether the number of motion steps make sense after multiplying the control by a factor of 10. I understand that perturbations should be much smaller compared to the motions.

I have conducted another simulation with 1,000 steps, and the setup was configured as requested. Based on this, I believe there is sufficient evidence to conclude that the filter is working properly. Really appreciate your insight and help!

u_nom    << 0.1, 0.0, 0.1;
u_sigmas << 0.1, 0.1, 0.05;
y_sigmas << 0.01, 0.01;

X STATE     :    X      Y    THETA
----------------------------------
X initial   : +0.000 +0.000 +0.000
----------------------------------
X simulated : +0.100 +0.000 +0.100
X estimated : +0.103 -0.003 +0.143
X unfilterd : +0.197 +0.057 +0.144
----------------------------------
.
// deleted
.
----------------------------------
X simulated : -0.531 -0.000 -0.531
X estimated : -0.528 +0.031 -0.429
X unfilterd : -1.674 +2.503 -1.781
----------------------------------

[artivis]

I believe this should be X.lplus(-dx).

[Chris7462]

Please see my comment here: #308 (comment)

[joansola]

This PR provides an example of the left IEKF method based on position measurements (GPS-like). The example was adapted from here with some corrections in the update step.

Yes, the last step is wrong

[Chris7462]

Update the PR based on the discussion above. The following results are for 1000 steps.

u_nom    << 0.1, 0.0, 0.1;
u_sigmas << 0.1, 0.1, 0.05;
y_sigmas << 0.01, 0.01;

X STATE     :    X      Y    THETA
----------------------------------
X initial   : +0.000 +0.000 +0.000
----------------------------------
X simulated : +0.100 +0.000 +0.100
X estimated : +0.091 -0.003 +0.065
X unfilterd : +0.022 -0.010 +0.065
----------------------------------
.
// deleted
.
----------------------------------
X simulated : -0.531 -0.000 -0.531
X estimated : -0.536 -0.001 -0.538
X unfilterd : +0.976 -0.442 -0.807
----------------------------------