 |
0.3.0
|
|
0.3.0
|
![\begin{equation} \label{eq:eq-LCKF_e-stateMatrix-short}
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},nodes={rectangle,text width=1.8em,align=center},row sep=0.3em, column sep=0.4em] {
\boldsymbol{\delta \dotup{\psi}}_{eb}^e \\
\boldsymbol{\delta \dotup{v}}_{eb}^e \\
\boldsymbol{\delta \dotup{r}}_{eb}^e \\
\boldsymbol{\delta \dotup{f}}_{ib}^{\raise-0.45ex\hbox{$\scriptstyle b$}} \\
\boldsymbol{\delta \dotup{\omega}}_{ib}^b \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\boldsymbol{\delta}\mathbf{\dotup{x}}$} (m-1-1.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[blue!20]{m-2-1}{m-2-1}
\fhighlight[cyan!20]{m-3-1}{m-3-1}
\fhighlight[red!20]{m-4-1}{m-4-1}
\fhighlight[orange!20]{m-5-1}{m-5-1}
\end{pgfonlayer}
\end{tikzpicture}
=
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=2.5em,align=center},row sep=0.3em, column sep=0.4em]
{
\mathbf{F}_{\dotup{\psi},\psi}^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{C}_b^e \\
\mathbf{F}_{\delta \dotup{v},\psi}^e & \mathbf{F}_{\delta \dotup{v},\delta v}^e & \mathbf{F}_{\delta \dotup{v},\delta r}^e & \mathbf{C}_b^e & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\mathbf{F}$} (m-1-5.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[orange!20]{m-1-5}{m-1-5}
\fhighlight[purple!20]{m-2-1}{m-2-1}
\fhighlight[blue!20]{m-2-2}{m-2-2}
\fhighlight[violet!20]{m-2-3}{m-2-3}
\fhighlight[red!20]{m-2-4}{m-2-4}
\fhighlight[cyan!20]{m-3-2}{m-3-2}
\end{pgfonlayer}
\end{tikzpicture}
\cdot
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=1.8em,align=center},row sep=0.3em, column sep=0.4em] {
\boldsymbol{\delta\psi}_{eb}^e \\
\boldsymbol{\delta v}_{eb}^e \\
\boldsymbol{\delta r}_{eb}^e \\
\boldsymbol{\delta f}_{ib}^b \\
\boldsymbol{\delta \omega}_{ib}^b \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\boldsymbol{\delta}\mathbf{x}$} (m-1-1.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[blue!20]{m-2-1}{m-2-1}
\fhighlight[cyan!20]{m-3-1}{m-3-1}
\fhighlight[red!20]{m-4-1}{m-4-1}
\fhighlight[orange!20]{m-5-1}{m-5-1}
\end{pgfonlayer}
\end{tikzpicture}
\end{equation}](form_241.svg)
![\begin{equation} \label{eq:eq-LCKF_e-stateMatrix}
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=2.4em,align=center,minimum height=2.3em,anchor=center},row sep=0.3em, column sep=0.4em,] {
\delta \dotup{\psi}_{eb,1}^e \\ \delta \dotup{\psi}_{eb,2}^e \\ \delta \dotup{\psi}_{eb,3}^e \\
\delta \dotup{v}_{eb,x}^e \\ \delta \dotup{v}_{eb,y}^e \\ \delta \dotup{v}_{eb,z}^e \\
\delta \dotup{r}_{eb,x}^e \\ \delta \dotup{r}_{eb,y}^e \\ \delta \dotup{r}_{eb,z}^e \\
\delta \dotup{f}_{x}^{b} \\ \delta \dotup{f}_{y}^{b} \\ \delta \dotup{f}_{z}^{b} \\
\delta \dotup{\omega}_{x}^{b} \\ \delta \dotup{\omega}_{y}^{b} \\ \delta \dotup{\omega}_{z}^{b} \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\boldsymbol{\delta}\mathbf{\dotup{x}}$} (m-1-1.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-3-1}
\fhighlight[blue!20]{m-4-1}{m-6-1}
\fhighlight[cyan!20]{m-7-1}{m-9-1}
\fhighlight[red!20]{m-10-1}{m-12-1}
\fhighlight[orange!20]{m-13-1}{m-15-1}
\end{pgfonlayer}
\end{tikzpicture}
=
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},nodes in empty cells,
row sep=0.3em, column sep=0.4em,
nodes={rectangle,minimum height=2.3em,anchor=center},
column 6/.style = {nodes={minimum width=1.0cm}},] {
0 & \omega_{ie} & 0 & & & & & & & & & & & & \\
-\omega_{ie} & 0 & 0 & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{C}_b^e & \\
0 & 0 & 0 & & & & & & & & & & & & \\
0 & f_{ib,z}^e & -f_{ib,y}^e & 0 & 2 \omega_{ie} & 0 & & & & & & & & & \\
-f_{ib,z}^e & 0 & f_{ib,x}^e & -2 \omega_{ie} & 0 & 0 & & - \left( \dfrac{2 \boldsymbol{\gamma}_{ib}^e}{r_{eS}^e} \dfrac{{\mathbf{r}_{eb}^e}^T}{\left| \mathbf{r}_{eb}^e \right|} + \boldsymbol{\Omega}_{ie}^e \boldsymbol{\Omega}_{ie}^e \right) & & & \mathbf{C}_b^e & & & \mathbf{0}_3 & \\
f_{ib,y}^e & -f_{ib,x}^e & 0 & 0 & 0 & 0 & & & & & & & & & \\
& & & & & & & & & & & & & & \\
& \mathbf{0}_3 & & & \mathbf{I}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\
& & & & & & & & & & & & & & \\
& & & & & & & & & & & & & & \\
& \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\
& & & & & & & & & & & & & & \\
& & & & & & & & & & & & & & \\
& \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\
& & & & & & & & & & & & & & \\
};
\foreach \r in {1,3,4,6,7,9} {
\foreach \c in {1,3,4,6,7,9,10,12,13,15} {
\coordinate (m-\r-\c-nw) at (100,-100);
\coordinate (m-\r-\c-se) at (-100,100);
\foreach \row in {1,...,9} {
\path let \p1=(m-\row-\c.west), \p2=(m-\r-\c-nw) in coordinate (m-\r-\c-nw) at ({min(\x1,\x2)},\y2);
\path let \p1=(m-\row-\c.east), \p2=(m-\r-\c-se) in coordinate (m-\r-\c-se) at ({max(\x1,\x2)},\y2);
}
\foreach \col in {1,...,15} {
\path let \p1=(m-\r-\col.north), \p2=(m-\r-\c-nw) in coordinate (m-\r-\c-nw) at (\x2,{max(\y1,\y2)});
\path let \p1=(m-\r-\col.south), \p2=(m-\r-\c-se) in coordinate (m-\r-\c-se) at (\x2,{min(\y1,\y2)});
}
}
}
\path let \p1=(m-1-1-nw), \p2=(m-1-15-se) in coordinate (m-1-15-ne) at (\x2,\y1);
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1-nw) -- node[above=2pt] {$\mathbf{F}$} (m-1-15-ne);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1-nw}{m-3-3-se}
\fhighlight[orange!20]{m-1-13-nw}{m-3-15-se}
\fhighlight[purple!20]{m-4-1-nw}{m-6-3-se}
\fhighlight[blue!20]{m-4-4-nw}{m-6-6-se}
\fhighlight[violet!20]{m-4-7-nw}{m-6-9-se}
\fhighlight[red!20]{m-4-10-nw}{m-6-12-se}
\fhighlight[cyan!20]{m-7-4-nw}{m-9-6-se}
\end{pgfonlayer}
\end{tikzpicture}
\cdot
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},row sep=0.3em, column sep=0.4em,
nodes={rectangle,text width=2.4em,align=center,minimum height=2.3em,anchor=center},] {
\delta \psi_{eb,1}^e \\ \delta \psi_{eb,2}^e \\ \delta \psi_{eb,3}^e \\
\delta v_{eb,x}^e \\ \delta v_{eb,y}^e \\ \delta v_{eb,z}^e \\
\delta r_{eb,x}^e \\ \delta r_{eb,y}^e \\ \delta r_{eb,z}^e \\
\delta f_{x}^{b} \\ \delta f_{y}^{b} \\ \delta f_{z}^{b} \\
\delta \omega_{x}^{b} \\ \delta \omega_{y}^{b} \\ \delta \omega_{z}^{b} \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\boldsymbol{\delta}\mathbf{x}$} (m-1-1.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-3-1}
\fhighlight[blue!20]{m-4-1}{m-6-1}
\fhighlight[cyan!20]{m-7-1}{m-9-1}
\fhighlight[red!20]{m-10-1}{m-12-1}
\fhighlight[orange!20]{m-13-1}{m-15-1}
\end{pgfonlayer}
\end{tikzpicture}
\end{equation}](form_242.svg)
This form corresponds to
The continuous linear dynamic system can be expressed as
Process noise is introduced into the 
and 
matrices in the accelerometer and gyroscope bias terms. Therefore the error equations can be extended as follows
![\begin{equation} \label{eq:eq-LCKF_e-dynamicModel-short}
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},nodes={rectangle,text width=1.8em,align=center},row sep=0.3em, column sep=0.4em] {
\boldsymbol{\delta \dotup{\psi}}_{eb}^e \\
\boldsymbol{\delta \dotup{v}}_{eb}^e \\
\boldsymbol{\delta \dotup{r}}_{eb}^e \\
\boldsymbol{\delta \dotup{f}}_{ib}^{\raise-0.45ex\hbox{$\scriptstyle b$}} \\
\boldsymbol{\delta \dotup{\omega}}_{ib}^b \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\boldsymbol{\delta} \mathbf{\dotup{x}}$} (m-1-1.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[blue!20]{m-2-1}{m-2-1}
\fhighlight[cyan!20]{m-3-1}{m-3-1}
\fhighlight[red!20]{m-4-1}{m-4-1}
\fhighlight[orange!20]{m-5-1}{m-5-1}
\end{pgfonlayer}
\end{tikzpicture}
=
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=2.5em,align=center},row sep=0.3em, column sep=0.4em]
{
\mathbf{F}_{\dotup{\psi},\psi}^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{C}_b^e \\
\mathbf{F}_{\delta \dotup{v},\psi}^e & \mathbf{F}_{\delta \dotup{v},\delta v}^e & \mathbf{F}_{\delta \dotup{v},\delta r}^e & \mathbf{C}_b^e & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -\boldsymbol{\beta}_f & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -\boldsymbol{\beta}_\omega \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\mathbf{F}$} (m-1-5.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[orange!20]{m-1-5}{m-1-5}
\fhighlight[purple!20]{m-2-1}{m-2-1}
\fhighlight[blue!20]{m-2-2}{m-2-2}
\fhighlight[violet!20]{m-2-3}{m-2-3}
\fhighlight[red!20]{m-2-4}{m-2-4}
\fhighlight[cyan!20]{m-3-2}{m-3-2}
\fhighlight[magenta!20]{m-4-4}{m-4-4}
\fhighlight[olive!20]{m-5-5}{m-5-5}
\end{pgfonlayer}
\end{tikzpicture}
\cdot
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=1.8em,align=center},row sep=0.3em, column sep=0.4em] {
\boldsymbol{\delta \psi}_{eb}^e \\
\boldsymbol{\delta v}_{eb}^e \\
\boldsymbol{\delta r}_{eb}^e \\
\boldsymbol{\delta f}_{ib}^b \\
\boldsymbol{\delta \omega}_{ib}^b \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\boldsymbol{\delta} \mathbf{x}$} (m-1-1.north east);
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[blue!20]{m-2-1}{m-2-1}
\fhighlight[cyan!20]{m-3-1}{m-3-1}
\fhighlight[red!20]{m-4-1}{m-4-1}
\fhighlight[orange!20]{m-5-1}{m-5-1}
\end{pgfonlayer}
\end{tikzpicture}
+
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=2em,align=center},row sep=0.3em, column sep=0.4em]
{
\mathbf{C}_b^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{C}_b^e & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \\
};
\draw[decorate,transform canvas={yshift=.5em},thick] (m-1-1.north west) -- node[above=2pt] {$\mathbf{G}$} (m-1-4.north east);
\begin{pgfonlayer}{background}
\fhighlight[darkgray!20]{m-1-1}{m-1-1}
\fhighlight[brown!20]{m-2-2}{m-2-2}
\fhighlight[pink!20]{m-4-3}{m-4-3}
\fhighlight[lightgray!20]{m-5-4}{m-5-4}
\end{pgfonlayer}
\end{tikzpicture}
\cdot
\mathbf{w}
\end{equation}](form_246.svg)
The random noise on the accelerometer and gyro measurements can be estimated as a Random Walk in the angle and velocity errors.
In general, the noise can have an arbitrary characteristic. In a Kalman filter, however, it must be normally distributed and is therefore limited to a Wiener Process. The terms in the noise input matrix are not scaled and therefore need to be compensated with a noise scale matrix 
depending on the amplitude of the noise process. The amplitude is defined by the standard deviation 
. This parameter can be chosen by the user for the accel and gyro measurements:
![\begin{equation} \label{eq:eq-LCKF_e-random-walk-S-noise}
\highlight[darkgray!20]{S_{ra,j}} = \sigma_{ra,j}, \qquad
\highlight[brown!20]{S_{rg,j}} = \sigma_{rg,j}, \qquad\qquad j \in x,y,z
\end{equation}](form_249.svg)
where
is the standard deviation of the noise on the accelerometer specific-force measurements
is the standard deviation of the noise on the gyro angular-rate measurementsThe bias states are also modeled as a random process. In case of random walk, their amplitudes are defined analogously to the above and 
.
Another option to model the bias states is given by the Gauss-Markov 1. Order process:
This introduces a deterministic part into the equation, namely the correlation coefficient 
. This parameter defines, how much a current state is correlated with past states. The elements of the matrix are then
![\begin{equation} \label{eq:eq-LCKF_e-random-walk-beta}
\highlight[magenta!20]{\boldsymbol{\beta}_f} = \begin{pmatrix} \beta_{f} & 0 & 0 \\
0 & \beta_{f} & 0 \\
0 & 0 & \beta_{f} \end{pmatrix}, \qquad
\highlight[olive!20]{\boldsymbol{\beta}_\omega} = \begin{pmatrix} \beta_{\omega} & 0 & 0 \\
0 & \beta_{\omega} & 0 \\
0 & 0 & \beta_{\omega} \end{pmatrix}
\end{equation}](form_255.svg)
where
is the Gauss-Markov constant for the accelerometer 𝛽 = 1 / 𝜏 (𝜏 correlation length)
is the Gauss-Markov constant for the gyroscope 𝛽 = 1 / 𝜏 (𝜏 correlation length)Like before, the scaling is done inside the noise scale matrix. Now, the bias noise's amplitude is given by
![\begin{equation} \label{eq:eq-LCKF_e-random-walk-S-bias_Brown}
\highlight[pink!20]{S_{bad,j}} = \sqrt{\frac{2\sigma_{bad,j}^2}{\tau_{bad}}} , \qquad
\highlight[lightgray!20]{S_{bgd,j}} = \sqrt{\frac{2\sigma_{bgd,j}^2}{\tau_{bgd}}}, \qquad\qquad j \in x,y,z
\end{equation}](form_258.svg)
with a given standard deviation of 
for the accelerometer's bias noise and 
for the gyro's bias noise. The correlation lengths 
and 
define how much a current state is correlated to values in the past. This process is generally limited to normal distributions. A detailed example of this type of system can be found in Brown & Hwang [10] (example 9.6).
![\begin{equation} \label{eq:eq-LCKF_e-random-walk-noise-scale-matrix-W}
\mathbf{W} =
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},
nodes={rectangle,text width=2em,align=center},row sep=0.3em, column sep=0.4em]
{
\mathbf{S}_{rg} & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{S}_{ra} & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{S}_{bad} & \mathbf{0}_3 \\
\mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{S}_{bgd} \\
};
\begin{pgfonlayer}{background}
\fhighlight[darkgray!20]{m-1-1}{m-1-1}
\fhighlight[brown!20]{m-2-2}{m-2-2}
\fhighlight[pink!20]{m-3-3}{m-3-3}
\fhighlight[lightgray!20]{m-4-4}{m-4-4}
\end{pgfonlayer}
\end{tikzpicture}
\end{equation}](form_263.svg)
are diagonal matrices where entries are filled with the PSDs from 
and 
or 
.
In Groves [17], process noise is defined without the need of integration. Instead, noise amplitudes are given directly integrated over one time step 
. Therefore, the PSDs of the accelerometer and gyro noise are:
![\begin{equation} \label{eq:eq-LCKF_e-random-walk-S-noise-Groves}
\highlight[darkgray!20]{S_{ra,j}} = \sigma_{ra,j}^2 \tau_i, \qquad
\highlight[brown!20]{S_{rg,j}} = \sigma_{rg,j}^2 \tau_i, \qquad\qquad j \in x,y,z
\end{equation}](form_269.svg)
where
is the standard deviation of the noise on the accelerometer specific-force measurements is the standard deviation of the noise on the gyro angular-rate measurements
is the interval between the input of successive accelerometer and gyro outputs to the inertial navigation equationsThe bias states are modeled by Groves with the noise PSD's:
![\begin{equation} \label{eq:eq-LCKF_e-random-walk-S-bias}
\highlight[pink!20]{S_{bad,j}} = \frac{\sigma_{bad,j}^2}{\tau_{bad}} , \qquad
\highlight[lightgray!20]{S_{bgd,j}} = \frac{\sigma_{bgd,j}^2}{\tau_{bgd}}, \qquad\qquad j \in x,y,z
\end{equation}](form_271.svg)
where
and 
are the standard deviations of the accelerometer and gyro dynamic biases and are the correlation times of the dynamic accelerometer and gyro biasessee [17] Groves, ch. 14.2.6, eq. 14.84, p. 592
Although correlation lengths 
and are considered, this process is rather a random walk than a Gauss-Markov 1. order process, because of the missing deterministic part in 
.
In INSTINCT, Groves' method is not altered from its reference [17] to avoid confusion.
The state transition matrix is defined as
see [17] Groves, ch. 3.2.3, eq. 3.33, p. 97
The exponential matrix cannot be calculated directly and numerical methods are computationally intensive. Therefore the transition matrix can be computed as a power-series expansion
see [17] Groves, ch. 3.2.3, eq. 3.34, p. 98
The process noise covariance matrix is defined as
In Groves [17], this matrix is approximated as (see ch. 3.2.3, eq. 3.43/3.45, p. 99):
This method resembles a Taylor expansion up to 1. order. Since Groves [17] models the IMU measurements and their biases as random walk processes (see section Groves' process noise definition), only the van-Loan-method can be configured to model biases as Gauss-Markov 1. order processes.
The state transition matrix and the system/process noise covariance matrix can be calculated with the van Loan method [48] . In short
matrix called 
( 
is the dimension of 
and is the power spectral density of the noise 
)
![\begin{equation} \label{eq:eq-LCKF_e-Loan-B}
\mathbf{B} = \text{expm}(\mathbf{A}) = \left[ \begin{array}{c;{2pt/2pt}c}
\dots & \mathbf{\Phi}^{-1} \mathbf{Q} \\[2mm]
\hdashline[2pt/2pt] & \\[-2mm]
\mathbf{0} & \mathbf{\Phi}^T \end{array} \right]
= \begin{bmatrix} \mathbf{B}_{11} & \mathbf{B}_{12} \\
\mathbf{B}_{21} & \mathbf{B}_{22} \end{bmatrix}
\end{equation}](form_298.svg)
as
as
The error covariance matrix is initialized as a diagonal matrix with the variance of the initial state as diagonal elements.
![\begin{equation} \label{eq:eq-LCKF_e-P}
\mathbf{P}_0 =
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},row sep=0.3em, column sep=0.4em,
nodes={rectangle,text width=2.5em,align=center,minimum height=2.3em,anchor=center},] {
\boldsymbol{\sigma}_{\boldsymbol{\delta \psi}}^2 & & \dots & & \mathbf{0}_3 \\
& \boldsymbol{\sigma}_{\boldsymbol{\delta v^e}}^2 & & \iddots & \\
\vdots & & \boldsymbol{\sigma}_{\boldsymbol{\delta r^e}}^2 & & \vdots \\
& \iddots & & \boldsymbol{\sigma}_{\boldsymbol{\delta f^b}}^2 & \\
\mathbf{0}_3 & & \dots & & \boldsymbol{\sigma}_{\boldsymbol{\delta \omega^b_{ib}}}^2 \\
};
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[blue!20]{m-2-2}{m-2-2}
\fhighlight[cyan!20]{m-3-3}{m-3-3}
\fhighlight[red!20]{m-4-4}{m-4-4}
\fhighlight[orange!20]{m-5-5}{m-5-5}
\end{pgfonlayer}
\end{tikzpicture}
\end{equation}](form_309.svg)
where
denotes GNSS indicated measurements
is the lever arm from the INS to the GNSS antenna in [m]This form corresponds to
see
![\begin{equation} \label{eq:eq-LCKF_e-H}
\mathbf{H}^e =
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},row sep=0.3em, column sep=0.4em,
nodes={rectangle,text width=2.5em,align=center,minimum height=2.3em,anchor=center},] {
\mathbf{H}_{r,\delta \psi}^e & \mathbf{0}_3 & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\
\mathbf{H}_{v,\delta \psi}^e & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{H}_{v,\omega_{ib}^b}^e \\
};
\begin{pgfonlayer}{background}
\fhighlight[green!20]{m-1-1}{m-1-1}
\fhighlight[green!20]{m-2-1}{m-2-1}
\fhighlight[blue!20]{m-2-2}{m-2-2}
\fhighlight[cyan!20]{m-1-3}{m-1-3}
\fhighlight[orange!20]{m-2-5}{m-2-5}
\end{pgfonlayer}
\end{tikzpicture}
\end{equation}](form_325.svg)
![\begin{equation} \label{eq:eq-LCKF_e-H-sub}
\begin{aligned}
\highlight[green!20]{\mathbf{H}_{r,\delta \psi}^e} &\approx \left[ (\mathbf{C}_b^e \mathbf{l}_{ba}^b) \times \right] \\
\highlight[green!20]{\mathbf{H}_{v,\delta \psi}^e} &\approx \left[ \left\{ \mathbf{C}_b^e (\boldsymbol{\omega}_{ib}^b \times \mathbf{l}_{ba}^b) - \boldsymbol{\Omega}_{ie}^e \mathbf{C}_b^e \mathbf{l}_{ba}^b \right\} \times \right] \\
\highlight[orange!20]{\mathbf{H}_{v,\omega_{ib}^b}^e} &\approx \mathbf{C}_b^e \left[ \mathbf{l}_{ba}^b \times \right] \\
\end{aligned}
\end{equation}](form_327.svg)
See
The measurement noise covariance matrix is a diagonal matrix with the variances of the measurements as diagonal elements.
![\begin{equation} \label{eq:eq-LCKF_e-R}
\mathbf{R} = E \left( (\mathbf{z} - \mathbf{H} \mathbf{x})(\mathbf{z} - \mathbf{H} \mathbf{x})^T \right) =
\begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace]
\matrix (m)[matrix of math nodes,left delimiter={(},right delimiter={)},row sep=0.3em, column sep=0.1em,
nodes={rectangle,text width=2.2em,align=center,minimum height=2.3em,anchor=center},] {
\boldsymbol{\sigma}^2_{\mathbf{z}_{\boldsymbol{\delta r}^e}} & \mathbf{0}_3 \\
\mathbf{0}_3 & \boldsymbol{\sigma}^2_{\mathbf{z}_{\boldsymbol{\delta v}^e}} \\
};
\begin{pgfonlayer}{background}
\fhighlight[cyan!20]{m-1-1}{m-1-1}
\fhighlight[blue!20]{m-2-2}{m-2-2}
\end{pgfonlayer}
\end{tikzpicture}
\end{equation}](form_331.svg)
This variances can be set static or ideally come from the GNSS receiver.
This section describes how the errors are applied to the total state.
The position and velocity errors get applied by simply subtracting them from the previous total state (see [17] Groves, ch. 14.1.1, eq. 14.8, 14.9, p. 564)
The attitude correction can be derived from equation 
(see [17] Groves, ch. 14.1.1, eq. 14.7, p. 564)
![\begin{equation} \label{eq:eq-LCKF_e-correction-attitude}
\mathbf{C}_b^{e,+} = (\mathbf{I}_3 - [\boldsymbol{\delta \psi} \times]) \mathbf{C}_b^{e,-}
\end{equation}](form_338.svg)
After applying the errors to the total state, the errors have to be reset to zero (closed-loop).
To correct the accelerometer and gyroscope measurements, we need to accumulate the error states. This has to be done, because contrary to the normal state, the expected value of the biases is not zero.
Afterwards this accumulated value can be applied to the measurements by subtracting it
Afterwards the bias in the error state gets reset to zero, as it is accounted for in the accumulated value.
The time derivative of the position is simply a function of the Earth-referenced velocity in Earth frame axes
see
The time derivative of the position error thus is
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Position-differential}
\boldsymbol{\delta}\mathbf{\dotup{r}}_{eb}^e = \highlight[cyan!20]{\mathbf{I}_3} \cdot \mathbf{v}_{eb}^e
\end{equation}](form_349.svg)
see
The estimated attitude 
can be written in terms of true direction cosine matrix 
where 
is the transformation from true reference axes to estimated reference axes and it can be approximated for small angle misalignments as
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Attitude-transformation}
\mathbf{B} = [\mathbf{I} - \mathbf{\Psi}], \quad \text{with}~ \mathbf{\Psi} = \begin{pmatrix} \delta\alpha \\ \delta\beta \\ \delta\gamma \end{pmatrix} \times
\end{equation}](form_356.svg)
with 
being a skew symmetric matrix and 
being the attitude errors.
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Attitude-estimate-errors}
\mathbf{\hat{C}}_b^e \approx [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e
\end{equation}](form_363.svg)
Differentiating this equation yields:
Using the following equations for the transformation
[17] Groves, ch. 2.3.1, eq. 2.48, p. 45
[17] Groves, ch. 2.3.1, eq. 2.51, p. 45the time derivative of the DCM matrix is
see
where
is the skew-symmetric matrix of the absolute body rate,
is the skew-symmetric matrix of the Earth rotation in Earth frame coordinatesSimilarly, the time time differential of the estimated matrix 
can be calculated:
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Attitude-skew-mat-dot-calc1}
\begin{aligned}
\mathbf{\dotup{\Psi}} &= -(\mathbf{\hat{C}}_b^e \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\hat{C}}_b^e) {\mathbf{C}_b^e}^T
- \mathbf{\hat{C}}_b^e (\mathbf{C}_b^e \mathbf{\Omega}_{ib}^b - \mathbf{\Omega}_{ie}^e \mathbf{C}_b^e)^T \\
&= - \mathbf{\hat{C}}_b^e \mathbf{\hat{\Omega}}_{ib}^b {\mathbf{C}_b^e}^T
+ \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\hat{C}}_b^e {\mathbf{C}_b^e}^T
+ \mathbf{\hat{C}}_b^e \mathbf{\Omega}_{ib}^b {\mathbf{C}_b^e}^T
- \mathbf{\hat{C}}_b^e {\mathbf{C}_b^e}^T \mathbf{\Omega}_{ie}^e \\
&= - \mathbf{\hat{C}}_b^e \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^e}^T
+ \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\hat{C}}_b^e {\mathbf{C}_b^e}^T
- \mathbf{\hat{C}}_b^e {\mathbf{C}_b^e}^T \mathbf{\Omega}_{ie}^e
\end{aligned}
\end{equation}](form_390.svg)
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Attitude-skew-mat-dot-calc2}
\begin{aligned}
\mathbf{\dotup{\Psi}} &= - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^e}^T
+ \mathbf{\hat{\Omega}}_{ie}^e [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e {\mathbf{C}_b^e}^T
- [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e {\mathbf{C}_b^e}^T \mathbf{\Omega}_{ie}^e \\
&= - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^e}^T
+ \mathbf{\hat{\Omega}}_{ie}^e - \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\Psi}
- \mathbf{\Omega}_{ie}^e + \mathbf{\Psi} \mathbf{\Omega}_{ie}^e \\
&= - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^e}^T
+ \left[ \mathbf{\hat{\Omega}}_{ie}^e - \mathbf{\Omega}_{ie}^e \right]
- \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\Psi} + \mathbf{\Psi} \mathbf{\Omega}_{ie}^e
\end{aligned}
\end{equation}](form_394.svg)
Now we make the following substitutions
and neglect error product terms. This leads to
Finally by performing an element by element comparison with
the equation can be expressed in vector form as:
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Attitude-differential}
\begin{aligned}
\boldsymbol{\dotup{\psi}} &\approx \highlight[green!20]{-\boldsymbol{\omega}_{ie}^e \times} \boldsymbol{\psi}
+ \overbrace{\boldsymbol{\delta \omega}_{ie}^e}^{= 0}
\highlight[orange!20]{- \mathbf{C}_b^e} \boldsymbol{\delta \omega}_{ib}^b \\
&= \highlight[green!20]{\begin{pmatrix} 0 & \omega_{ie} & 0 \\ -\omega_{ie} & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix}} \cdot \boldsymbol{\psi}
\highlight[orange!20]{- \mathbf{C}_b^e} \boldsymbol{\delta \omega}_{ib}^b \\
\end{aligned}
\end{equation}](form_403.svg)
The time derivative of the velocity in Earth frame coordinates is
see
where
is the velocity with respect to the Earth in Earth frame coordinates,
is the specific force vector as measured by a triad of accelerometers and resolved into Earth frame coordinates
is the turn rate of the Earth expressed in Earth frame coordinates
is the local gravity vector which is a combination of (see [47] Titterton, ch. 3.5.1, eq. 3.14, p. 27)
the gravitation vector (caused by effects of mass attraction)![$ -\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{r}^e ] $](form_414.svg)
the centrifugal acceleration caused by the Earth's rotationThe estimated velocity follows the same propagation
Differencing equation 
and 
we get
: ![$ \mathbf{\hat{C}}_b^e \approx [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e $](form_426.svg)
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Velocity-timeDerivative-differential-transformation}
\begin{aligned}
\boldsymbol{\delta \dotup{v}}^e &= [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e \boldsymbol{\hat{f}}^b
- \mathbf{C}_b^e \boldsymbol{f}^b
- 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{\hat{v}}^e
+ 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e
+ \boldsymbol{\delta} \mathbf{g}^e \\
&= \mathbf{C}_b^e \boldsymbol{\hat{f}}^b - \mathbf{\Psi} \mathbf{C}_b^e \boldsymbol{\hat{f}}^b
- \mathbf{C}_b^e \boldsymbol{f}^b
- 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{\hat{v}}^e
+ 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e
+ \boldsymbol{\delta} \mathbf{g}^e \\
&= - \boldsymbol{\psi} \times \mathbf{C}_b^e \boldsymbol{\hat{f}}^b
+ \highlight[red!20]{\mathbf{C}_b^e} \boldsymbol{\delta f}^b
- 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{\delta v}^e
+ \boldsymbol{\delta} \mathbf{g}^e \\
&= (\mathbf{C}_b^e \boldsymbol{\delta f}^b + \mathbf{C}_b^e \boldsymbol{f}^b) \times \boldsymbol{\psi}
+ \highlight[red!20]{\mathbf{C}_b^e} \boldsymbol{\delta f}^b
- 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{\delta v}^e
+ \boldsymbol{\delta} \mathbf{g}^e \\
\end{aligned}
\end{equation}](form_435.svg)
Now neglecting error product terms ( 
)
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Velocity-timeDerivative-differential-transformation-neglect}
\renewcommand*{\arraystretch}{1.9}
\boldsymbol{\delta \dotup{v}}^e \approx \highlight[purple!20]{\boldsymbol{f}^e \times} \boldsymbol{\psi}
+ \highlight[red!20]{\mathbf{C}_b^e} \boldsymbol{\delta f}^b
\highlight[blue!20]{- 2 \boldsymbol{\omega}_{ie}^e \times} \boldsymbol{\delta v}^e
+ \boldsymbol{\delta} \mathbf{g}^e
\end{equation}](form_438.svg)
The gravity error can be expressed as (see [17] Groves, ch. 14.2.3, eq. 14.45, p. 583)
where
is made
![\begin{equation} \label{eq:eq-LCKF_e-Derivation-Velocity-timeDerivative-final}
\renewcommand*{\arraystretch}{1.9}
\boldsymbol{\delta \dotup{v}}^e \approx \highlight[purple!20]{\boldsymbol{f}^e \times} \boldsymbol{\psi}
+ \highlight[red!20]{\mathbf{C}_b^e} \boldsymbol{\delta f}^b
+ \highlight[blue!20]{\begin{pmatrix} 0 & 2 \omega_{ie} & \\ -2 \omega_{ie} & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix}} \boldsymbol{\delta v}^e
\highlight[violet!20]{- \left( \dfrac{2 \boldsymbol{\gamma}_{ib}^e}{r_{eS}^e} \dfrac{{\mathbf{r}_{eb}^e}^T}{\left| \mathbf{r}_{eb}^e \right|}
+ \boldsymbol{\Omega}_{ie}^e \boldsymbol{\Omega}_{ie}^e \right)} \boldsymbol{\delta} \mathbf{r}_{eb}^e
\end{equation}](form_442.svg)
1.13.2 
