Noise propagation

doc/ImuFactor.lyx

@@ -455,7 +455,7 @@ and the 6-vectors
 \end_layout
 
 \begin_layout Subsubsection*
-Local Coordinates
+Derivative of The Local Coordinate Mapping
 \end_layout
 
 \begin_layout Standard
@@ -1112,11 +1112,11 @@ A Simple Euler Scheme
 \begin_layout Standard
 To solve the differential equation we can use a simple Euler scheme:
 \begin_inset Formula 
-\begin{eqnarray*}
+\begin{eqnarray}
-\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\\
+\theta_{k+1}=\theta_{k}+\dot{\theta}(t_{k})\Delta_{t} & = & \theta_{k}+H(\theta_{k})^{-1}\,\omega_{k}^{b}\Delta_{t}\label{eq:euler_theta}\\
-p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\\
+p_{k+1}=p_{k}+\dot{p}_{v}(t_{k})\Delta_{t} & = & p_{k}+v_{k}\Delta_{t}\label{eq:euler_p}\\
-v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\theta_{k}\right)a_{k}^{b}\Delta_{t}
+v_{k+1}=v_{k}+\dot{v}_{a}(t_{k})\Delta_{t} & = & v_{k}+\exp\left(\Skew{\theta_{k}}\right)a_{k}^{b}\Delta_{t}\label{eq:euler_v}
-\end{eqnarray*}
+\end{eqnarray}
 
 \end_inset
 
@@ -1135,25 +1135,264 @@ where
 .
 \end_layout
 
+\begin_layout Subsubsection*
+Noise Propagation
+\end_layout
+
 \begin_layout Standard
-In the above, we have to think about how to handle both bias 
+Even when we assume uncorrelated noise on 
-\begin_inset Formula $(b_{g},b_{a})$
+\begin_inset Formula $\omega^{b}$
-\end_inset
-
- and lever arm 
-\begin_inset Formula $T_{s}^{b}$
-\end_inset
-
-.
- Both of them can be seen as arguments to two functions
-\begin_inset Formula $\omega_{k}^{b}(b_{g})$
 \end_inset
 
  and 
-\begin_inset Formula $a_{k}^{b}(b_{a},T_{s}^{b})$
+\begin_inset Formula $a^{b}$
 \end_inset
 
