 |
0.2.0
|
|
0.2.0
|
\begin{equation} \begin{bmatrix} \boldsymbol{\delta \dot{\psi}}_{eb}^e \\ \boldsymbol{\delta \dot{v}}_{eb}^e \\ \boldsymbol{\delta \dot{r}}_{eb}^e \\ \boldsymbol{\delta \dot{f}}_{ib}^{\raise-0.45ex\hbox{$\scriptstyle b$}} \\ \boldsymbol{\delta \dot{\omega}}_{ib}^b \\ \end{bmatrix} = \begin{bmatrix} \mathbf{F}_{\dot{\psi},\psi}^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{C}_b^e \\ \mathbf{F}_{\delta \dot{v},\psi}^e & \mathbf{F}_{\delta \dot{v},\delta v}^e & \mathbf{F}_{\delta \dot{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 \\ \end{bmatrix} \cdot \begin{bmatrix} \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 \\ \end{bmatrix} \end{equation}
\begin{equation} \begin{bmatrix} \delta \dot{\psi}_{eb,1}^e \\ \delta \dot{\psi}_{eb,2}^e \\ \delta \dot{\psi}_{eb,3}^e \\ \hdashline \delta \dot{v}_{eb,x}^e \\ \delta \dot{v}_{eb,y}^e \\ \delta \dot{v}_{eb,z}^e \\ \hdashline \delta \dot{r}_{eb,x}^e \\ \delta \dot{r}_{eb,y}^e \\ \delta \dot{r}_{eb,z}^e \\ \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_{ie} & 0 & & & & & & & & & & & & \\ -\omega_{ie} & 0 & 0 & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{C}_b^e & \\ 0 & 0 & 0 & & & & & & & & & & & & \\ \hdashline 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 & & & & & & & & & \\ \hdashline & & & & & & & & & & & & & & \\ & \mathbf{0}_3 & & & \mathbf{I}_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 & \\ & & & & & & & & & & & & & & \\ \hdashline & & & & & & & & & & & & & & \\ & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & & & \mathbf{0}_3 & \\ & & & & & & & & & & & & & & \\ \end{array} \right] \cdot \begin{bmatrix} \delta \psi_{eb,1}^e \\ \delta \psi_{eb,2}^e \\ \delta \psi_{eb,3}^e \\ \hdashline \delta v_{eb,x}^e \\ \delta v_{eb,y}^e \\ \delta v_{eb,z}^e \\ \hdashline \delta r_{eb,x}^e \\ \delta r_{eb,y}^e \\ \delta r_{eb,z}^e \\ \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 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}}_{eb}^e \\ \boldsymbol{\delta \dot{v}}_{eb}^e \\ \boldsymbol{\delta \dot{r}}_{eb}^e \\ \boldsymbol{\delta \dot{f}}_{ib}^{\raise-0.45ex\hbox{$\scriptstyle b$}} \\ \boldsymbol{\delta \dot{\omega}}_{ib}^b \\ \end{bmatrix} = \begin{bmatrix} \mathbf{F}_{\dot{\psi},\psi}^e & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{C}_b^e \\ \mathbf{F}_{\delta \dot{v},\psi}^e & \mathbf{F}_{\delta \dot{v},\delta v}^e & \mathbf{F}_{\delta \dot{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 \\ \end{bmatrix} \cdot \begin{bmatrix} \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 \\ \end{bmatrix} + \begin{bmatrix} \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 \\ \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_e-random-walk-S-noise and eq-LCKF_e-random-walk-S-bias or eq-LCKF_e-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_e-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}
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^e}}^2 & & & \\ \vdots & & \boldsymbol{\sigma}_{\boldsymbol{\delta r^e}}^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}^e = \begin{pmatrix} \tilde{\mathbf{r}}_{b,G}^e - \hat{\mathbf{r}}_{b}^e - \mathbf{C}_b^e \mathbf{l}_{ba}^b \\ \tilde{\mathbf{v}}^e_{G} - \hat{\mathbf{v}}^e - \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 \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}^e = \begin{bmatrix} \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 \\ \end{bmatrix} \end{equation}
\begin{equation} \begin{aligned} {\mathbf{H}_{r,\delta \psi}^e} &\approx \left[ (\mathbf{C}_b^e \mathbf{l}_{ba}^b) \times \right] \\ {\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] \\ {\mathbf{H}_{v,\omega_{ib}^b}^e} &\approx \mathbf{C}_b^e \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} \boldsymbol{\sigma}^2_{\mathbf{z}_{\boldsymbol{\delta r}^e}} & \mathbf{0}_3 \\ \mathbf{0}_3 & \boldsymbol{\sigma}^2_{\mathbf{z}_{\boldsymbol{\delta v}^e}} \\ \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.9, p. 564)
\begin{equation} \begin{aligned} \boldsymbol{r}^{e,+} &= \boldsymbol{r}^{e,-} - \boldsymbol{\delta r}^{e} \\ \boldsymbol{v}^{e,+} &= \boldsymbol{v}^{e,-} - \boldsymbol{\delta v}^{e} \end{aligned} \end{equation}
The attitude correction can be derived from equation eq-LCKF_e-Derivation-Attitude-estimate-errors (see [17] Groves, ch. 14.1.1, eq. 14.7, p. 564)
\begin{equation} \mathbf{C}_b^{e,+} = (\mathbf{I}_3 - [\boldsymbol{\delta \psi} \times]) \mathbf{C}_b^{e,-} \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 time derivative of the position is simply a function of the Earth-referenced velocity in Earth frame axes
\begin{equation} \mathbf{\dot{r}}_{eb}^e = \mathbf{v}_{eb}^e \end{equation}
see
The time derivative of the position error thus is
\begin{equation} \boldsymbol{\delta}\mathbf{\dot{r}}_{eb}^e = {\mathbf{I}_3} \cdot \mathbf{v}_{eb}^e \end{equation}
see
The estimated attitude \( \mathbf{\hat{C}}_b^e \) can be written in terms of true direction cosine matrix \( \mathbf{C}_b^e \)
\begin{equation} \mathbf{\hat{C}}_b^e = \mathbf{B} \mathbf{C}_b^e \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.
Substituting eq-LCKF_e-Derivation-Attitude-transformation into eq-LCKF_e-Derivation-Attitude-estimate-true gives
\begin{equation} \mathbf{\hat{C}}_b^e \approx [\mathbf{I} - \mathbf{\Psi}] \mathbf{C}_b^e \end{equation}
and solving for \( \mathbf{\Psi} \)
\begin{equation} \mathbf{\Psi} = \mathbf{I} - \mathbf{\hat{C}}_b^e {\mathbf{C}_b^e}^T \end{equation}
Differentiating this equation yields:
\begin{equation} \mathbf{\dot{\Psi}} = -\mathbf{\dot{\hat{C}}}_b^{\raise-1.25ex\hbox{$\scriptstyle e$}} {\mathbf{C}_b^e}^T - \mathbf{\hat{C}}_b^e {\mathbf{\dot{C}}_b^e}\raise1.25ex\hbox{$\scriptstyle T$} \end{equation}
Using the following equations for the transformation
the time derivative of the DCM matrix is
\begin{equation} \begin{aligned} \mathbf{\dot{C}}_b^e &= \mathbf{C}_b^e \boldsymbol{\Omega}_{eb}^b \\ &= \mathbf{C}_b^e \boldsymbol{\Omega}_{ib}^b - \boldsymbol{\Omega}_{ie}^e \mathbf{C}_b^e \end{aligned} \end{equation}
see
where
Similarly, the time time differential of the estimated matrix \( \mathbf{\dot{\hat{C}}}_b^{\raise-1.25ex\hbox{$\scriptstyle e$}} \) can be calculated:
\begin{equation} \mathbf{\dot{\hat{C}}}_b^{\raise-1.25ex\hbox{$\scriptstyle e$}} = \mathbf{\hat{C}}_b^e \mathbf{\hat{\Omega}}_{ib}^b - \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\hat{C}}_b^e \end{equation}
Substituting eq-LCKF_e-Derivation-Attitude-DCM-dot and eq-LCKF_e-Derivation-Attitude-DCM-dot-estimated into eq-LCKF_e-Derivation-Attitude-skew-mat-dot
\begin{equation} \begin{aligned} \mathbf{\dot{\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}
Substituting eq-LCKF_e-Derivation-Attitude-estimate-errors into eq-LCKF_e-Derivation-Attitude-skew-mat-dot-calc1 gives:
\begin{equation} \begin{aligned} \mathbf{\dot{\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}
Now we make the following substitutions
and neglect error product terms. This leads to
\begin{equation} \mathbf{\dot{\Psi}} \approx \mathbf{\Psi} \mathbf{\Omega}_{ie}^e - \mathbf{\hat{\Omega}}_{ie}^e \mathbf{\Psi} + \boldsymbol{\delta}\mathbf{\Omega}_{ie}^e - \mathbf{C}_b^e \boldsymbol{\delta}\mathbf{\Omega}_{ib}^b {\mathbf{C}_b^e}^T \end{equation}
Finally by performing an element by element comparison with
\begin{equation} \boldsymbol{\psi} \times = \mathbf{\Psi} \qquad\quad \boldsymbol{\omega}_{ie}^e \times = \mathbf{\Omega}_{ie}^e \qquad\quad \boldsymbol{\delta \omega}_{ie}^e \times = \boldsymbol{\delta}\mathbf{\Omega}_{ie}^e \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{\dot{\psi}} &\approx {-\boldsymbol{\omega}_{ie}^e \times} \boldsymbol{\psi} + \overbrace{\boldsymbol{\delta \omega}_{ie}^e}^{= 0} {- \mathbf{C}_b^e} \boldsymbol{\delta \omega}_{ib}^b \\ &= {\begin{pmatrix} 0 & \omega_{ie} & 0 \\ -\omega_{ie} & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix}} \cdot \boldsymbol{\psi} {- \mathbf{C}_b^e} \boldsymbol{\delta \omega}_{ib}^b \\ \end{aligned} \end{equation}
The time derivative of the velocity in Earth frame coordinates is
\begin{equation} \boldsymbol{\dot{v}}^e = \overbrace{\boldsymbol{f}^e}^{\text{measured}} -\ \underbrace{2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e}_{\text{coriolis acceleration}} +\ \overbrace{\mathbf{g}^e}^{\text{gravity}} \end{equation}
see
where
The estimated velocity follows the same propagation
\begin{equation} \boldsymbol{\dot{\hat{v}}}^{\raise-0.45ex\hbox{$\scriptstyle e$}} = \boldsymbol{\hat{f}}^e - 2 \boldsymbol{\hat{\omega}}_{ie}^e \times \boldsymbol{\hat{v}}_e^e + \mathbf{\hat{g}}^e \end{equation}
Differencing equation eq-LCKF_e-Derivation-Velocity-timeDerivative and eq-LCKF_e-Derivation-Velocity-timeDerivative-estimate we get
\begin{equation} \begin{aligned} \boldsymbol{\delta} \boldsymbol{\dot{v}}^e &= \boldsymbol{\dot{\hat{v}}}^{\raise-0.45ex\hbox{$\scriptstyle e$}} - \boldsymbol{\dot{v}}^e \\ &= \mathbf{\hat{C}}_b^e \boldsymbol{\hat{f}}^b - \mathbf{C}_b^e \boldsymbol{f}^b - 2 \boldsymbol{\hat{\omega}}_{ie}^e \times \boldsymbol{\hat{v}}^e + 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e + \mathbf{\hat{g}}^e - \mathbf{g}^e \end{aligned} \end{equation}
\begin{equation} \begin{aligned} \boldsymbol{\delta \dot{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 + {\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} + {\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}
Now neglecting error product terms ( \( \boldsymbol{\delta f}^b \times \boldsymbol{\psi} \approx 0 \))
\begin{equation} \renewcommand*{\arraystretch}{1.9} \boldsymbol{\delta \dot{v}}^e \approx {\boldsymbol{f}^e \times} \boldsymbol{\psi} + {\mathbf{C}_b^e} \boldsymbol{\delta f}^b {- 2 \boldsymbol{\omega}_{ie}^e \times} \boldsymbol{\delta v}^e + \boldsymbol{\delta} \mathbf{g}^e \end{equation}
The gravity error can be expressed as (see [17] Groves, ch. 14.2.3, eq. 14.45, p. 583)
\begin{equation} \begin{aligned} \boldsymbol{\delta} \mathbf{g}^e = \overbrace{\boldsymbol{\hat{\gamma}}_{ib}^e - \boldsymbol{\gamma}_{ib}^e}^{\text{gravitation error}} - \underbrace{\boldsymbol{\Omega}_{ie}^e \boldsymbol{\Omega}_{ie}^e \boldsymbol{\delta} \mathbf{r}_{eb}^e}_{\text{centrifugal acceleration error}} \approx - \frac{2 \boldsymbol{\gamma}_{ib}^e}{r_{eS}^e} \frac{{\mathbf{r}_{eb}^e}^T}{\left| \mathbf{r}_{eb}^e \right|} \boldsymbol{\delta} \mathbf{r}_{eb}^e - \boldsymbol{\Omega}_{ie}^e \boldsymbol{\Omega}_{ie}^e \boldsymbol{\delta} \mathbf{r}_{eb}^e \end{aligned} \end{equation}
where
\begin{equation} \renewcommand*{\arraystretch}{1.9} \boldsymbol{\delta \dot{v}}^e \approx {\boldsymbol{f}^e \times} \boldsymbol{\psi} + {\mathbf{C}_b^e} \boldsymbol{\delta f}^b + {\begin{pmatrix} 0 & 2 \omega_{ie} & \\ -2 \omega_{ie} & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix}} \boldsymbol{\delta v}^e {- \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}
1.10.0