卡尔曼滤波公式推导

卡尔曼滤波公式推导

1.运动方程及观测方程

已知时刻$k-1$位置状态向量为$X_{k-1}$，下一时刻$k$位置状态向量为$X_k$，两者之间的转换关系如下:
$$X_k=F_k{X}_{k-1}+n_m$$
其中，$F_k$表示从$k-1$时刻到$k$时刻之间位置状态向量的转移关系，$n_m$表示运动模型噪声，假设服从于正态分布$N(0,Q)$

此外，在不同的位置观测到不同的路标点$Z_k$，因此观测方程关系如下：
$$Z_k=H_k{X}_k+n_w$$
其中，$H_x$表示根据位置向量生成观测$Z_k$的观测矩阵，$n_w$表示观测噪声，假设服从于正态分布$N(0,R)$

联立运动方程和观测方程可得
$$\begin{gathered} X_k=F_k{X}_{k-1}+n_m \\ {Z}_k=H_k{X}_k+n_w \end{gathered}$$

需要注意的是，此处方程都是线性系统的表达形式，实际问题中，$F_k$和$H_k$应当是关于$X$的非线性函数，对于这种非线性系统，需要做线性化，即扩展卡尔曼滤波

2.目标函数构建

卡尔曼滤波实际上可以看做是优化问题的迭代求解，因此，以优化问题的角度看，应当先构建优化目标函数。

从结果来看，我们想要求解出满足运动方程和观测方程的最优位置状态向量$X_k^{\ast }$，因此，若假设$X_k^{-}$表示基于运动方程得到的预测位置，$X_k^{+}$表示基于观测方程得到的观测位置，那么，由于噪声不可测量，先忽略运动方程与观测方程之间的噪声，理想情况下目标函数应该是
$$L(X_k)=(X_k-(X_k^{-}+X_k^{+})/2{)}^T(X_k-(X_k^{-}+X_k^{+})/2)$$
上式是个简单的最小二乘问题，可以求出解析解为
$${\hat{X}}_k=(X_k^{-}+X_k^{+})/2$$
其中，$X_k^{-}=F_k{X}_{k-1}$，$X_k^{+}=H_k^{-1}{Z}_k$

显然，理想情况下最优结果是多个观测结果的均值，但在实际问题中，必然存在着噪声，各个观测之间的不确定性是不同的，因此优化估计得到的位置状态向量应该写成如下形式
$${\hat{X}}_k=X_k^{-}+G_k(X_k^{+}-X_k^{-})$$
其中，权重因子$G_k$取值范围从0到1，趋于0时更相信运动方程预测的结果，趋于1时更相信观测方程的结果

进一步地，需要确定权重因子$G_k$的值；显然，最优的权重因子应该使得估计位置状态向量${\hat{X}}_k$最接近真实位置状态向量$X_k$，即构造目标函数如下：
$$L(G_k)=E((X_k-{\hat{X}}_k{)}^T(X_k-{\hat{X}}_k))=E(e_k^T{e}_k)$$
其中，
$$\begin{gathered} e_k=X_k-{\hat{X}}_k \\ =X_k-X_k^{-}-G_k(X_k^{+}-X_k^{-}) \\ =X_k-（I-G_k）X_k^{-1}-G_k{H}_k^{-1}{Z}_k \\ =X_k-(I-G_k)X_k^{-1}-G_k{X}_k-G_k{H}_k^{-1}{n}_w \\ =(I-G_k)(X_k-X_k^{-1})-G_k{H}_k^{-1}{n}_w \end{gathered}$$
即，将上式代入目标函数可写成
$$\begin{gathered} L(G_k)=E(e_k^T{e}_k) \\ =E\{[(I-G_k)(X_k-X_k^{-1}){]}^T[(I-G_k)(X_k-X_k^{-1})]\}-E\{[(I-G_k)(X_k-X_k^{-1}){]}^T{G}_k{H}_k^{-1}{n}_w\} \\ -E\{[G_k{H}_k^{-1}{n}_w{]}^T[(I-G_k)(X_k-X_k^{-1})]\}+E\{[G_k{H}_k^{-1}{n}_w{]}^T{G}_k{H}_k^{-1}{n}_w\} \\ =(I-G_k)E({e_k^{-}}^T{e}_k^{-})(I-G_k{)}^T+(G_k{H}_k^{-1})E(n_w^T{n}_w)(G_k{H}_k^{-1}{)}^T \\ =G_k[E({e_k^{-}}^T{e}_k^{-})+H_k^{-1}E(n_w^T{n}_w){H_k^{-1}}^T]G_k^T-2G_{k}E({e_k^{-}}^T{e}_k^{-})+E({e_k^{-}}^T{e}_k^{-}) \end{gathered}$$
其中，$e_k^{-}$实质上可以表示$X_k^{-}$协方差；第三个等号处，中间两项包含噪声的期望都为0，可以消掉

3.目标函数求解

对目标函数等式(9)求导可得
$$\frac{\partial L}{\partial G_k}=2G_K[E({e_k^{-}}^T{e}_k^{-})+H_k^{-1}E(n_w^T{n}_w){H_k^{-1}}^T)]-2E^T({e_k^{-}}^T{e}_k^{-})$$
令导数为0可得，
$$\begin{gathered} G_K=[E({e_k^{-}}^T{e}_k^{-})+H_k^{-1}E(n_w^T{n}_w){H_k^{-1}}^T){]}^{-1}{E}^T({e_k^{-}}^T{e}_k^{-}) \\ =\frac{P_k^{-}}{P_k^{-}+H_k^{-1}R{H_k^{-1}}^T} \end{gathered}$$
其中，$P_k$表示预测公式推导出协方差，$R$表示噪声$n_w$的协方差，由此，求解出权重因子$G_k$

需要注意的是，$G_k$即常说的卡尔曼增益因子，这里形式与常规公式不一致，是因为等式(6)中将$H_k$放到括号里面，实质上是一致的

为了和一般公式一致，对等式(6)和等式(11)进行改写
$$\begin{gathered} {\hat{X}}_k=X_k^{-}+G_k(Z_k-H_k{X}_k^{-}) \\ {G}_k=\frac{P_k^{-}{H}^T}{H_k{P}_k^{-}{H}_k^T+R} \end{gathered}$$

整理可得，

• 预测方程:
  $$\begin{gathered} X_k^{-}=F_k{\hat{X}}_{k-1} \\ {P}_k^{-}=F_k{\hat{P}}_{k-1}{F}_k^T+Q \end{gathered}$$

• 卡尔曼增益:
  $$G_k=\frac{P_k^{-}{H}_k^T}{H{P}_k^{-}{H}^T+R}$$

• 更新方程:
  $$\begin{gathered} {\hat{X}}_k=X_k^{-}+G_k(Z_k-H_k{X}_k^{-}) \\ {\hat{P}}_k=(I-G_k{H}_k)P_k^{-} \end{gathered}$$

4.实际使用经验

• 初值怎么给
• 噪声方差如何确定