-, and hence we have to properly account for their derivatives.
+, the noise on the final computed quantities will have a non-trivial covariance
+ structure, because the intermediate quantities 
+\begin_inset Formula $\theta_{k}$
+\end_inset
+
+and 
+\begin_inset Formula $v_{k}$
+\end_inset
+
+ appear in multiple places.
+ To model the noise propagation, let us define 
+\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
+\end_inset
+
+ and rewrite Eqns.
+ (
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq:euler_theta"
+
+\end_inset
+
+
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq:euler_v"
+
+\end_inset
+
+) as the non-linear function 
+\begin_inset Formula $f$
+\end_inset
+
+
+\begin_inset Formula 
+\[
+\zeta_{k+1}=f\left(\zeta_{k},\omega_{k}^{b},a_{k}^{b}\right)
+\]
+
+\end_inset
+
+Then the noise on 
+\begin_inset Formula $\zeta_{k+1}$
+\end_inset
+
+ propagates as
+\begin_inset Formula 
+\begin{equation}
+\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{gd}B_{k}+C_{k}\Sigma_{\eta}^{ad}C_{k}\label{eq:prop}
+\end{equation}
+
+\end_inset
+
+where 
+\begin_inset Formula $A_{k}$
+\end_inset
+
+ is the 
+\begin_inset Formula $9\times9$
+\end_inset
+
+ partial derivative of 
+\begin_inset Formula $f$
+\end_inset
+
+ wrpt 
+\begin_inset Formula $\zeta$
+\end_inset
+
+, and 
+\begin_inset Formula $B_{k}$
+\end_inset
+
+ and 
+\begin_inset Formula $C_{k}$
+\end_inset
+
+ the respective 
+\begin_inset Formula $9\times3$
+\end_inset
+
+ partial derivatives with respect to the measured quantities 
+\begin_inset Formula $\omega^{b}$
+\end_inset
+
+ and 
+\begin_inset Formula $a^{b}$
+\end_inset
+
+.
+ Noting that
+\begin_inset Formula 
+\[
+H(\theta)=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\Skew{\theta}^{k}\approx I-\frac{1}{2}\Skew{\theta}
+\]
+
+\end_inset
+
+for small 
+\begin_inset Formula $\theta$
+\end_inset
+
+, and 
+\begin_inset Formula 
+\[
+\deriv{\Skew{\theta}\omega}{\theta}=\deriv{\left(\theta\times\omega\right)}{\theta}=-\deriv{\left(\omega\times\theta\right)}{\theta}=-\deriv{\Skew{\omega}\theta}{\theta}=-\Skew{\omega}
+\]
+
+\end_inset
+
+we have 
+\begin_inset Formula 
+\[
+\deriv{H(\theta)\omega}{\theta}\approx\frac{1}{2}\Skew{\omega}
+\]
+
+\end_inset
+
+Similarly, 
+\begin_inset Formula 
+\[
+\exp\left(\Skew{\theta}\right)=\sum_{k=0}^{\infty}\frac{1}{k!}\Skew{\theta}^{k}\approx I+\Skew{\theta}
+\]
+
+\end_inset
+
+and hence
+\begin_inset Formula 
+\[
+\deriv{\exp\left(\Skew{\theta}\right)a}{\theta}\approx-\Skew a
+\]
+
+\end_inset
+
+so we finally obtain
+\begin_inset Formula 
+\[
+A_{k}\approx\left[\begin{array}{ccc}
+I_{3\times3}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Delta_{t}\\
+ & I_{3\times3} & I_{3\times3}\Delta_{t}\\
+-\Skew{a_{k}^{b}}\Delta_{t} &  & I_{3\times3}
+\end{array}\right]
+\]
+
+\end_inset
+
+The other partial derivatives are simply
+\begin_inset Formula 
+\[
+B_{k}=\left[\begin{array}{c}
+H(\theta_{k})^{-1}\Delta^{t}\\
+0_{3\times3}\\
+0_{3\times3}
+\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c}
+0_{3\times3}\\
+0_{3\times3}\\
+\exp\left(\Skew{\theta_{k}}\right)\Delta_{t}
+\end{array}\right]
+\]
+
+\end_inset
+
+Substituting these expressions into Eq.
+ 
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq:prop"
+
+\end_inset
+
+ and dropping terms involving 
+\begin_inset Formula $\Delta_{t}^{2}$
+\end_inset
+
+, we obtain
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\uuline off
+\uwave off
+\noun off
+\color none
+
+\begin_inset Formula 
+\[
+\Sigma_{k+1}=\Sigma_{k}+\left[\begin{array}{ccc}
+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta\theta}-\Sigma_{k}^{\theta\theta}\frac{1}{2}\Skew{\omega_{k}^{b}} & \Sigma_{k}^{\theta v}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta p} & \Sigma_{k}^{\theta\theta}\Skew{a_{k}^{b}}+\frac{1}{2}\Skew{\omega_{k}^{b}}\Sigma_{k}^{\theta v}\\
+. & \Sigma_{k}^{pv}+\Sigma_{k}^{vp} & \Sigma_{k}^{vv}+\Sigma_{k}^{p\theta}\Skew{a_{k}^{b}}\\
+. & . & \Sigma_{k}^{v\theta}\Skew{a_{k}^{b}}-\Skew{a_{k}^{b}}\Sigma_{k}^{\theta v}
+\end{array}\right]\Delta^{t}+\Sigma_{k}^{\eta}
+\]
+
+\end_inset
+
+where we only show the upper-triangular part (the matrix is symmetric) and
+ where 
+\begin_inset Formula 
+\[
+\Sigma_{k}^{\eta}=B_{k}\Sigma_{\eta}^{gd}B_{k}+C_{k}\Sigma_{\eta}^{ad}C_{k}=\left[\begin{array}{ccc}
+\sigma^{g}I_{3\times3}\\
+\\
+ &  & \sigma^{a}I_{3\times3}
+\end{array}\right]\Delta_{t}
+\]
+
+\end_inset
+
+The equality in the last line holds in the case of isotropic Gaussian measuremen
+t noise, in which case 
+\begin_inset Formula $\Sigma_{\eta}^{gd}$
+\end_inset
+
+=
+\begin_inset Formula $\sigma^{g}I_{3\times3}/\Delta_{t}$
+\end_inset
+
+ and 
+\begin_inset Formula $\Sigma_{\eta}^{ga}$
+\end_inset
+
+=
+\begin_inset Formula $\sigma^{a}I_{3\times3}/\Delta_{t}$
+\end_inset
+
+, and used the identities 
+\begin_inset Formula $H(\theta)^{-1}H(\theta)^{-T}\approx I_{3\times3}$
+\end_inset
+
+ for small 
+\begin_inset Formula $\theta$
+\end_inset
+
+, and 
+\begin_inset Formula $\exp\left(\Skew{\theta}\right)\exp\left(\Skew{\theta}\right)^{T}=I_{3\times3}$
+\end_inset
+
+ for all 
+\begin_inset Formula $\theta$
+\end_inset
+
+.
 \end_layout
 
 \begin_layout Section