Innovation Adaptive Estimation Based on Inverse Wishart Distribution

For most INS integration applications the extended Kalman filter is the algorithm of choice. For the general application, a schematic overview of the algorithm is provided in Figure 1. From this figure it can be seen that the Kalman filter has two main steps: prediction and measurement update. In addition to the two main steps, the initialization of the Q, R matrices is incredible important for system convergence.

For most applications, the Kalman filter’s system noise covariance matrix, Q , and measurement noise covariance matrix, R , are determined during the development phase by laboratory measurements of the system, simulation and trials. However, there are some cases where this cannot be done. For example, if an INS/GNSS integration algorithm or INS calibration algorithm is designed for use with a range of different inertial sensors, the system noise covariance will not be known in advance of operation. Similarly, if a transfer alignment algorithm (Section 15.1) is designed for use on different aircraft and weapon stores without prior knowledge of the flexure and vibration environment, the measurement noise covariance will not be known in advance. ** REF :: Paul D. Groves Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems

Figure 1 :: General Schematic of Extended Kalman Filter.

The traditional method to adaptively tune the Q and R matrices is known as Innovation Adaptive Estimation. The first step in this method is to calculate the measurement innovation covariance for the last n measurements, as show below.

$C_K = \frac{1}{n} \sum_{j=k-n}^k \delta z_j \delta z_j^T$

With this covariance estimate, the Q and R matricies can be updated.

$Q_k = K_k C_k K_K^T$ $R_k = C_k - H_k P_K^- H_K^T$

Extension to Traditional IAE

Today, Clark proposed the idea of utilizing a similar approach to the one were implementing for robust pose-graph optimization to adaptively update the Q, R matrices? This approach will utilize the Gaussian Inverse Wishart distribution to estimate the innovation covariance matrix. The Gaussian Inverse Wishart distribution is defined below,

$GIW(\mu,\Sigma | m_o, \kappa_o, \nu_o, S_o) := \mathcal{N}(\mu | m_o, \frac{1}{\kappa_o}\Sigma) IW(\Sigma| S_o,\nu_o) ,$

which can be represented as,

$GIW(\mu,\Sigma | m_o, \kappa_o, \nu_o, S_o) = e^{-\frac{\kappa_o}{2}|| \mu - m_o||_{\Sigma}} e^{-\frac{1}{2} tr(\Sigma^{-1}S_o)} \frac{1}{Z_{GIW}(D,\kappa_o,\nu_o,S_o} |\Sigma|^{-\frac{\nu_o+D+2}{2}},$

where

$Z_{GIW}(D,\kappa_o,\nu_o,S_o) = 2^{\frac{(\nu_o+1)D}{2}}\pi^{\frac{D(D+1)}{4}}\kappa_o^{-D/2} \|S_o\|^{-\nu_o/2} \prod_{d=1}^{D} \Gamma(\frac{\nu_o+1-d}{2})$

Parameter Definition

$m_o$ –> Prior mean for $\mu$

$\kappa_o$ –> belief in $m_o$

$S_o$ –> prior $\Sigma$

$\nu_o$ –> belief in $S_o$

Extension to Traditional IAE

Parameter Definition

Related Posts: