 |
0.2.0
|
|
0.2.0
|
\begin{equation} \begin{bmatrix} \boldsymbol{\delta \dot{\psi}} \\ \boldsymbol{\delta \dot{v}}^{n} \\ \boldsymbol{\delta \dot{p}}_b \\ \boldsymbol{\delta \dot{f}}^{\raise-0.45ex\hbox{$\scriptstyle b$}} \\ \boldsymbol{\delta \dot{\omega}}_{ib}^b \\ \end{bmatrix} = \begin{bmatrix} \mathbf{F}_{\delta \dot{\psi},\delta \psi}^n & \mathbf{F}_{\delta \dot{\psi},\delta v}^n & \mathbf{F}_{\delta \dot{\psi},\delta r}^n & \mathbf{0}_3 & \mathbf{C}_b^n \\ \mathbf{F}_{\delta \dot{v},\delta \psi}^n & \mathbf{F}_{\delta \dot{v},\delta v}^n & \mathbf{F}_{\delta \dot{v},\delta r}^n & \mathbf{C}_b^n & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{F}_{\delta \dot{r},\delta v}^n & \mathbf{F}_{\delta \dot{r},\delta r}^n & \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 \\ \end{bmatrix} \cdot \begin{bmatrix} \boldsymbol{\delta \psi} \\ \boldsymbol{\delta v}^{n} \\ \boldsymbol{\delta p}_b \\ \boldsymbol{\delta f}^b \\ \boldsymbol{\delta \omega}_{ib}^b \\ \end{bmatrix} \end{equation}
\begin{equation} \begin{bmatrix} \delta \dot{R} \\ \delta \dot{P} \\ \delta \dot{Y} \\ \hdashline \delta \dot{v}_{N} \\ \delta \dot{v}_{E} \\ \delta \dot{v}_{D} \\ \hdashline \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta \dot{h} \\ \hdashline \delta \dot{f}_{x}^{b} \\ \delta \dot{f}_{y}^{b} \\ \delta \dot{f}_{z}^{b} \\ \hdashline \delta \dot{\omega}_{x}^{b} \\ \delta \dot{\omega}_{y}^{b} \\ \delta \dot{\omega}_{z}^{b} \\ \end{bmatrix} = \left[ \begin{array}{ccc:ccc:ccc:ccc:ccc} 0 & \omega_{in,3}^{n} & -\omega_{in,2}^{n} & 0 & -\frac{1}{R_E + h} & 0 & \omega_{ie}\sin{\phi} & 0 & \frac{v_E}{(R_E + h)^2} & & & & & & \\ -\omega_{in,3}^{n} & 0 & \omega_{in,1}^{n} & \frac{1}{R_N + h} & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} & & \mathbf{0}_3 & & & \mathbf{C}_b^n & \\ \omega_{in,2}^{n} & -\omega_{in,1}^{n} & 0 & 0 & \frac{\tan{\phi}}{R_E + h} & 0 & \omega_{ie}\cos{\phi} + \frac{v_E}{(R_E + h)\cos^2{\phi}} & 0 & -\frac{v_E \tan{\phi}}{(R_E + h)^2} & & & & & & \\ \hdashline 0 & f_D & -f_E & \frac{v_D}{R_N + h} & -2\frac{v_E \tan{\phi}}{R_E + h} - 2\omega_{ie}\sin{\phi} & \frac{v_N}{R_N + h} & -\frac{v_E^2}{(R_E+h)\cos^2{\phi}}-2v_E\omega_{ie}\cos{\phi} & 0 & \frac{v_E^2\tan{\phi}}{(R_E+h)^2} - \frac{v_Nv_D}{(R_N+h)^2} & & & & & & \\ -f_D & 0 & f_N & \frac{v_E \tan{\phi}}{R_E + h} + 2\omega_{ie}\sin{\phi} & \frac{v_N \tan{\phi} + v_D}{R_E + h} & \frac{v_E}{R_E + h} + 2\omega_{ie}\cos{\phi} & \frac{v_N v_E}{(R_E+h)\cos^2{\phi}} + 2v_N\omega_{ie}\cos{\phi} - 2v_D\omega_{ie}\sin{\phi} & 0 & -\frac{v_Nv_E\tan{\phi} + v_Ev_D}{(R_E + h)^2} & & \mathbf{C}_b^n & & & \mathbf{0}_3 & \\ f_E & -f_N & 0 & -\frac{2v_N}{R_N + h} & -\frac{2v_E}{R_E + h} - 2\omega_{ie}\cos{\phi} & 0 & 2v_E\omega_{ie}\sin{\phi} & 0 & \frac{v_E^2}{(R_E + h)^2} + \frac{v_N^2}{(R_N + h)^2} - \frac{2 g_0}{r_{eS}^e} & & & & & & \\ \hdashline & & & \frac{1}{R_N+h} & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} & & & & & & \\ & \mathbf{0}_3 & & 0 & \frac{1}{(R_E+h)\cos{\phi}} & 0 & \frac{v_E\tan{\phi}}{(R_E+h)\cos{\phi}} & 0 & -\frac{v_E}{(R_E + h)^2\cos{\phi}} & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & 0 & 0 & -1 & 0 & 0 & 0 & & & & & & \\ \hdashline & & & & & & & & & & & & & & \\ & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & & & & & & & & & & & & \\ \hdashline & & & & & & & & & & & & & & \\ & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & & & & & & & & & & & & \\ \end{array}\right] \cdot \begin{bmatrix} \delta R \\ \delta P \\ \delta Y \\ \hdashline \delta v_{N} \\ \delta v_{E} \\ \delta v_{D} \\ \hdashline \delta \phi \\ \delta \lambda \\ \delta h \\ \hdashline \delta f_{x}^{b} \\ \delta f_{y}^{b} \\ \delta f_{z}^{b} \\ \hdashline \delta \omega_{x}^{b} \\ \delta \omega_{y}^{b} \\ \delta \omega_{z}^{b} \\ \end{bmatrix} \end{equation}
This form corresponds to
The following sources can be taken as reference, but have at least one deviation from the form presented here
However in section Error Equations comparison it is shown, that they can be transformed into each other.
The continuous linear dynamic system can be expressed as
\begin{equation} \boldsymbol{\delta} \mathbf{\dot{x}} = \mathbf{F} \cdot \boldsymbol{\delta} \mathbf{x} + \mathbf{G} \cdot \mathbf{w} \end{equation}
Process noise is introduced into the \( \mathbf{F} \) and \( \mathbf{G} \) matrices in the accelerometer and gyroscope bias terms. Therefore the error equations can be extended as follows
\begin{equation} \begin{bmatrix} \boldsymbol{\delta \dot{\psi}} \\ \boldsymbol{\delta \dot{v}}^{n} \\ \boldsymbol{\delta \dot{p}}_b \\ \boldsymbol{\delta \dot{f}}^{\raise-0.45ex\hbox{$\scriptstyle b$}} \\ \boldsymbol{\delta \dot{\omega}}_{ib}^b \\ \end{bmatrix} = \begin{bmatrix} \mathbf{F}_{\delta \dot{\psi},\delta \psi}^n & \mathbf{F}_{\delta \dot{\psi},\delta v}^n & \mathbf{F}_{\delta \dot{\psi},\delta r}^n & \mathbf{0}_3 & \mathbf{C}_b^n \\ \mathbf{F}_{\delta \dot{v},\delta \psi}^n & \mathbf{F}_{\delta \dot{v},\delta v}^n & \mathbf{F}_{\delta \dot{v},\delta r}^n & \mathbf{C}_b^n & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{F}_{\delta \dot{r},\delta v}^n & \mathbf{F}_{\delta \dot{r},\delta r}^n & \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 \\ \end{bmatrix} \cdot \begin{bmatrix} \boldsymbol{\delta \psi} \\ \boldsymbol{\delta v}^{n} \\ \boldsymbol{\delta p}_b \\ \boldsymbol{\delta f}^b \\ \boldsymbol{\delta \omega}_{ib}^b \ \end{bmatrix} + \begin{bmatrix} \mathbf{C}_b^n & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n & \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 \\ \end{bmatrix} \cdot \mathbf{w} \end{equation}
The random noise on the accelerometer and gyro measurements can be estimated as a Random Walk in the angle and velocity errors.
\begin{equation} \dot{x} = 0 + W(t) \end{equation}
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 \( \mathbf{G} \) are not scaled and therefore need to be compensated with a noise scale matrix \( \mathbf{W} \) depending on the amplitude of the noise process. The amplitude is defined by the standard deviation \(\sigma\). This parameter can be chosen by the user for the accel and gyro measurements:
\begin{equation} S_{ra,j} = \sigma_{ra,j}, \qquad S_{rg,j} = \sigma_{rg,j}, \qquad\qquad j \in x,y,z \end{equation}
where
The bias states are also modeled as a random process. In case of random walk, their amplitudes are defined analogously to the above and \(\beta = 0\).
Another option to model the bias states is given by the Gauss-Markov 1. Order process:
\begin{equation} \dot{x} = -\beta x + W(t) \end{equation}
This introduces a deterministic part into the equation, namely the correlation coefficient \(\beta\). This parameter defines, how much a current state is correlated with past states. The elements of the \( \mathbf{F} \) matrix are then
\begin{equation} \boldsymbol{\beta}_f = \begin{pmatrix} \beta_{f} & 0 & 0 \\ 0 & \beta_{f} & 0 \\ 0 & 0 & \beta_{f} \end{pmatrix}, \qquad \boldsymbol{\beta}_\omega = \begin{pmatrix} \beta_{\omega} & 0 & 0 \\ 0 & \beta_{\omega} & 0 \\ 0 & 0 & \beta_{\omega} \end{pmatrix} \end{equation}
where
Like before, the scaling is done inside the noise scale matrix. Now, the bias noise's amplitude is given by
\begin{equation} S_{bad,j} = \sqrt{\frac{2\sigma_{bad,j}^2}{\tau_{bad}}} , \qquad S_{bgd,j} = \sqrt{\frac{2\sigma_{bgd,j}^2}{\tau_{bgd}}}, \qquad\qquad j \in x,y,z \end{equation}
with a given standard deviation of \(\sigma_{bad}\) for the accelerometer's bias noise and \(\sigma_{bgd}\) for the gyro's bias noise. The correlation lengths \( \tau_{bad} \) and \( \tau_{bgd} \) 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} \mathbf{W} = \begin{bmatrix} \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} \\ \end{bmatrix} \end{equation}
\( \mathbf{S}_{rg}, \mathbf{S}_{ra}, \mathbf{S}_{bad}, \mathbf{S}_{bgd} \) are diagonal matrices where entries are filled with the PSDs from eq-LCKF_n-random-walk-S-noise and eq-LCKF_n-random-walk-S-bias or eq-LCKF_n-random-walk-S-bias_Brown.
In Groves [17], process noise is defined without the need of integration. Instead, noise amplitudes are given directly integrated over one time step \(\tau_i\). Therefore, the PSDs of the accelerometer and gyro noise are:
\begin{equation} S_{ra,j} = \sigma_{ra,j}^2 \tau_i, \qquad S_{rg,j} = \sigma_{rg,j}^2 \tau_i, \qquad\qquad j \in x,y,z \end{equation}
where
The bias states are modeled by Groves with the noise PSD's:
\begin{equation} S_{bad,j} = \frac{\sigma_{bad,j}^2}{\tau_{bad}} , \qquad S_{bgd,j} = \frac{\sigma_{bgd,j}^2}{\tau_{bgd}}, \qquad\qquad j \in x,y,z \end{equation}
where
see [17] Groves, ch. 14.2.6, eq. 14.84, p. 592
Although correlation lengths \(\tau_{bad} \) and \( \tau_{bgd} \) are considered, this process is rather a random walk than a Gauss-Markov 1. order process, because of the missing deterministic part in \(\mathbf{F}\).
In INSTINCT, Groves' method eq-LCKF_n-random-walk-S-bias is not altered from its reference [17] to avoid confusion.
The state transition matrix is defined as
\begin{equation} \boldsymbol{\Phi} = \exp{(\mathbf{F} \tau_s)} \end{equation}
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
\begin{equation} \boldsymbol{\Phi} = \sum_{r=0}^{\infty} \frac{\mathbf{F}^r \tau_s^r}{r!} = \mathbf{I} + \mathbf{F} \tau_s + \frac{1}{2} \mathbf{F}^2 \tau^2 + \frac{1}{6} \mathbf{F}^3 \tau_s^3 + \dots \end{equation}
see [17] Groves, ch. 3.2.3, eq. 3.34, p. 98
The process noise covariance matrix is defined as
\begin{equation} \mathbf{Q}_{k-1} = \int_{t_k - \tau_s}^{t_k} \exp{(\mathbf{F}_{k-1} (t_k-t'))} \mathbf{G}_{k-1} \mathbf{W}_{k-1} \mathbf{G}_{k-1}^T \exp{(\mathbf{F}_{k-1}^T (t_k-t'))} dt' \end{equation}
In Groves [17], this matrix is approximated as (see ch. 3.2.3, eq. 3.43/3.45, p. 99):
\begin{equation} \mathbf{Q}_{k-1} \approx \mathbf{G}_{k-1} \mathbf{W}_{k-1} \mathbf{G}_{k-1}^T \tau_s \end{equation}
Taking the noise input and scale matrices from eq-LCKF_n-dynamicModel-short and eq-LCKF_n-random-walk-noise-scale-matrix-W and putting it into the exact form eq-LCKF_n-Q-exact, it becomes
\begin{equation} \begin{aligned} \mathbf{Q}_{k-1} &= \int_{t_k - \tau_s}^{t_k} \exp{(\mathbf{F}_{k-1} (t_k-t'))} \begin{bmatrix} \mathbf{C}_b^n \cdot \mathbf{S}_{rg} & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n \cdot \mathbf{S}_{ra} & \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 \cdot \mathbf{S}_{bad} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \cdot \mathbf{S}_{bgd} \\ \end{bmatrix} \begin{bmatrix} \mathbf{C}_n^b & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_n^b & \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{0}_3 & \mathbf{I}_3 \\ \end{bmatrix} \exp{(\mathbf{F}_{k-1}^T (t_k-t'))} dt' \\ &= \int_{t_k - \tau_s}^{t_k} \exp{(\mathbf{F}_{k-1} (t_k-t'))} \begin{bmatrix} \mathbf{C}_b^n \mathbf{S}_{rg} \mathbf{C}_n^b & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n \mathbf{S}_{ra} \mathbf{C}_n^b & \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{S}_{bad} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{S}_{bgd} \\ \end{bmatrix} \exp{(\mathbf{F}_{k-1}^T (t_k-t'))} dt' \end{aligned} \end{equation}
The exponential \( \mathbf{F} \) matrix shall be approximated by first order. This leads to:
\begin{equation} \begin{aligned} \mathbf{Q}_{k-1} &\approx \int_{t_k - \tau_s}^{t_k} \begin{bmatrix} \mathbf{I}_3 + \mathbf{F}_{\delta \dot{\psi},\delta \psi}^n \tau_s & \mathbf{F}_{\delta \dot{\psi},\delta v}^n \tau_s & \mathbf{F}_{\delta \dot{\psi},\delta r}^n \tau_s & \mathbf{0}_3 & \mathbf{C}_b^n \tau_s \\ \mathbf{F}_{\delta \dot{v},\delta \psi}^n \tau_s & \mathbf{I}_3 + \mathbf{F}_{\delta \dot{v},\delta v}^n \tau_s & \mathbf{F}_{\delta \dot{v},\delta r}^n \tau_s & \mathbf{C}_b^n \tau_s & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{F}_{\delta \dot{r},\delta v}^n \tau_s & \mathbf{I}_3 + \mathbf{F}_{\delta \dot{r},\delta r}^n \tau_s & \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{0}_3 & \mathbf{I}_3 \\ \end{bmatrix} \begin{bmatrix} \mathbf{C}_b^n \mathbf{S}_{rg} \mathbf{C}_n^b & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n \mathbf{S}_{ra} \mathbf{C}_n^b & \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{S}_{bad} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{S}_{bgd} \\ \end{bmatrix} \begin{bmatrix} \mathbf{I}_3 + \mathbf{F}_{\delta \dot{\psi},\delta \psi}^n \tau_s & \mathbf{F}_{\delta \dot{v},\delta \psi}^n \tau_s & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{F}_{\delta \dot{\psi},\delta v}^n \tau_s & \mathbf{I}_3 + \mathbf{F}_{\delta \dot{v},\delta v}^n \tau_s & \mathbf{F}_{\delta \dot{r},\delta v}^n \tau_s & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{F}_{\delta \dot{\psi},\delta r}^n \tau_s & \mathbf{F}_{\delta \dot{v},\delta r}^n \tau_s & \mathbf{I}_3 + \mathbf{F}_{\delta \dot{r},\delta r}^n \tau_s & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n \tau_s & \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{C}_b^n \tau_s & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3 \\ \end{bmatrix} dt' \end{aligned} \end{equation}
The final equation is
\begin{equation} \mathbf{Q} = \begin{bmatrix} (\mathbf{S}_{rg}\tau_s + \frac{1}{3} \mathbf{S}_{bgd} \tau_s^3) \mathbf{I} & (\frac{1}{2} \mathbf{S}_{rg} \tau_s^2 + \frac{1}{4} \mathbf{S}_{bgd} \tau_s^4) \mathbf{F}_{\delta \dot{v},\delta \psi}^n & \\ \end{bmatrix} \end{equation}
where
see [17] Groves, ch. 14.2.6, eq. 14.80/14.81, p. 591
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 [46] . In short
\begin{equation} \mathbf{A} = \begin{bmatrix} -\mathbf{F} & \mathbf{G} \mathbf{W} \mathbf{G}^T \\ \mathbf{0} & \mathbf{F}^T \end{bmatrix} \Delta t \end{equation}
\begin{equation} \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}
\begin{equation} \mathbf{\Phi} = \mathbf{B}_{22}^T \end{equation}
\begin{equation} \mathbf{Q} = \mathbf{\Phi} \mathbf{B}_{12} \end{equation}
The error covariance matrix is initialized as a diagonal matrix with the variance of the initial state as diagonal elements.
\begin{equation} \mathbf{P}_0 = \begin{bmatrix} \boldsymbol{\sigma}_{\boldsymbol{\delta \psi}}^2 & & \dots & & \mathbf{0}_3 \\ & \boldsymbol{\sigma}_{\boldsymbol{\delta v^n}}^2 & & & \\ \vdots & & \boldsymbol{\sigma}_{\boldsymbol{\delta p_b}}^2 & & \vdots \\ & & & \boldsymbol{\sigma}_{\boldsymbol{\delta f^b}}^2 & \\ \mathbf{0}_3 & & \dots & & \boldsymbol{\sigma}_{\boldsymbol{\delta \omega^b_{ib}}}^2 \\ \end{bmatrix} \end{equation}
\begin{equation} \boldsymbol{\delta} \mathbf{z}^n = \begin{pmatrix} \tilde{\mathbf{p}}_{b,G} - \hat{\mathbf{p}}_{b} - \mathbf{T}_{r(n)}^{p} \mathbf{C}_b^n \mathbf{l}_{ba}^b \\ \tilde{\mathbf{v}}^n_{G} - \hat{\mathbf{v}}^n - \mathbf{C}_b^n (\boldsymbol{\omega}_{ib}^b \times \mathbf{l}_{ba}^b) + \boldsymbol{\Omega}_{ie}^n \mathbf{C}_b^n \mathbf{l}_{ba}^b \end{pmatrix} \end{equation}
where
This form corresponds to
\begin{equation} \mathbf{H}_k = \left. \frac{\partial \mathbf{h} (\mathbf{x}, t_k)}{\partial \mathbf{x}} \right|_{x = \hat{x}_k^{-}} = \left. \frac{\partial \mathbf{z} (\mathbf{x}, t_k)}{\partial \mathbf{x}} \right|_{x = \hat{x}_k^{-}} \end{equation}
see
\begin{equation} \mathbf{H}^n = \begin{bmatrix} \mathbf{H}_{r,\delta \psi}^n & \mathbf{0}_3 & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{H}_{v,\delta \psi}^n & -\mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{H}_{\delta v,\omega_{ib}^b}^n \\ \end{bmatrix} \end{equation}
\begin{equation} \begin{aligned} \mathbf{H}_{r,\delta \psi}^n &\approx \mathbf{T}_{r(n)}^{p} \left[ (\mathbf{C}_b^n \mathbf{l}_{ba}^b) \times \right] \\ \mathbf{H}_{v,\delta \psi}^n &\approx \left[ \left\{ \mathbf{C}_b^n (\boldsymbol{\omega}_{ib}^b \times \mathbf{l}_{ba}^b) - \boldsymbol{\Omega}_{ie}^n \mathbf{C}_b^n \mathbf{l}_{ba}^b \right\} \times \right] \\ \mathbf{H}_{v,\omega_{ib}^b}^n &\approx \mathbf{C}_b^n \left[ \mathbf{l}_{ba}^b \times \right] \\ \end{aligned} \end{equation}
See
The measurement noise covariance matrix is a diagonal matrix with the variances of the measurements as diagonal elements.
\begin{equation} \mathbf{R} = E \left( (\mathbf{z} - \mathbf{H} \mathbf{x})(\mathbf{z} - \mathbf{H} \mathbf{x})^T \right) = \begin{bmatrix} \sigma^2_{z_\phi} & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma^2_{z_\lambda} & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma^2_{z_h} & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma^2_{z_{v_N}} & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma^2_{z_{v_E}} & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma^2_{z_{v_D}} \\ \end{bmatrix} \end{equation}
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.10, p. 564)
\begin{equation} \begin{aligned} \boldsymbol{p}_b^{+} &= \boldsymbol{p}_b^{-} - \boldsymbol{\delta p}_b \\ \boldsymbol{v}^{n,+} &= \boldsymbol{v}^{n,-} - \boldsymbol{\delta v}^{n} \end{aligned} \end{equation}
The attitude correction can be derived from equation eq-LCKF_n-Derivation-Attitude-estimate-errors (see [17] Groves, ch. 14.1.1, eq. 14.7, p. 564)
\begin{equation} \mathbf{C}_b^{n,+} = (\mathbf{I}_3 - [\boldsymbol{\delta \psi} \times]) \mathbf{C}_b^{n,-} \end{equation}
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.
\begin{equation} \begin{aligned} \boldsymbol{\Delta f}^{b,+} &= \boldsymbol{\Delta f}^{b,-} + \boldsymbol{\delta f}^b \\ \boldsymbol{\Delta \omega}_{ib}^{b,+} &= \boldsymbol{\Delta \omega}_{ib}^{b,-} + \boldsymbol{\delta \omega}_{ib}^b \end{aligned} \end{equation}
Afterwards this accumulated value can be applied to the measurements by subtracting it
\begin{equation} \begin{aligned} \boldsymbol{f}^{p,+} &= \boldsymbol{f}^{p,-} - \mathbf{C}_b^p \boldsymbol{\Delta f}^{b,+} \\ \boldsymbol{\omega}_{ib}^{p,+} &= \boldsymbol{\omega}_{ib}^{p,-} - \mathbf{C}_b^p \boldsymbol{\Delta \omega}_{ib}^{b,+} \end{aligned} \end{equation}
Afterwards the bias in the error state gets reset to zero, as it is accounted for in the accumulated value.
The units of the state vector \( \mathbf{x} \) and the \( \mathbf{F} \) matrix are as follows
\begin{equation} \begin{bmatrix} \delta \dot{R} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \dot{P} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \dot{Y} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \dot{v}_{N} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \dot{v}_{E} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \dot{v}_{D} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \dot{\phi} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \dot{\lambda} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \dot{h} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta \dot{f}_{x}^{b} & \left[ \frac{\text{m}}{\text{s}^3} \right] \\ \delta \dot{f}_{y}^{b} & \left[ \frac{\text{m}}{\text{s}^3} \right] \\ \delta \dot{f}_{z}^{b} & \left[ \frac{\text{m}}{\text{s}^3} \right] \\ \delta \dot{\omega}_{x}^{b} & \left[ \frac{\text{rad}}{\text{s}^2} \right] \\ \delta \dot{\omega}_{y}^{b} & \left[ \frac{\text{rad}}{\text{s}^2} \right] \\ \delta \dot{\omega}_{z}^{b} & \left[ \frac{\text{rad}}{\text{s}^2} \right] \\ \end{bmatrix} = \begin{bmatrix} 0 & \omega_{in,3}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & -\omega_{in,2}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & -\frac{1}{R_E + h} \left[ \frac{1}{\text{m}} \right] & 0 & \omega_{ie}\sin{\phi} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & \frac{v_E}{(R_E + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & & & & & \\ -\omega_{in,3}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & \omega_{in,1}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & \frac{1}{R_N + h} \left[ \frac{1}{\text{m}} \right] & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & \mathbf{0}_3 & & & \mathbf{C}_b^n \left[ - \right] & \\ \omega_{in,2}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & -\omega_{in,1}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & 0 & \frac{\tan{\phi}}{R_E + h} \left[ \frac{1}{\text{m}} \right] & 0 & \omega_{ie}\cos{\phi} + \frac{v_E}{(R_E + h)\cos^2{\phi}} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & -\frac{v_E \tan{\phi}}{(R_E + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & & & & & \\ 0 & f_D \left[ \frac{\text{m}}{\text{s}^2} \right] & -f_E \left[ \frac{\text{m}}{\text{s}^2} \right] & \frac{v_D}{R_N + h} \left[ \frac{1}{\text{s}} \right] & -2\frac{v_E \tan{\phi}}{R_E + h} - 2\omega_{ie}\sin{\phi} \left[ \frac{1}{\text{s}} \right] & \frac{v_N}{R_N + h} \left[ \frac{1}{\text{s}} \right] & -\frac{v_E^2}{(R_E+h)\cos^2{\phi}}-2v_E\omega_{ie}\cos{\phi} \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & \frac{v_E^2\tan{\phi}}{(R_E+h)^2} - \frac{v_Nv_D}{(R_N+h)^2} \left[ \frac{1}{\text{s}^2} \right] & & & & & & \\ -f_D \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & f_N \left[ \frac{\text{m}}{\text{s}^2} \right] & \frac{v_E \tan{\phi}}{R_E + h} + 2\omega_{ie}\sin{\phi} \left[ \frac{1}{\text{s}} \right] & \frac{v_N \tan{\phi} + v_D}{R_E + h} \left[ \frac{1}{\text{s}} \right] & \frac{v_E}{R_E + h} + 2\omega_{ie}\cos{\phi} \left[ \frac{1}{\text{s}} \right] & \frac{v_N v_E}{(R_E+h)\cos^2{\phi}} + 2v_N\omega_{ie}\cos{\phi} - 2v_D\omega_{ie}\sin{\phi} \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & -\frac{v_Nv_E\tan{\phi} + v_Ev_D}{(R_E + h)^2} \left[ \frac{1}{\text{s}^2} \right] & & \mathbf{C}_b^n \left[ - \right] & & & \mathbf{0}_3 & \\ f_E \left[ \frac{\text{m}}{\text{s}^2} \right] & -f_N \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & -\frac{2v_N}{R_N + h} \left[ \frac{1}{\text{s}} \right] & -\frac{2v_E}{R_E + h} - 2\omega_{ie}\cos{\phi} \left[ \frac{1}{\text{s}} \right] & 0 & 2v_E\omega_{ie}\sin{\phi} \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & \frac{v_E^2}{(R_E + h)^2} + \frac{v_N^2}{(R_N + h)^2} \left[ \frac{1}{\text{s}^2} \right] & & & & & & \\ & & & \frac{1}{R_N+h} \left[ \frac{1}{\text{m}} \right] & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & & & & & \\ & \mathbf{0}_3 & & 0 & \frac{1}{(R_E+h)\cos{\phi}} \left[ \frac{1}{\text{m}} \right] & 0 & \frac{v_E\tan{\phi}}{(R_E+h)\cos{\phi}} \left[ \frac{1}{\text{s}} \right] & 0 & -\frac{v_E}{(R_E + h)^2\cos{\phi}} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & 0 & 0 & -1 \left[ - \right] & 0 & 0 & 0 & & & & & & \\ & & & & & & & & & & & & & & \\ & \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 & \\ & & & & & & & & & & & & & & \\ \end{bmatrix} \cdot \begin{bmatrix} \delta R & \left[ \text{rad} \right] \\ \delta P & \left[ \text{rad} \right] \\ \delta Y & \left[ \text{rad} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta \phi & \left[ \text{rad} \right] \\ \delta \lambda & \left[ \text{rad} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta f_{x}^{b} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta f_{y}^{b} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta f_{z}^{b} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \omega_{x}^{b} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \omega_{y}^{b} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \omega_{z}^{b} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \end{bmatrix} \end{equation}
When scaling elements in \( \boldsymbol{\delta} \mathbf{x} \) the relevant parts of the \( \mathbf{F} \) matrix have to be scaled as well.
\begin{equation} \begin{bmatrix} \delta \dot{R} & \left[ {\frac{\text{deg}}{\text{s}}} \right] \\ \delta \dot{P} & \left[ {\frac{\text{deg}}{\text{s}}} \right] \\ \delta \dot{Y} & \left[ {\frac{\text{deg}}{\text{s}}} \right] \\ \delta \dot{v}_{N} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \dot{v}_{E} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \dot{v}_{D} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \dot{\phi} & \left[ {\frac{\text{m'}}{\text{s}}} \right] \\ \delta \dot{\lambda} & \left[ {\frac{\text{m'}}{\text{s}}} \right] \\ \delta \dot{h} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta \dot{f}_{x}^{b} & \left[ {\frac{\text{mg}}{\text{s}}} \right] \\ \delta \dot{f}_{y}^{b} & \left[ {\frac{\text{mg}}{\text{s}}} \right] \\ \delta \dot{f}_{z}^{b} & \left[ {\frac{\text{mg}}{\text{s}}} \right] \\ \delta \dot{\omega}_{x}^{b} & \left[ {\frac{\text{mrad}}{\text{s}^2}} \right] \\ \delta \dot{\omega}_{y}^{b} & \left[ {\frac{\text{mrad}}{\text{s}^2}} \right] \\ \delta \dot{\omega}_{z}^{b} & \left[ {\frac{\text{mrad}}{\text{s}^2}} \right] \\ \end{bmatrix} = \quad \begin{bmatrix} 0 & \omega_{in,3}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & -\omega_{in,2}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & -\frac{1}{R_E + h} \left[ \frac{1}{\text{m}} \right] & 0 & \omega_{ie}\sin{\phi} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & \frac{v_E}{(R_E + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & & & & & \\ -\omega_{in,3}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & \omega_{in,1}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & \frac{1}{R_N + h} \left[ \frac{1}{\text{m}} \right] & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & \mathbf{0}_3 & & & \mathbf{C}_b^n \left[ - \right] & \\ \omega_{in,2}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & -\omega_{in,1}^{n} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & 0 & \frac{\tan{\phi}}{R_E + h} \left[ \frac{1}{\text{m}} \right] & 0 & \omega_{ie}\cos{\phi} + \frac{v_E}{(R_E + h)\cos^2{\phi}} \left[ \frac{\text{rad}}{\text{s}} \right] & 0 & -\frac{v_E \tan{\phi}}{(R_E + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & & & & & \\ 0 & f_D \left[ \frac{\text{m}}{\text{s}^2} \right] & -f_E \left[ \frac{\text{m}}{\text{s}^2} \right] & \frac{v_D}{R_N + h} \left[ \frac{1}{\text{s}} \right] & -2\frac{v_E \tan{\phi}}{R_E + h} - 2\omega_{ie}\sin{\phi} \left[ \frac{1}{\text{s}} \right] & \frac{v_N}{R_N + h} \left[ \frac{1}{\text{s}} \right] & -\frac{v_E^2}{(R_E+h)\cos^2{\phi}}-2v_E\omega_{ie}\cos{\phi} \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & \frac{v_E^2\tan{\phi}}{(R_E+h)^2} - \frac{v_Nv_D}{(R_N+h)^2} \left[ \frac{1}{\text{s}^2} \right] & & & & & & \\ -f_D \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & f_N \left[ \frac{\text{m}}{\text{s}^2} \right] & \frac{v_E \tan{\phi}}{R_E + h} + 2\omega_{ie}\sin{\phi} \left[ \frac{1}{\text{s}} \right] & \frac{v_N \tan{\phi} + v_D}{R_E + h} \left[ \frac{1}{\text{s}} \right] & \frac{v_E}{R_E + h} + 2\omega_{ie}\cos{\phi} \left[ \frac{1}{\text{s}} \right] & \frac{v_N v_E}{(R_E+h)\cos^2{\phi}} + 2v_N\omega_{ie}\cos{\phi} - 2v_D\omega_{ie}\sin{\phi} \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & -\frac{v_Nv_E\tan{\phi} + v_Ev_D}{(R_E + h)^2} \left[ \frac{1}{\text{s}^2} \right] & & \mathbf{C}_b^n \left[ - \right] & & & \mathbf{0}_3 & \\ f_E \left[ \frac{\text{m}}{\text{s}^2} \right] & -f_N \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & -\frac{2v_N}{R_N + h} \left[ \frac{1}{\text{s}} \right] & -\frac{2v_E}{R_E + h} - 2\omega_{ie}\cos{\phi} \left[ \frac{1}{\text{s}} \right] & 0 & 2v_E\omega_{ie}\sin{\phi} \left[ \frac{\text{m}}{\text{s}^2} \right] & 0 & \frac{v_E^2}{(R_E + h)^2} + \frac{v_N^2}{(R_N + h)^2} \left[ \frac{1}{\text{s}^2} \right] & & & & & & \\ & & & \frac{1}{R_N+h} \left[ \frac{1}{\text{m}} \right] & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & & & & & \\ & \mathbf{0}_3 & & 0 & \frac{1}{(R_E+h)\cos{\phi}} \left[ \frac{1}{\text{m}} \right] & 0 & \frac{v_E\tan{\phi}}{(R_E+h)\cos{\phi}} \left[ \frac{1}{\text{s}} \right] & 0 & -\frac{v_E}{(R_E + h)^2\cos{\phi}} \left[ \frac{1}{\text{m} \cdot \text{s}} \right] & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & 0 & 0 & -1 \left[ - \right] & 0 & 0 & 0 & & & & & & \\ & & & & & & & & & & & & & & \\ & \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 & \\ & & & & & & & & & & & & & & \\ \end{bmatrix} \cdot \begin{bmatrix} \delta R & \left[ {\text{deg}} \right] \\ \delta P & \left[ {\text{deg}} \right] \\ \delta Y & \left[ {\text{deg}} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta \phi & \left[ {\text{m'}} \right] \\ \delta \lambda & \left[ {\text{m'}} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta f_{x}^{b} & \left[ {\text{mg}} \right] \\ \delta f_{y}^{b} & \left[ {\text{mg}} \right] \\ \delta f_{z}^{b} & \left[ {\text{mg}} \right] \\ \delta \omega_{x}^{b} & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \delta \omega_{y}^{b} & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \delta \omega_{z}^{b} & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \end{bmatrix} \end{equation}
The unit \( m' \) is [pseudometre] and is latitude or longitude converted from radian into metre by multiplying it with the Earth radius. However the radius is assumed as a constant for the conversion.
If a bias state is modeled by a Gauss-Markov first order process instead of random walk, the deterministic part of the equation is extended.
\begin{equation} \begin{bmatrix} \boldsymbol{\delta \dot{\psi}} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \boldsymbol{\delta \dot{v}}^{n} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \boldsymbol{\delta \dot{p}}_b & \dots \\ \boldsymbol{\delta \dot{f}}^{\raise-0.45ex\hbox{$\scriptstyle b$}} & \left[ \frac{\text{m}}{\text{s}^3} \right] \\ \boldsymbol{\delta \dot{\omega}}_{ib}^b & \left[ \frac{\text{rad}}{\text{s}^2} \right] \\ \end{bmatrix} = \begin{bmatrix} \dots & \dots & \dots & \dots & \dots \\ \dots & \dots & \dots & \dots & \dots \\ \dots & \dots & \dots & \dots & \dots \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -\boldsymbol{\beta}_f \left[ \frac{1}{\text{s}} \right] & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -\boldsymbol{\beta}_\omega \left[ \frac{1}{\text{s}} \right] \\ \end{bmatrix} \cdot \begin{bmatrix} \boldsymbol{\delta \psi} & \left[ \text{rad} \right] \\ \boldsymbol{\delta v}^{n} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \boldsymbol{\delta p}_b & \dots \\ \boldsymbol{\delta f}^b & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \boldsymbol{\delta \omega}_{ib}^b & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \end{bmatrix} + \begin{bmatrix} \mathbf{C}_b^n & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n & \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 \\ \end{bmatrix} \cdot \mathbf{w} \left[ \frac{1}{\sqrt{\text{s}}} \right] \end{equation}
When scaling the biases in \( \boldsymbol{\delta} \mathbf{x} \) the \( \mathbf{\beta} \) factors stay unscaled. However the \( \mathbf{G} \) matrix has to be adapted.
\begin{equation} \begin{bmatrix} \boldsymbol{\delta \dot{\psi}} & \left[ {\text{deg}} \right] \\ \boldsymbol{\delta \dot{v}}^{n} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \boldsymbol{\delta \dot{p}}_b & \dots \\ \boldsymbol{\delta \dot{f}}^{\raise-0.45ex\hbox{$\scriptstyle b$}} & \left[ {\frac{\text{mg}}{\text{s}}} \right] \\ \boldsymbol{\delta \dot{\omega}}_{ib}^b & \left[ {\frac{\text{mrad}}{\text{s}^2}} \right] \\ \end{bmatrix} = \begin{bmatrix} \dots & \dots & \dots & \dots & \dots \\ \dots & \dots & \dots & \dots & \dots \\ \dots & \dots & \dots & \dots & \dots \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -\boldsymbol{\beta}_f \left[ \frac{1}{\text{s}} \right] & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & -\boldsymbol{\beta}_\omega \left[ \frac{1}{\text{s}} \right] \\ \end{bmatrix} \cdot \begin{bmatrix} \boldsymbol{\delta \dot{\psi}} & \left[ {\frac{\text{deg}}{\text{s}}} \right] \\ \boldsymbol{\delta \dot{v}}^{n} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \boldsymbol{\delta \dot{p}}_b & \dots \\ \boldsymbol{\delta f}^b & \left[ {\text{mg}} \right] \\ \boldsymbol{\delta \omega}_{ib}^b & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \end{bmatrix} + \qquad \begin{bmatrix} \mathbf{C}_b^n & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{C}_b^n & \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 \\ \end{bmatrix} \cdot \mathbf{w} \left[ \frac{1}{\sqrt{\text{s}}} \right] \end{equation}
If the noise input matrix \( \mathbf{G} \) is scaled, then the noise scale matrix \( \mathbf{W} \) does not have to be scaled.
If the alternative process noise covariance matrix \( \mathbf{Q} \) is used, it also has to be scaled.
The error covariance matrix is initialized as a diagonal matrix with the variance of the initial state as diagonal elements. Therefore the units are the squared units of the state vector.
\begin{equation} \mathbf{P}_0 = \begin{bmatrix} \sigma_{\delta R}^2 \left[ \text{rad}^2 \right] & & & & & & & \dots & & & & & & & 0 \\ & \sigma_{\delta P}^2 \left[ \text{rad}^2 \right] & & & & & & & & & & & & & \\ & & \sigma_{\delta Y}^2 \left[ \text{rad}^2 \right] & & & & & & & & & & & & \\ & & & \sigma_{\delta v_N}^2 \left[ \frac{\text{m}^2}{\text{s}^2} \right] & & & & & & & & & & & \\ & & & & \sigma_{\delta v_E}^2 \left[ \frac{\text{m}^2}{\text{s}^2} \right] & & & & & & & & & & \\ & & & & & \sigma_{\delta v_D}^2 \left[ \frac{\text{m}^2}{\text{s}^2} \right] & & & & & & & & & \\ & & & & & & \sigma_{\delta \phi}^2 \left[ \text{rad}^2 \right] & & & & & & & & \\ \vdots & & & & & & & \sigma_{\delta \lambda}^2 \left[ \text{rad}^2 \right] & & & & & & & \vdots \\ & & & & & & & & \sigma_{\delta h}^2 \left[ \text{m}^2 \right] & & & & & & \\ & & & & & & & & & \sigma_{\delta f_{x}^{b}}^2 \left[ \frac{\text{m}^2}{\text{s}^4} \right] & & & & & \\ & & & & & & & & & & \sigma_{\delta f_{y}^{b}}^2 \left[ \frac{\text{m}^2}{\text{s}^4} \right] & & & & \\ & & & & & & & & & & & \sigma_{\delta f_{z}^{b}}^2 \left[ \frac{\text{m}^2}{\text{s}^4} \right] & & & \\ & & & & & & & & & & & & \sigma_{\delta \omega_{x}^{b}}^2 \left[ \frac{\text{rad}^2}{\text{s}^2} \right] & & \\ & & & & & & & & & & & & & \sigma_{\delta \omega_{y}^{b}}^2 \left[ \frac{\text{rad}^2}{\text{s}^2} \right] & \\ 0 & & & & & & & \dots & & & & & & & \sigma_{\delta \omega_{z}^{b}}^2 \left[ \frac{\text{rad}^2}{\text{s}^2} \right] \\ \end{bmatrix} \end{equation}
When scaling elements in \( \boldsymbol{\delta} \mathbf{x} \) the relevant parts of the \( \mathbf{P} \) matrix have to be adapted. This means the scale factors have to be multiplied with the correct rows and columns. As \( \mathbf{P} \) is a diagonal matrix, this means only the diagonal terms have to be scaled.
\begin{equation} \mathbf{P}_0 = \begin{bmatrix} \sigma_{\delta R}^2 \left[ \text{rad}^2 \right] & & & & & & & \dots & & & & & & & 0 \\ & \sigma_{\delta P}^2 \left[ \text{rad}^2 \right] & & & & & & & & & & & & & \\ & & \sigma_{\delta Y}^2 \left[ \text{rad}^2 \right] & & & & & & & & & & & & \\ & & & \sigma_{\delta v_N}^2 \left[ \frac{\text{m}^2}{\text{s}^2} \right] & & & & & & & & & & & \\ & & & & \sigma_{\delta v_E}^2 \left[ \frac{\text{m}^2}{\text{s}^2} \right] & & & & & & & & & & \\ & & & & & \sigma_{\delta v_D}^2 \left[ \frac{\text{m}^2}{\text{s}^2} \right] & & & & & & & & & \\ & & & & & & \sigma_{\delta \phi}^2 \left[ \text{rad}^2 \right] & & & & & & & & \\ \vdots & & & & & & & \sigma_{\delta \lambda}^2 \left[ \text{rad}^2 \right] & & & & & & & \vdots \\ & & & & & & & & \sigma_{\delta h}^2 \left[ \text{m}^2 \right] & & & & & & \\ & & & & & & & & & \sigma_{\delta f_{x}^{b}}^2 \left[ \frac{\text{m}^2}{\text{s}^4} \right] & & & & & \\ & & & & & & & & & & \sigma_{\delta f_{y}^{b}}^2 \left[ \frac{\text{m}^2}{\text{s}^4} \right] & & & & \\ & & & & & & & & & & & \sigma_{\delta f_{z}^{b}}^2 \left[ \frac{\text{m}^2}{\text{s}^4} \right] & & & \\ & & & & & & & & & & & & \sigma_{\delta \omega_{x}^{b}}^2 \left[ \frac{\text{rad}^2}{\text{s}^2} \right] & & \\ & & & & & & & & & & & & & \sigma_{\delta \omega_{y}^{b}}^2 \left[ \frac{\text{rad}^2}{\text{s}^2} \right] & \\ 0 & & & & & & & \dots & & & & & & & \sigma_{\delta \omega_{z}^{b}}^2 \left[ \frac{\text{rad}^2}{\text{s}^2} \right] \\ \end{bmatrix} \end{equation}
\begin{equation} \boldsymbol{\delta} \mathbf{z} = \begin{bmatrix} \delta \phi & \left[ \text{rad} \right] \\ \delta \lambda & \left[ \text{rad} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \end{bmatrix} = \begin{bmatrix} \tilde{\phi}_G - \hat{\phi} & \left[ \text{rad} \right] \\ \tilde{\lambda}_G - \hat{\lambda} & \left[ \text{rad} \right] \\ \tilde{h}_G - \hat{h} & \left[ \text{m} \right] \\ \tilde{v}_{N,G} - \hat{v}_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \tilde{v}_{E,G} - \hat{v}_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \tilde{v}_{D,G} - \hat{v}_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \end{bmatrix} + \begin{bmatrix} - \begin{pmatrix} \frac{1}{R_N + h} \left[ \frac{1}{\text{m}} \right] & 0 & 0 \\[1.2em] 0 & \frac{1}{(R_E + h)\cos{\phi}} \left[ \frac{1}{\text{m}} \right] & 0 \\[1.2em] 0 & 0 & -1 \left[ - \right] \end{pmatrix} \mathbf{C}_b^n \left[ - \right] \mathbf{l}_{ba}^b \left[ \text{m} \right] \\ - \mathbf{C}_b^n \left[ - \right] (\boldsymbol{\omega}_{ib}^b \left[ \frac{\text{rad}}{\text{s}} \right] \times \mathbf{l}_{ba}^b \left[ \text{m} \right]) + \boldsymbol{\Omega}_{ie}^n \left[ \frac{\text{rad}}{\text{s}} \right] \mathbf{C}_b^n \left[ - \right] \mathbf{l}_{ba}^b \left[ \text{m} \right] \\ \end{bmatrix} \end{equation}
When scaling elements in \( \boldsymbol{\delta} \mathbf{x} \) the relevant parts of the measurement innovation \( \boldsymbol{\delta} \mathbf{z} \) have to be adapted
\begin{equation} \boldsymbol{\delta} \mathbf{z} = \begin{bmatrix} \delta \phi & \left[ {\text{m'}} \right] \\ \delta \lambda & \left[ {\text{m'}} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \end{bmatrix} = \quad \begin{bmatrix} \tilde{\phi}_G - \hat{\phi} & \left[ \text{rad} \right] \\ \tilde{\lambda}_G - \hat{\lambda} & \left[ \text{rad} \right] \\ \tilde{h}_G - \hat{h} & \left[ \text{m} \right] \\ \tilde{v}_{N,G} - \hat{v}_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \tilde{v}_{E,G} - \hat{v}_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \tilde{v}_{D,G} - \hat{v}_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \end{bmatrix} + \quad \begin{bmatrix} - \begin{pmatrix} \frac{1}{R_N + h} \left[ \frac{1}{\text{m}} \right] & 0 & 0 \\[1.2em] 0 & \frac{1}{(R_E + h)\cos{\phi}} \left[ \frac{1}{\text{m}} \right] & 0 \\[1.2em] 0 & 0 & -1 \left[ - \right] \end{pmatrix} \mathbf{C}_b^n \left[ - \right] \mathbf{l}_{ba}^b \left[ \text{m} \right] \\ - \mathbf{C}_b^n \left[ - \right] (\boldsymbol{\omega}_{ib}^b \left[ \frac{\text{rad}}{\text{s}} \right] \times \mathbf{l}_{ba}^b \left[ \text{m} \right]) + \boldsymbol{\Omega}_{ie}^n \left[ \frac{\text{rad}}{\text{s}} \right] \mathbf{C}_b^n \left[ - \right] \mathbf{l}_{ba}^b \left[ \text{m} \right] \\ \end{bmatrix} \end{equation}
\begin{equation} \begin{bmatrix} \delta \phi & \left[ \text{rad} \right] \\ \delta \lambda & \left[ \text{rad} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \end{bmatrix} = \begin{bmatrix} & & & & & & -1 \left[ - \right] & 0 & 0 & & & & & & \\ & \mathbf{H}_{r,\delta \psi}^n \left[ \text{rad} \right] & & & \mathbf{0}_3 & & 0 & -1 \left[ - \right] & 0 & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & & & & 0 & 0 & -1 \left[ - \right] & & & & & & \\ & & & -1 \left[ - \right] & 0 & 0 & & & & & & & & & \\ & \mathbf{H}_{v,\delta \psi}^n \left[ \frac{\text{m}}{\text{s}} \right] & & 0 & -1 \left[ - \right] & 0 & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{H}_{\delta v,\omega_{ib}^b}^n \left[ \text{m} \right] & \\ & & & 0 & 0 & -1 \left[ - \right] & & & & & & & & & \\ \end{bmatrix} \cdot \begin{bmatrix} \delta R & \left[ \text{rad} \right] \\ \delta P & \left[ \text{rad} \right] \\ \delta Y & \left[ \text{rad} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta \phi & \left[ \text{rad} \right] \\ \delta \lambda & \left[ \text{rad} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta f_{x}^{b} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta f_{y}^{b} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta f_{z}^{b} & \left[ \frac{\text{m}}{\text{s}^2} \right] \\ \delta \omega_{x}^{b} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \omega_{y}^{b} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \delta \omega_{z}^{b} & \left[ \frac{\text{rad}}{\text{s}} \right] \\ \end{bmatrix} \end{equation}
When scaling elements in \( \boldsymbol{\delta} \mathbf{x} \) the relevant parts of the measurement innovation \( \boldsymbol{\delta} \mathbf{z} \) have to be adapted. The \( \mathbf{H} \) matrix stays unscaled.
\begin{equation} \begin{bmatrix} \delta \phi & \left[ {\text{m'}} \right] \\ \delta \lambda & \left[ {\text{m'}} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \end{bmatrix} = \begin{bmatrix} & & & & & & -1 \left[ - \right] & 0 & 0 & & & & & & \\ & \mathbf{H}_{r,\delta \psi}^n \left[ \text{rad} \right] & & & \mathbf{0}_3 & & 0 & -1 \left[ - \right] & 0 & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & & & & 0 & 0 & -1 \left[ - \right] & & & & & & \\ & & & -1 \left[ - \right] & 0 & 0 & & & & & & & & & \\ & \mathbf{H}_{v,\delta \psi}^n \left[ \frac{\text{m}}{\text{s}} \right] & & 0 & -1 \left[ - \right] & 0 & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{H}_{\delta v,\omega_{ib}^b}^n \left[ \text{m} \right] & \\ & & & 0 & 0 & -1 \left[ - \right] & & & & & & & & & \\ \end{bmatrix} \cdot \begin{bmatrix} \delta R & \left[ {\text{deg}} \right] \\ \delta P & \left[ {\text{deg}} \right] \\ \delta Y & \left[ {\text{deg}} \right] \\ \delta v_{N} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{E} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta v_{D} & \left[ \frac{\text{m}}{\text{s}} \right] \\ \delta \phi & \left[ {\text{m'}} \right] \\ \delta \lambda & \left[ {\text{m'}} \right] \\ \delta h & \left[ \text{m} \right] \\ \delta f_{x}^{b} & \left[ {\text{mg}} \right] \\ \delta f_{y}^{b} & \left[ {\text{mg}} \right] \\ \delta f_{z}^{b} & \left[ {\text{mg}} \right] \\ \delta \omega_{x}^{b} & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \delta \omega_{y}^{b} & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \delta \omega_{z}^{b} & \left[ {\frac{\text{mrad}}{\text{s}}} \right] \\ \end{bmatrix} \end{equation}
\begin{equation} \mathbf{R} = E \left( (\mathbf{z} - \mathbf{H} \mathbf{x})(\mathbf{z} - \mathbf{H} \mathbf{x})^T \right) = \begin{bmatrix} \sigma^2_{z_\phi} \left[ \text{rad}^2 \right] & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma^2_{z_\lambda} \left[ \text{rad}^2 \right] & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma^2_{z_h} \left[ \text{m}^2 \right] & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma^2_{z_{v_N}} \left[ \frac{\text{m}^2}{\text{s}^2} \right] & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma^2_{z_{v_E}} \left[ \frac{\text{m}^2}{\text{s}^2} \right] & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma^2_{z_{v_D}} \left[ \frac{\text{m}^2}{\text{s}^2} \right] \\ \end{bmatrix} \end{equation}
When scaling elements in \( \boldsymbol{\delta} \mathbf{x} \) the measurement innovation \( \boldsymbol{\delta} \mathbf{z} \) is scaled and therefore the relevant parts of the \( \mathbf{R} \) matrix have to be adapted.
\begin{equation} \mathbf{R} = E \left( (\mathbf{z} - \mathbf{H} \mathbf{x})(\mathbf{z} - \mathbf{H} \mathbf{x})^T \right) = \begin{bmatrix} \sigma^2_{z_\phi} \left[ \text{rad}^2 \right] & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma^2_{z_\lambda} \left[ \text{rad}^2 \right] & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma^2_{z_h} \left[ \text{m}^2 \right] & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma^2_{z_{v_N}} \left[ \frac{\text{m}^2}{\text{s}^2} \right] & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma^2_{z_{v_E}} \left[ \frac{\text{m}^2}{\text{s}^2} \right] & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma^2_{z_{v_D}} \left[ \frac{\text{m}^2}{\text{s}^2} \right] \\ \end{bmatrix} \end{equation}
The time derivative of curvilinear position as a function of the Earth-referenced velocity in local navigation frame axes (see [17] Groves, ch. 2.4.2, eq. 2.111, p. 61 or [45] Titterton, ch. 3.7, eq. 3.81,3.85,3.86, p. 48ff):
\begin{equation} \begin{aligned} \dot{\phi} &= \frac{v_N}{R_N + h} \\ \dot{\lambda} &= \frac{v_E}{(R_E + h)\cos{\phi}} \\ \dot{h} &= -v_D \\ \end{aligned} \end{equation}
Differential of left and right side neglecting errors in \(R_N \) and \(R_E \) ( \( \frac{\partial R_N}{\partial p_i} = 0 \) and \( \quad \frac{\partial R_E}{\partial p_i} = 0\))
\begin{equation} \begin{aligned} \delta\dot{\phi} &= \frac{\partial \dot{\phi}}{\partial v_N} \delta v_N + \frac{\partial \dot{\phi}}{\partial h} \delta h = {\dfrac{1}{R_N + h}} \delta v_N {- \dfrac{v_N}{(R_N + h)^2}} \delta h \\ \delta\dot{\lambda} &= \frac{\partial \dot{\lambda}}{\partial v_E} \delta v_E + \frac{\partial \dot{\lambda}}{\partial h} \delta h + \frac{\partial \dot{\lambda}}{\partial \phi} \delta \phi = {\dfrac{1}{(R_E + h)\cos{\phi}}} \delta v_E {- \dfrac{v_E}{(R_E + h)^2\cos{\phi}}} \delta h + {\dfrac{v_E \tan{\phi}}{(R_E + h)\cos{\phi}}} \delta \phi \\ \delta\dot{h} &= \frac{\partial \dot{h}}{\partial v_D} \delta v_D = {-1} \cdot \delta v_D \\ \end{aligned} \end{equation}
The equations here mainly follow [45] Titterton, ch. 12.3.1.1, p. 342f
The estimated attitude \( \mathbf{\hat{C}}_b^n \) can be written in terms of true direction cosine matrix \( \mathbf{C}_b^n \)
\begin{equation} \mathbf{\hat{C}}_b^n = \mathbf{B} \mathbf{C}_b^n \end{equation}
where \( \mathbf{B} \) is the transformation from true reference axes to estimated reference axes and it can be approximated for small angle misalignments as
\begin{equation} \mathbf{B} = [\mathbf{I} - \mathbf{\Psi}], \quad \text{with}~ \mathbf{\Psi} = \begin{pmatrix} \delta\alpha \\ \delta\beta \\ \delta\gamma \end{pmatrix} \times \end{equation}
with \( \mathbf{\Psi} \) being a skew symmetric matrix and \( \delta\alpha, \delta\beta, \delta\gamma \) being the attitude errors (e.g. roll, pitch, yaw for small euler angles).
Substituting eq-LCKF_n-Derivation-Attitude-transformation into eq-LCKF_n-Derivation-Attitude-estimate-true gives
\begin{equation} \mathbf{\hat{C}}_b^n \approx [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n \end{equation}
and solving for \( \mathbf{\Psi} \)
\begin{equation} \mathbf{\Psi} = \mathbf{I} - \mathbf{\hat{C}}_b^n {\mathbf{C}_b^n}^T \end{equation}
Differentiating this equation yields:
\begin{equation} \mathbf{\dot{\Psi}} = -\mathbf{\dot{\hat{C}}}_b^{\raise-1.25ex\hbox{$\scriptstyle n$}} {\mathbf{C}_b^n}^T - \mathbf{\hat{C}}_b^n {\mathbf{\dot{C}}_b^n}\raise1.25ex\hbox{$\scriptstyle T$} \end{equation}
Using the following equations
leads to the time derivative of the coordinate transformation matrix (see [17] Groves, ch. 5.4.1, eq. 5.40, p. 177 or [45] Titterton, ch. 12.3.1.1, eq. 12.12, p. 342)
\begin{equation} \begin{aligned} \mathbf{\dot{C}}_b^n &= \mathbf{C}_b^n \mathbf{\Omega}_{nb}^b \\ &= \mathbf{C}_b^n (\mathbf{\Omega}_{ib}^b - \mathbf{\Omega}_{in}^b) \\ &= \mathbf{C}_b^n (\mathbf{\Omega}_{ib}^b - \mathbf{C}_n^b \mathbf{\Omega}_{in}^n \mathbf{C}_b^n) \\ &= \mathbf{C}_b^n \mathbf{\Omega}_{ib}^b - \mathbf{\Omega}_{in}^n \mathbf{C}_b^n \end{aligned} \end{equation}
where
Similarly, the time time differential of the estimated matrix \( \mathbf{\dot{\hat{C}}}_b^{\raise-1.25ex\hbox{$\scriptstyle n$}} \) can be calculated:
\begin{equation} \mathbf{\dot{\hat{C}}}_b^{\raise-1.25ex\hbox{$\scriptstyle n$}} = \mathbf{\hat{C}}_b^n \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\hat{\Omega}}_{in}^n \mathbf{\hat{C}}_b^n \end{equation}
where
Substituting eq-LCKF_n-Derivation-Attitude-DCM-dot and eq-LCKF_n-Derivation-Attitude-DCM-dot-estimated into eq-LCKF_n-Derivation-Attitude-skew-mat-dot
\begin{equation} \begin{aligned} \mathbf{\dot{\Psi}} &= -(\mathbf{\hat{C}}_b^n \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\hat{\Omega}}_{in}^n \mathbf{\hat{C}}_b^n) {\mathbf{C}_b^n}^T - \mathbf{\hat{C}}_b^n (\mathbf{C}_b^n \mathbf{\Omega}_{ib}^b - \mathbf{\Omega}_{in}^n \mathbf{C}_b^n)^T \\ &= - \mathbf{\hat{C}}_b^n \mathbf{\hat{\Omega}}_{ib}^b {\mathbf{C}_b^n}^T + \mathbf{\hat{\Omega}}_{in}^n \mathbf{\hat{C}}_b^n {\mathbf{C}_b^n}^T + \mathbf{\hat{C}}_b^n \mathbf{\Omega}_{ib}^b {\mathbf{C}_b^n}^T - \mathbf{\hat{C}}_b^n {\mathbf{C}_b^n}^T \mathbf{\Omega}_{in}^n \\ &= - \mathbf{\hat{C}}_b^n \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^n}^T + \mathbf{\hat{\Omega}}_{in}^n \mathbf{\hat{C}}_b^n {\mathbf{C}_b^n}^T - \mathbf{\hat{C}}_b^n {\mathbf{C}_b^n}^T \mathbf{\Omega}_{in}^n \end{aligned} \end{equation}
Substituting eq-LCKF_n-Derivation-Attitude-estimate-errors into eq-LCKF_n-Derivation-Attitude-skew-mat-dot-calc1 gives:
\begin{equation} \begin{aligned} \mathbf{\dot{\Psi}} &= - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^n}^T + \mathbf{\hat{\Omega}}_{in}^n [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n {\mathbf{C}_b^n}^T - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n {\mathbf{C}_b^n}^T \mathbf{\Omega}_{in}^n \\ &= - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^n}^T + \mathbf{\hat{\Omega}}_{in}^n - \mathbf{\hat{\Omega}}_{in}^n \mathbf{\Psi} - \mathbf{\Omega}_{in}^n + \mathbf{\Psi} \mathbf{\Omega}_{in}^n \\ &= - [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n \left[ \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\Omega}_{ib}^b \right] {\mathbf{C}_b^n}^T + \left[ \mathbf{\hat{\Omega}}_{in}^n - \mathbf{\Omega}_{in}^n \right] - \mathbf{\hat{\Omega}}_{in}^n \mathbf{\Psi} + \mathbf{\Psi} \mathbf{\Omega}_{in}^n \end{aligned} \end{equation}
Now we make the following substitutions
and neglect error product terms. This leads to (same as [45] Titterton, ch. 12.3.1.1, eq. 12.17, p. 343)
\begin{equation} \mathbf{\dot{\Psi}} \approx \mathbf{\Psi} \mathbf{\Omega}_{in}^n - \mathbf{\hat{\Omega}}_{in}^n \mathbf{\Psi} + \boldsymbol{\delta}\mathbf{\Omega}_{in}^n - \mathbf{C}_b^n \boldsymbol{\delta}\mathbf{\Omega}_{ib}^b {\mathbf{C}_b^n}^T \end{equation}
Finally by performing an element by element comparison with
\begin{equation} \boldsymbol{\delta \psi} \times = \mathbf{\Psi} \qquad\quad \boldsymbol{\omega}_{in}^n \times = \mathbf{\Omega}_{in}^n \qquad\quad \boldsymbol{\delta \omega}_{in}^n \times = \boldsymbol{\delta}\mathbf{\Omega}_{in}^n \qquad\quad \boldsymbol{\delta \omega}_{ib}^b \times = \boldsymbol{\delta}\mathbf{\Omega}_{ib}^b \end{equation}
the equation can be expressed in vector form as:
\begin{equation} \begin{aligned} \boldsymbol{\delta \dot{\psi}} &\approx {-\boldsymbol{\omega}_{in}^n \times} \boldsymbol{\delta \psi} + \boldsymbol{\delta \omega}_{in}^n {- \mathbf{C}_b^n} \boldsymbol{\delta \omega}_{ib}^b \\ &= {-\boldsymbol{\omega}_{in}^n \times} \boldsymbol{\delta \psi} + \boldsymbol{\delta}(\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) {- \mathbf{C}_b^n} \boldsymbol{\delta \omega}_{ib}^b \end{aligned} \end{equation}
The transport rate \( \boldsymbol{\omega}_{en}^n \) is defined as (see [17] Groves, ch. 5.4.1, eq. 5.44, p. 177 or [14] Gleason, ch. 6.2.3.2, eq. 6.15, p. 155)
\begin{equation} \boldsymbol{\omega}_{en}^n = \begin{bmatrix} \dfrac{v_E}{R_E + h} & -\dfrac{v_N}{R_N + h} & -\dfrac{v_E \tan{\phi}}{R_E + h} \end{bmatrix}^T \end{equation}
The differential of left and right side neglecting errors in \(R_N \) and \(R_E \) ( \( \frac{\partial R_N}{\partial p_i} = 0 \) and \( \quad \frac{\partial R_E}{\partial p_i} = 0\))
\begin{equation} \begin{aligned} \delta \omega_{en,1}^n &= \frac{\partial \omega_{en,1}^n}{\partial v_E} \delta v_E + \frac{\partial\omega_{en,1}^n}{\partial h} \delta h = \frac{1}{R_E + h} \delta v_E - \frac{v_E}{(R_E + h)^2} \delta h \\ \delta \omega_{en,2}^n &= \frac{\partial \omega_{en,2}^n}{\partial v_N} \delta v_N + \frac{\partial \omega_{en,2}^n}{\partial h} \delta h = -\frac{1}{R_N + h} \delta v_N + \frac{v_N}{(R_N + h)^2} \delta h \\ \delta \omega_{en,3}^n &= \frac{\partial \omega_{en,3}^n}{\partial v_E} \delta v_E + \frac{\partial \omega_{en,3}^n}{\partial h} \delta h + \frac{\partial \omega_{en,3}^n}{\partial \phi} \delta \phi = -\frac{\tan{\phi}}{R_E + h} \delta v_E + \frac{v_E \tan{\phi}}{(R_E + h)^2} \delta h - \frac{v_E}{(R_E + h)\cos^2{\phi}} \delta \phi \end{aligned} \end{equation}
The Earth rotation in local-navigation-frame coordinates \( \boldsymbol{\omega}_{ie}^n \) is given
\begin{equation} \boldsymbol{\omega}_{ie}^n = \mathbf{C}_e^n \boldsymbol{\omega}_{ie}^e = \begin{pmatrix} -\sin{\phi}\cos{\lambda} & -\sin{\phi}\sin{\lambda} & \cos{\phi} \\ -\sin{\lambda} & \cos{\lambda} & 0 \\ -\cos{\phi}\cos{\lambda} & -\cos{\phi}\sin{\lambda} & -\sin{\phi} \end{pmatrix} \cdot \begin{pmatrix} 0 \\ 0 \\ \omega_{ie} \end{pmatrix} = \omega_{ie} \cdot \begin{pmatrix} \cos{\phi} \\ 0 \\ -\sin{\phi} \end{pmatrix} \end{equation}
The differential of left and right side is
\begin{equation} \begin{aligned} \boldsymbol{\delta \omega}_{ie}^n = \frac{\partial \boldsymbol{\omega}_{ie}^n}{\partial \phi} \delta \phi = \begin{pmatrix} -\omega_{ie}\sin{\phi} \\ 0 \\ -\omega_{ie}\cos{\phi} \end{pmatrix} \delta \phi \end{aligned} \end{equation}
Substituting eq-LCKF_n-Derivation-Attitude-transport-rate-differential and eq-LCKF_n-Derivation-Attitude-earth-rotation-n-differential into eq-LCKF_n-Derivation-Attitude-differential gives:
\begin{equation} \boldsymbol{\delta \dot{\psi}} \approx {-\boldsymbol{\omega}_{in}^n \times} \boldsymbol{\delta \psi} + \begin{bmatrix} -\omega_{ie}\sin{\phi} \\ 0 \\ -\omega_{ie}\cos{\phi} \\ \end{bmatrix} \delta \phi + \begin{bmatrix} 0 & \frac{1}{R_E + h} & 0 \\ -\frac{1}{R_N + h} & 0 & 0 \\ 0 & -\frac{\tan{\phi}}{R_E + h} & 0 \\ \end{bmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \end{pmatrix} + \begin{bmatrix} 0 & 0 & -\frac{v_E}{(R_E + h)^2} \\ 0 & 0 & \frac{v_N}{(R_N + h)^2} \\ - \frac{v_E}{(R_E + h)\cos^2{\phi}} & 0 & \frac{v_E \tan{\phi}}{(R_E + h)^2} \\ \end{bmatrix} \begin{pmatrix} \delta \phi \\ \delta \lambda \\ \delta h \end{pmatrix} {- \mathbf{C}_b^n} \boldsymbol{\delta \omega}_{ib}^b \end{equation}
The equations here mainly follow [45] Titterton, ch. 12.3.1.2, p. 343f
The time derivative of the velocity in local-navigation-frame coordinates is (see [45] Titterton, ch. 3.7.1, eq. 3.69, p. 47 or [22] Jekeli, ch. 4.3.4, eq. 4.88, p. 127)
\begin{equation} \boldsymbol{\dot{v}}^n = \overbrace{\boldsymbol{f}^n}^{\text{measured}} -\ \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} +\ \overbrace{\mathbf{g}^n}^{\text{gravity}} \end{equation}
where
The estimated velocity follows the same propagation
\begin{equation} \boldsymbol{\dot{\hat{v}}}^{\raise-0.45ex\hbox{$\scriptstyle n$}} = \boldsymbol{\hat{f}}^n - (2 \boldsymbol{\hat{\omega}}_{ie}^n + \boldsymbol{\hat{\omega}}_{en}^n) \times \boldsymbol{\hat{v}}_e^n + \mathbf{\hat{g}}^n \end{equation}
Differencing equation eq-LCKF_n-Derivation-Velocity-timeDerivative and eq-LCKF_n-Derivation-Velocity-timeDerivative-estimate we get
\begin{equation} \begin{aligned} \boldsymbol{\delta \dot{v}}^n &= \boldsymbol{\dot{\hat{v}}}^{\raise-0.45ex\hbox{$\scriptstyle n$}} - \boldsymbol{\dot{v}}^n \\ &= \mathbf{\hat{C}}_b^n \boldsymbol{\hat{f}}^b - \mathbf{C}_b^n \boldsymbol{f}^b - (2 \boldsymbol{\hat{\omega}}_{ie}^n + \boldsymbol{\hat{\omega}}_{en}^n) \times \boldsymbol{\hat{v}}^n + (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n + \mathbf{\hat{g}}^n - \mathbf{g}^n \end{aligned} \end{equation}
\begin{equation} \begin{aligned} \boldsymbol{\delta \dot{v}}^n &= [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^n \boldsymbol{\hat{f}}^b - \mathbf{C}_b^n \boldsymbol{f}^b - (2 \boldsymbol{\delta \omega}_{ie}^n + 2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{\hat{v}}^n + (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n + \boldsymbol{\delta} \mathbf{g}^n \\ &= \mathbf{C}_b^n \boldsymbol{\hat{f}}^b - \mathbf{\Psi} \mathbf{C}_b^n \boldsymbol{\hat{f}}^b - \mathbf{C}_b^n \boldsymbol{f}^b - (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{\hat{v}}^n - (2 \boldsymbol{\delta \omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n) \times \boldsymbol{\hat{v}}^n + (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n + \boldsymbol{\delta} \mathbf{g}^n \\ &= - \boldsymbol{\delta \psi} \times \mathbf{C}_b^n \boldsymbol{\hat{f}}^b + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} - (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{\delta v}^n - (2 \boldsymbol{\delta \omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n) \times \boldsymbol{\hat{v}}^n + \boldsymbol{\delta} \mathbf{g}^n \\ &= (\mathbf{C}_b^n \boldsymbol{\delta f}^b + \mathbf{C}_b^n \boldsymbol{f}^b) \times \boldsymbol{\delta \psi} + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} - (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{\delta v}^n - (2 \boldsymbol{\delta \omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n) \times (\boldsymbol{\delta v}^n + \boldsymbol{v}^n) + \boldsymbol{\delta} \mathbf{g}^n \\ \end{aligned} \end{equation}
Now neglecting error product terms \( \boldsymbol{\delta f}^b \times \boldsymbol{\delta \psi} \approx 0 \) and \( (2 \boldsymbol{\delta \omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n) \times \boldsymbol{\delta v}^n \approx 0\)
\begin{equation} \boldsymbol{\delta \dot{v}}^n \approx {\boldsymbol{f}^n \times} \boldsymbol{\delta \psi} + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} - (2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{\delta v}^n - (2 \boldsymbol{\delta \omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n) \times \boldsymbol{v}^n + \boldsymbol{\delta} \mathbf{g}^n \end{equation}
This is equal to [45] Titterton, ch. 12.3.1.2, eq. 12.21, p. 344, except the sign of the gravity error term.
Substituting equations eq-LCKF_n-Derivation-Attitude-transport-rate, eq-LCKF_n-Derivation-Attitude-transport-rate-differential, eq-LCKF_n-Derivation-Attitude-earth-rotation-n and eq-LCKF_n-Derivation-Attitude-earth-rotation-n-differential leads to
\begin{equation} \renewcommand*{\arraystretch}{1.9} \begin{aligned} \boldsymbol{\delta \dot{v}}^n &\approx {\boldsymbol{f}^n \times} \boldsymbol{\delta \psi} + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} - \left[\begin{pmatrix} 2 \omega_{ie} \cos{\phi} \\ 0 \\ - 2 \omega_{ie} \sin{\phi} \end{pmatrix} + \begin{pmatrix} \frac{v_E}{R_E + h} \\ -\frac{v_N}{R_N + h} \\ -\frac{v_E \tan{\phi}}{R_E + h} \end{pmatrix} \right] \times \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \end{pmatrix} - \left[\begin{pmatrix} -2\omega_{ie}\sin{\phi}\ \delta \phi \\ 0 \\ -2\omega_{ie}\cos{\phi}\ \delta \phi \end{pmatrix} + \begin{pmatrix} \frac{1}{R_E + h} \delta v_E - \frac{v_E}{(R_E + h)^2} \delta h \\ -\frac{1}{R_N + h} \delta v_N + \frac{v_N}{(R_N + h)^2} \delta h \\ -\frac{\tan{\phi}}{R_E + h} \delta v_E + \frac{v_E \tan{\phi}}{(R_E + h)^2} \delta h - \frac{v_E}{(R_E + h)\cos^2{\phi}} \delta \phi \\ \end{pmatrix} \right] \times \begin{pmatrix} v_N \\ v_E \\ v_D \end{pmatrix} + \boldsymbol{\delta} \mathbf{g}^n \\ &= {\boldsymbol{f}^n \times} \boldsymbol{\delta \psi} + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} + \begin{pmatrix} - 2 \omega_{ie} \cos{\phi} - \frac{v_E}{R_E + h} \\ \frac{v_N}{R_N + h} \\ 2 \omega_{ie} \sin{\phi} + \frac{v_E \tan{\phi}}{R_E + h} \end{pmatrix} \times \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \\ \end{pmatrix} + \begin{pmatrix} 2\omega_{ie}\sin{\phi}\ \delta \phi - \frac{1}{R_E + h} \delta v_E + \frac{v_E}{(R_E + h)^2} \delta h \\ \frac{1}{R_N + h} \delta v_N - \frac{v_N}{(R_N + h)^2} \delta h \\ 2\omega_{ie}\cos{\phi}\ \delta \phi + \frac{\tan{\phi}}{R_E + h} \delta v_E - \frac{v_E \tan{\phi}}{(R_E + h)^2} \delta h + \frac{v_E}{(R_E + h)\cos^2{\phi}} \delta \phi \end{pmatrix} \times \begin{pmatrix} v_N \\ v_E \\ v_D \\ \end{pmatrix} + \boldsymbol{\delta} \mathbf{g}^n \\ &= {\boldsymbol{f}^n \times} \boldsymbol{\delta \psi} + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} + \begin{pmatrix} \left(\frac{v_N}{R_N + h} \right) \cdot \delta v_D - \left(2 \omega_{ie} \sin{\phi} + \frac{v_E \tan{\phi}}{R_E + h} \right) \cdot \delta v_E \\ \left(2 \omega_{ie} \sin{\phi} + \frac{v_E \tan{\phi}}{R_E + h} \right) \cdot \delta v_N - \left(- 2 \omega_{ie} \cos{\phi} - \frac{v_E}{R_E + h} \right) \cdot \delta v_D \\ \left(- 2 \omega_{ie} \cos{\phi} - \frac{v_E}{R_E + h} \right) \cdot \delta v_E - \left(\frac{v_N}{R_N + h} \right) \cdot \delta v_N \\ \end{pmatrix} + \begin{pmatrix} \left( \frac{1}{R_N + h} \delta v_N - \frac{v_N}{(R_N + h)^2} \delta h \right) \cdot v_D - \left( 2\omega_{ie}\cos{\phi}\ \delta \phi + \frac{\tan{\phi}}{R_E + h} \delta v_E - \frac{v_E \tan{\phi}}{(R_E + h)^2} \delta h + \frac{v_E}{(R_E + h)\cos^2{\phi}} \delta \phi \right) \cdot v_E \\ \left( 2\omega_{ie}\cos{\phi}\ \delta \phi + \frac{\tan{\phi}}{R_E + h} \delta v_E - \frac{v_E \tan{\phi}}{(R_E + h)^2} \delta h + \frac{v_E}{(R_E + h)\cos^2{\phi}} \delta \phi \right) \cdot v_N - \left( 2\omega_{ie}\sin{\phi}\ \delta \phi - \frac{1}{R_E + h} \delta v_E + \frac{v_E}{(R_E + h)^2} \delta h \right) \cdot v_D \\ \left( 2\omega_{ie}\sin{\phi}\ \delta \phi - \frac{1}{R_E + h} \delta v_E + \frac{v_E}{(R_E + h)^2} \delta h \right) \cdot v_E - \left( \frac{1}{R_N + h} \delta v_N - \frac{v_N}{(R_N + h)^2} \delta h \right) \cdot v_N \\ \end{pmatrix} + \boldsymbol{\delta} \mathbf{g}^n \\ \end{aligned} \end{equation}
The gravity error can be expressed as (see [17] Groves, ch. 14.2.4, eq. 14.59, p. 586)
\begin{equation} \begin{aligned} \boldsymbol{\delta} \mathbf{g}^e \approx - \frac{2 g_0}{r_{eS}^e} \delta h \mathbf{u}_D^e \end{aligned} \end{equation}
where
Reordering the terms leads to the final form
\begin{equation} \boldsymbol{\delta \dot{v}}^n \approx {\boldsymbol{f}^n \times} \boldsymbol{\delta \psi} + \begin{bmatrix} \frac{v_D}{R_N + h} & - \frac{v_E \tan{\phi}}{R_E + h} - 2 \omega_{ie} \sin{\phi} - \frac{v_E \tan{\phi}}{R_E + h} & \frac{v_N}{R_N + h} \\ \frac{v_E \tan{\phi}}{R_E + h} + 2 \omega_{ie} \sin{\phi} & \frac{v_N \tan{\phi}}{R_E + h} + \frac{v_D}{R_E + h} & \frac{v_E}{R_E + h} + 2 \omega_{ie} \cos{\phi} \\ - \frac{v_N}{R_N + h} - \frac{v_N}{R_N + h} & - 2 \omega_{ie} \cos{\phi} - \frac{v_E}{R_E + h} - \frac{v_E}{R_E + h} & 0 \\ \end{bmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \\ \end{pmatrix} + \begin{bmatrix} - \frac{v_E^2}{(R_E + h)\cos^2{\phi}} - 2 v_E \omega_{ie}\cos{\phi} & 0 & \frac{v_E^2 \tan{\phi}}{(R_E + h)^2} - \frac{v_N v_D}{(R_N + h)^2} \\ \frac{v_N v_E}{(R_E + h)\cos^2{\phi}} + 2 v_N \omega_{ie}\cos{\phi} - 2 v_D \omega_{ie}\sin{\phi} & 0 & - \frac{v_N v_E \tan{\phi}}{(R_E + h)^2} - \frac{v_E v_D}{(R_E + h)^2} \\ 2 v_E \omega_{ie}\sin{\phi} & 0 & \frac{v_E^2}{(R_E + h)^2} + \frac{v_N^2}{(R_N + h)^2} - \frac{2 g_0}{r_{eS}^e} \\ \end{bmatrix} \begin{pmatrix} \delta \phi \\ \delta \lambda \\ \delta h \\ \end{pmatrix} + {\mathbf{C}_b^n \boldsymbol{\delta f}^b} \end{equation}
[45] Titterton, ch. 12.3.1.3, eq. 12.28, p. 345 has different sign in equations \( {\mathbf{F}_{\delta \dot{\psi},\delta v}^n} \), \( {\mathbf{F}_{\delta \dot{\psi},\delta r}^n} \), \( {\mathbf{F}_{\delta \dot{\psi},\delta \omega}^n} \), \( {\mathbf{F}_{\delta \dot{v},\delta \psi}^n} \)
\begin{equation} \begin{bmatrix} \delta \dot{R} \\ \delta \dot{P} \\ \delta \dot{Y} \\ \delta \dot{v}_{N} \\ \delta \dot{v}_{E} \\ \delta \dot{v}_{D} \\ \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta \dot{h} \\ \delta \dot{f}_{x}^{b} \\ \delta \dot{f}_{y}^{b} \\ \delta \dot{f}_{z}^{b} \\ \delta \dot{\omega}_{x}^{b} \\ \delta \dot{\omega}_{y}^{b} \\ \delta \dot{\omega}_{z}^{b} \\ \end{bmatrix} = \begin{bmatrix} 0 & \omega_{in,3}^{n} & -\omega_{in,2}^{n} & 0 & {+}\frac{1}{R_E + h} & 0 & {-}\omega_{ie}\sin{\phi} & 0 & {-}\frac{v_E}{(R_E + h)^2} & & & & & & \\ -\omega_{in,3}^{n} & 0 & \omega_{in,1}^{n} & {-}\frac{1}{R_N + h} & 0 & 0 & 0 & 0 & {+}\frac{v_N}{(R_N + h)^2} & & \mathbf{0}_3 & & & {-}\mathbf{C}_b^n & \\ \omega_{in,2}^{n} & -\omega_{in,1}^{n} & 0 & 0 & {-}\frac{\tan{\phi}}{R_E + h} & 0 & {-}\omega_{ie}\cos{\phi} {-} \frac{v_E}{(R_E + h)\cos^2{\phi}} & 0 & {+}\frac{v_E \tan{\phi}}{(R_E + h)^2} & & & & & & \\ 0 & {-}f_D & {+}f_E & \frac{v_D}{R_N + h} & -2\frac{v_E \tan{\phi}}{R_E + h} - 2\omega_{ie}\sin{\phi} & \frac{v_N}{R_N + h} & -\frac{v_E^2}{(R_E+h)\cos^2{\phi}}-2v_E\omega_{ie}\cos{\phi} & 0 & \frac{v_E^2\tan{\phi}}{(R_E+h)^2} - \frac{v_Nv_D}{(R_N+h)^2} & & & & & & \\ {+}f_D & 0 & {-}f_N & \frac{v_E \tan{\phi}}{R_E + h} + 2\omega_{ie}\sin{\phi} & \frac{v_N \tan{\phi} + v_D}{R_E + h} & \frac{v_E}{R_E + h} + 2\omega_{ie}\cos{\phi} & \frac{v_N v_E}{(R_E+h)\cos^2{\phi}} + 2v_N\omega_{ie}\cos{\phi} - 2v_D\omega_{ie}\sin{\phi} & 0 & -\frac{v_Nv_E\tan{\phi} + v_Ev_D}{(R_E + h)^2} & & \mathbf{C}_b^n & & & \mathbf{0}_3 & \\ {-}f_E & {+}f_N & 0 & -\frac{2v_N}{R_N + h} & -\frac{2v_E}{R_E + h} - 2\omega_{ie}\cos{\phi} & 0 & 2v_E\omega_{ie}\sin{\phi} & 0 & \frac{v_E^2}{(R_E + h)^2} + \frac{v_N^2}{(R_N + h)^2} -\frac{2g_0}{r_{eS}^e} & & & & & & \\ & & & \frac{1}{R_N+h} & 0 & 0 & 0 & 0 & -\frac{v_N}{(R_N + h)^2} & & & & & & \\ & \mathbf{0}_3 & & 0 & \frac{1}{(R_E+h)\cos{\phi}} & 0 & \frac{v_E\tan{\phi}}{(R_E+h)\cos{\phi}} & 0 & -\frac{v_E}{(R_E + h)^2\cos{\phi}} & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & 0 & 0 & -1 & 0 & 0 & 0 & & & & & & \\ & & & & & & & & & & & & & & \\ & \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 & \\ & & & & & & & & & & & & & & \\ \end{bmatrix} \cdot \begin{bmatrix} \delta R \\ \delta P \\ \delta Y \\ \delta v_{N} \\ \delta v_{E} \\ \delta v_{D} \\ \delta \phi \\ \delta \lambda \\ \delta h \\ \delta f_{x}^{b} \\ \delta f_{y}^{b} \\ \delta f_{z}^{b} \\ \delta \omega_{x}^{b} \\ \delta \omega_{y}^{b} \\ \delta \omega_{z}^{b} \\ \end{bmatrix} \end{equation}
As all signs related to the attitude error have a different sign, it can be explained by the attitude error being defined in the opposite direction. To check this, we can look at the definition of the correction of the state with the attitude errors ([17] Groves, ch. 14.1.1, eq. 14.7, p. 564)
\begin{equation} \mathbf{C}_b^n \approx (\mathbf{I}_3 - [\boldsymbol{\delta \psi}_{nb}^n \times]) \mathbf{\hat{C}}_b^n \end{equation}
while Titterton rearranges equation eq-LCKF_n-Derivation-Attitude-estimate-errors and defines the correction as ([45] Titterton, ch. 13.6.2.3, eq. 13.15, p. 407)
\begin{equation} \mathbf{C}_b^n = (\mathbf{I}_3 + [\boldsymbol{\delta \psi} \times]) \mathbf{\hat{C}}_b^n \end{equation}
Therefore the attitude error is defined differently in Groves and Titterton, which explains the sign change in the error equations.
[14] Gleason, ch. 6.2.5, eq. 6.21-6.23, p. 157 define the same error equations as Titterton, but has a bracket mistake inside the velocity equation (bracket should include the '2')
\begin{equation} \begin{aligned} \boldsymbol{\delta} \mathbf{\dot{p}} &= T' \boldsymbol{\delta} \mathbf{p}^n + T \boldsymbol{\delta v}^n \\ \boldsymbol{\delta \dot{v}}^n &= {\left[ (\mathbf{C}_b^n \boldsymbol{f}^b) \times \right]} \boldsymbol{\delta \psi}_{nb}^n {+ \mathbf{C}_b^n} \boldsymbol{\delta f}^b - \left[ { 2 ( }\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \right] \boldsymbol{\delta v}^n - \left[ (2 \boldsymbol{\delta \omega}_{ie}^n + \boldsymbol{\delta \omega}_{en}^n) \times \right] \boldsymbol{v}^n + \boldsymbol{\delta} \mathbf{g}^n \\ \boldsymbol{\delta \dot{\psi}}_{nb}^n &= {- \left[ \boldsymbol{\omega}_{in}^n \times \right]} \boldsymbol{\delta \psi}_{nb}^n + \boldsymbol{\delta \omega}_{in}^n {-\mathbf{C}_b^n} \boldsymbol{\delta \omega}_{ib}^b \end{aligned} \end{equation}
Extracting the equations from [32] Noureldin, ch. 6.1.5, eq. 6.76, p. 219 gives
\begin{equation} \begin{pmatrix} \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta \dot{h} \end{pmatrix} = \begin{pmatrix} 0 & \frac{1}{R_N+h} & 0 \\ \frac{1}{(R_E+h)\cos{\phi}} & 0 & 0 \\ 0 & 0 & 1 \\ \end{pmatrix} \begin{pmatrix} \delta v_E \\ \delta v_N \\ \delta v_U \end{pmatrix} \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{v}_{E} \\ \delta \dot{v}_{N} \\ \delta \dot{v}_{U} \end{pmatrix} = \begin{pmatrix} 0 & f_U & -f_N \\ -f_U & 0 & f_E \\ f_N & -f_E & 0 \\ \end{pmatrix} \begin{pmatrix} \delta P \\ \delta R \\ \delta Y \end{pmatrix} + \mathbf{C}_b^{enu} \boldsymbol{\delta f}^b \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{P} \\ \delta \dot{R} \\ \delta \dot{Y} \end{pmatrix} = \begin{pmatrix} 0 & \frac{1}{R_N + h} & 0 \\ -\frac{1}{R_E + h} & 0 & 0 \\ -\frac{\tan{\phi}}{R_E + h} & 0 & 0 \\ \end{pmatrix} \begin{pmatrix} \delta v_E \\ \delta v_N \\ \delta v_U \end{pmatrix} + \mathbf{C}_b^{enu} \boldsymbol{\delta \omega}_{ib}^b \end{equation}
Reordering roll/pitch and east/north gives
\begin{equation} \begin{pmatrix} \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta \dot{h} \end{pmatrix} = \begin{pmatrix} \frac{1}{R_N+h} & 0 & 0 \\ 0 & \frac{1}{(R_E+h)\cos{\phi}} & 0 \\ 0 & 0 & 1 \\ \end{pmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_U \end{pmatrix} \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{v}_{N} \\ \delta \dot{v}_{E} \\ \delta \dot{v}_{U} \end{pmatrix} = \begin{pmatrix} 0 & -f_U & f_E \\ f_U & 0 & -f_N \\ -f_E & f_N & 0 \\ \end{pmatrix} \begin{pmatrix} \delta R \\ \delta P \\ \delta Y \end{pmatrix} + \mathbf{C}_b^{neu} \boldsymbol{\delta f}^b \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{R} \\ \delta \dot{P} \\ \delta \dot{Y} \end{pmatrix} = \begin{pmatrix} 0 & -\frac{1}{R_E + h} & 0 \\ \frac{1}{R_N + h} & 0 & 0 \\ 0 & -\frac{\tan{\phi}}{R_E + h} & 0 \\ \end{pmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_U \end{pmatrix} + \mathbf{C}_b^{neu} \boldsymbol{\delta \omega}_{ib}^b \end{equation}
In [32] ch. 2.2.6.1, p. 32 Noureldin defines the yaw angle as measured counterclockwise from north which is the opposite measurement direction of the NED convention. Therefore all yaw terms have to flip the sign.
\begin{equation} \begin{pmatrix} \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta \dot{h} \end{pmatrix} = \begin{pmatrix} \frac{1}{R_N+h} & 0 & 0 \\ 0 & \frac{1}{(R_E+h)\cos{\phi}} & 0 \\ 0 & 0 & 1 \\ \end{pmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_U \end{pmatrix} \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{v}_{N} \\ \delta \dot{v}_{E} \\ \delta \dot{v}_{U} \end{pmatrix} = \begin{pmatrix} 0 & -f_U & -f_E \\ f_U & 0 & f_N \\ -f_E & f_N & 0 \\ \end{pmatrix} \begin{pmatrix} \delta R \\ \delta P \\ \delta Y \end{pmatrix} + \mathbf{C}_b^{neu} \boldsymbol{\delta f}^b \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{R} \\ \delta \dot{P} \\ \delta \dot{Y} \end{pmatrix} = \begin{pmatrix} 0 & -\frac{1}{R_E + h} & 0 \\ \frac{1}{R_N + h} & 0 & 0 \\ 0 & \frac{\tan{\phi}}{R_E + h} & 0 \\ \end{pmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_U \end{pmatrix} + \mathbf{C}_b^{neu} \boldsymbol{\delta \omega}_{ib}^b \end{equation}
Next we replace the up component with a down component
\begin{equation} \begin{pmatrix} \delta \dot{\phi} \\ \delta \dot{\lambda} \\ \delta \dot{h} \end{pmatrix} = \begin{pmatrix} \frac{1}{R_N+h} & 0 & 0 \\ 0 & \frac{1}{(R_E+h)\cos{\phi}} & 0 \\ 0 & 0 & -1 \\ \end{pmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \end{pmatrix} \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{v}_{N} \\ \delta \dot{v}_{E} \\ \delta \dot{v}_{D} \end{pmatrix} = \begin{pmatrix} 0 & {+}f_D & {-}f_E \\ {-}f_D & 0 & {+}f_N \\ {+}f_E & {-}f_N & 0 \\ \end{pmatrix} \begin{pmatrix} \delta R \\ \delta P \\ \delta Y \end{pmatrix} {+} \mathbf{C}_b^n \boldsymbol{\delta f}^b \end{equation}
\begin{equation} \begin{pmatrix} \delta \dot{R} \\ \delta \dot{P} \\ \delta \dot{Y} \end{pmatrix} = \begin{pmatrix} 0 & {-}\frac{1}{R_E + h} & 0 \\ {+}\frac{1}{R_N + h} & 0 & 0 \\ 0 & {+}\frac{\tan{\phi}}{R_E + h} & 0 \\ \end{pmatrix} \begin{pmatrix} \delta v_N \\ \delta v_E \\ \delta v_D \end{pmatrix} + \mathbf{C}_b^n \boldsymbol{\delta \omega}_{ib}^b \end{equation}
As in [17] Groves, all signs concerning the attitude errors are now switched comparing to [45] Titterton. This leads to the same assumption, that the attitude errors are defined differently. And this can easily be shown by looking at [32] Noureldin, ch. 6.1.3, eq. 6.44, p. 212
\begin{equation} \mathbf{\hat{C}}_b^n = (\mathbf{I} + \mathbf{\Psi}) \mathbf{C}_b^n \end{equation}
Comparing this with equation eq-LCKF_n-Derivation-Attitude-estimate-errors we can see that the attitude error is defined in the opposite direction.
1.10.0