The Iterated Kalman Filter Update as a Gauss-Newton Method（迭代卡尔曼滤波器）

The Iterated Kalman Filter Update as a Gauss-Newton Method

Iterated Kalman filter(IKF)是应用高斯牛顿迭代法改良Extended Kalman filter(EKF)的方法，它牺牲掉少量计算时间，通过引入非线性优化去寻找状态后验的最大似然，因而能够解决EKF线性化近似时产生的近似误差的缺点，从而在很大程度上交EKF更精确，效果更好。

符号定义与问题模型

当前系统误差：$x\in R^n$

当前系统状态预测：$\hat{x}\in R^n$，$\hat{x}\sim N(x,P)$，其中$P$是该状态对应协方差矩阵

系统状态观测：$z\in R^n$，$z\sim N(h(x),R)$，其中$R$是观测模型噪声协方差矩阵

最优系统状态估计：${\hat{x}}^{+}$，${\hat{x}}^{+}\sim N(x,P^{+})$

观测模型：$h:R^n\to R^m$，$h(x)$含有随机噪声$\xi \in R^n$，其一阶导数：$h^{\prime }$

问题模型：已知$z,\hat{x},R,P$，寻找最优状态估计${\hat{x}}^{+}$及其对应的协方差$P^{+}$。

IKF预测模型

IKF的预测与EKF几乎相同~

本方法应用于离散时间系统，因此设系统处于的第$i$个更新周期内的最优状态估计及其协方差模为
$${\hat{x}}^{+}=x_i,P^{+}=P_i$$
预测：
$$x_0=\hat{x}=f(x_{i-1})=f({\hat{x}}_{last}^{+})\text{\hspace{1em}(1)}$$
记$F$为预测模型$f(x)$的$jacobian$，$Q$为过程噪声的协方差矩阵，
$$P_0={\hat{P}}^{+}=F^T{P}_{i-1}F+Q=F^T{P}_{last}^{+}F+Q\text{\hspace{1em}(2)}$$

IKF测量更新

构建新的观测量和测量函数：
$$Z=\left[\begin{array}{c}z\\ \hat{x}\end{array}\right],g(x)=\left[\begin{array}{c}h(x)\\ x\end{array}\right]$$
其中
$$Z\sim N(g(x),Q)whereQ=\left[\begin{array}{cc}R& 0\\ 0& P\end{array}\right]\text{\hspace{1em}(3)}$$
原本的问题模型变成：已知$Z,Q,g$，寻找最优状态估计${\hat{x}}^{+}$及其对应的协方差$P^{+}$

定义$Z$的似然域函数$L(\xi )$，其中$Z$里头的$x$被替换成$\xi$表达：
$$L(\xi )=\frac{1}{\sqrt{(2\pi {)}^{m+n}}|Q|}\cdot exp(-\frac{1}{2}(Z-g(\xi ){)}^T{Q}^{-1}(Z-g(\xi )))\text{\hspace{1em}(4)}$$
$x$的最大似然估计及其对应协方差可表达为：
$$\begin{gathered} {\hat{x}}^{+}=argmax(L(\xi )) \\ {P}^{+}=(G^T{Q}^{-1}G{)}^{-1}whereG=g^{\prime }({\hat{x}}^{+})\text{\hspace{1em}(5)} \end{gathered}$$
通常，对于公式$L(\xi )$，我们经常取负对数将最大似然问题转换为求最小值的问题，简化后我们需要最小化的函数设为$q(\xi )$：
$$q(\xi )=\frac{1}{2}(Z-g(\xi ){)}^T{Q}^{-1}(Z-g(\xi ))\text{\hspace{1em}(6)}$$
$x$的最大似然估计及其对应协方差可表达为：
$${\hat{x}}^{+}=argmin(q(\xi ))\text{\hspace{1em}(7)}$$
将公式（7）求导并等于0就可以得到最优的状态量所满足的条件：
$$0=g^{\prime }(\xi {)}^T{Q}^{-1}(Z-g(\xi ))\text{\hspace{1em}(8)}$$
根据上面的推导，公式（6）就是整个问题的优化目标函数，下面的内容主要是通过使用Gauss-Newton方法对目标函数进行优化，推导得到IKF（IEKF）的结论。

对于目标函数（6），构建非线性最小二乘问题：
$$minimizef(\xi )=\frac{1}{2}||r(\xi )|{|}^2\text{\hspace{1em}(9)}$$
为了求解最小二乘问题，定义如下符号：
$$\begin{gathered} S,S\ast S=Q^{-1} \\ r(x)=S(Z-g(x)),r：R^n\to R^{n+m} \\ \text{\hspace{1em}(10)} \end{gathered}$$
$r(x)$的$jacobian$可表示为：
$$r^{\prime }(x)=-S{g}^{\prime }(x)\text{\hspace{1em}(11)}$$
那么目标函数（6）可重写为：
$$q(x)=\frac{1}{2}||r(x)|{|}^2\text{\hspace{1em}(12)}$$
G-N迭代法求解：
$$\begin{gathered} x_{i+1}=x_i+(g^{\prime }(x_i{)}^T{S}^{T}S{g}^{\prime }(x_i){)}^{-1}{g}^{\prime }(x_i{)}^T{S}^{T}S(Z-g(x_i)) \\ =(G_i^T{Q}^{-1}{G}_i{)}^{-1}{G}_i^T{Q}^{-1}(Z-g(x_i)+G_i{x}_i)\text{\hspace{1em}(13)} \end{gathered}$$
其中$G_i=g^{\prime }(x_i)$，

经过一系列骚操作公式，可以得到卡尔曼增益$K_i，K_i=K(x_i)$：
$$K_i=(H_i^T{R}^{-1}{H}_i+P^{-1}{)}^{-1}{H}_i^T{R}^{-1}\text{\hspace{1em}(14)}$$
接着也是经过骚操作公式推导，公式（13）的$x_{i+1}$也可以改写：
$$x_{i+1}=\hat{x}+K_i(z-h(x_i)-H_i(\hat{x}-x_i))\text{\hspace{1em}(15)}$$
当$\Delta x=x_{i+1}-x_i$足够小，也就是迭代收敛时，即得到最大似然${\hat{x}}^{+}=x_{i+1}$，同样其对于的卡尔曼增益最优解为$K^{+}=K_{i+1}$

得到卡尔曼增益$K$的最优解后，即可推导出对于的协方差矩阵：
$$P^{+}=(I-KH)P$$
值得一提的是，$P$在参与G-N优化时并没有进行更新，始终是上一周期IKF的输出状态协方差，$P^{+}$是在完成G-N优化后才通过最优的$K^{+}$推算得出，并进行更新。

如此，系统的测量更新形成了迭代：

否

否

是

给定系统初值

根据系统模型预测更新

构建目标优化函数q

通过G-N法迭代

x_i+1 - x_i是否足够小

判断是否发散

收敛,结束迭代

得到系统最优估计状态及其协方差

IEKF流程

参考资料：

Bell B M , Cathey F W . The iterated Kalman filter update as a Gauss-Newton method[J]. IEEE Transactions on Automatic Control, 1993, 38(2):294-297.

迭代扩展卡尔曼滤波(IEKF)

Iterated Kalman Filter(IKF/IEKF)总结