Partial State Feedback Correction for Smoothing Navigational Parameters

Abstract

When an inertial navigation system regains a valid aiding navigation signal after a long-term outage, normal state feedback correction in the Kalman filter (KF) will cause a large instantaneous jump in the navigation parameters which may severely affect flight control stability. In this paper, we present a partial state feedback correction method in the KF to smooth the navigation parameters to solve this problem. The idea is to partially correct the navigation state in several steps according to the requirement of the guidance and control system. The simplicity of this method lies in the fact that no additional changes of the filter parameters are needed in the algorithm. The capability of residual monitoring and fault detection is the same with normal state feedback correction in the KF. Comparative simulation results show the effectiveness of the proposed method.

Appendix

• 1. $\widehat{X}_{k/k - 1} + \delta \widehat{X}_{k/k - 1 }$ in partial feedback and total feedback is equal.

Proof

Equation (4) can be simplified as

$$ \widehat{X}_{k/k - 1} = \widehat{X}_{k - 1} + \left( {f\left( {\widehat{X}_{k - 1} } \right) + B_{k} u_{k} } \right)\Delta T $$

Since $f(x)$ can be linearized as

$$ f\left( X \right) = f\left( {\widehat{X}} \right) + F\delta X $$

where $F$ is a Jacobian matrix. $\Phi$ in Eq. (5) can be simplified by

$$ \Phi = (I + F\Delta T) $$

Case 1: Partial feedback (PF),

$$ \begin{aligned} \widehat{X}_{k - 1} & = \widehat{X}_{k - 1/k - 2}^{\text{PF}} + D_{k} \delta \widehat{X}_{k - 1}^{r} \\ \widehat{X}_{k/k - 1} + \delta \widehat{X}_{k/k - 1 } & = \widehat{X}_{k - 1} + f\left( {\widehat{X}_{k - 1} + B_{k} u_{k} } \right)\Delta T + (I + F\Delta T)(I - D_{k} )\delta \widehat{X}_{k - 1}^{r} \\ & = \widehat{X}_{k - 1/k - 2}^{\text{PF}} + D_{k} \delta \widehat{X}_{k - 1}^{r} + \left[ {f(\widehat{X}_{k - 1/k - 2}^{\text{PF}} ) + B_{k} u_{k} + FD_{k} \delta \widehat{X}_{k - 1}^{r} } \right]\Delta T \\ & \quad + \left( {I + F\Delta T} \right)\left( {I - D_{k} } \right)\delta \widehat{X}_{k - 1}^{r} \\ & = \widehat{X}_{k - 1/k - 2}^{\text{PF}} + \delta \widehat{X}_{k - 1}^{{r,{\text{PF}}}} + \left[ {f(\widehat{X}_{k - 1/k - 2}^{PF} ) + B_{k} u_{k} } \right]\Delta T \\ \end{aligned} $$

(A.1)

Case 2: Total feedback (TF),

$$ \begin{aligned} \widehat{X}_{k/k - 1} + \delta \widehat{X}_{k/k - 1 } & = \widehat{X}_{k - 1} + f\left( {\widehat{X}_{k - 1} + B_{k} u_{k} } \right)\Delta T \\ & = \widehat{X}_{k - 1/k - 2}^{\text{TF}} + \delta \widehat{X}_{k - 1}^{{r,{\text{TF}}}} + \left[ {f(\widehat{X}_{k - 1/k - 2}^{\text{TF}} + \delta \widehat{X}_{k - 1}^{{r,{\text{TF}}}} ) + B_{k} u_{k} } \right]\Delta T \\ \end{aligned} $$

(A.2)

The sum of first two components in (A.1) and (A.2) is equal which are estimated as best as possible in the KF measurement update. Very little difference exists in the third part, since very short time interval is multiplied here. So, (A.1) and (A.2) can be thought as equal.

• 2. ‘fake’ state covariance matrix $P_{k}^{f}$ Calculation

Proof

Here we define ‘fake’ error of error state estimation $\delta \widetilde{X}_{k}$. Based on result of Eq. (23),

$$ \begin{aligned} \delta \widehat{X}_{k} & = \delta X_{k} - D_{k} \delta \widehat{X}_{k} = \delta X_{k} - D_{k} \left[ {\delta \widehat{X}_{k/k - 1 } + K_{k} (H_{k} \delta \widetilde{X}_{k/k - 1 } + v_{k} )} \right] \\ & = (I - D_{k} K_{k} H_{k} )\delta \widetilde{X}_{k/k - 1 } + (I - D_{k} ) \delta \widehat{X}_{k/k - 1 } - D_{k} K_{k} v_{k} \\ \end{aligned} $$

We know $\delta \widehat{X}_{k/k - 1}$ is the estimate based on the measurement at and before time $k - 1$, and $v_{k}$ is the measurement noise at time $k$. So $v_{k}$ is uncorrelated with $\delta \widetilde{X}_{k/k - 1}$ which equals to $\delta X_{k} - \delta \widehat{X}_{k/k - 1 }$. Therefore, we have

$$ \begin{aligned} P_{k}^{f} & = E\left[ {\delta \widetilde{X}_{k} \delta \widetilde{X}_{k}^{\text{T}} } \right] \\ & = \left( {I - D_{k} K_{k} H_{k} } \right)P_{k/k - 1} \left( {I - D_{k} K_{k} H_{k} } \right)^{\text{T}} \\ & \quad + \left( {I - D_{k} } \right)\left( {\widehat{X}_{k/k - 1} \widehat{X}_{k/k - 1}^{T} } \right)\left( {I - D_{k} } \right)^{\text{T}} + D_{k} K_{k} R_{k} K_{k}^{\text{T}} D_{k}^{\text{T}} \\ \end{aligned} $$