卡尔曼滤波推导思路总结

推导思路一：
(1) 混合高斯
一维高斯函数形式：
$\mathcal N(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^-\frac{(x-\mu)^2}{2\sigma^2}\tag1$

两个高斯函数相乘（未归一化）：
$\mathcal N(x,\mu_{0},\sigma_{0})\times\mathcal N(x,\mu_{1},\sigma_{1})=^?\mathcal N(x,\mu{'},\sigma{'})\tag2$

混合后高斯函数其均值和方差求解如下：

$\begin{aligned} \mu{'}&=\mu_{0}+\frac{\sigma_{0}^{2}(\mu_{1}-\mu_{0})}{\sigma_{0}^{2}+\sigma_{1}^{2}}\\ \sigma{'}^2&=\sigma_{0}^2-\frac{\sigma_{0}^4}{\sigma_{0}^{2}+\sigma_{1}^{2}} \end{aligned}\tag3$

则上式可简化如下：
$\begin{aligned} k&=\frac{\sigma_{0}^2}{\sigma_{0}^{2}+\sigma_{1}^{2}}\\ \mu{'}&=\mu_{0}+k{(\mu_{1}-\mu_{0})}\\ \sigma{'}^2&=\sigma_{0}^2-k\sigma_{0}^2 \end{aligned}\tag4$

将上式改写为矩阵形式：
$\begin{aligned} K&=\Sigma_{0}(\Sigma_{0}+\Sigma_1)^{-1}\\ \vec{\mu}{'}&=\vec{u_0}+ K(\vec{\mu_1}-\vec{u_{0}})\\ \Sigma{'}&=\Sigma_{0}-K\Sigma_{0} \end{aligned}\tag{5}$

其中 $\Sigma_{i}$ 表示协方差矩阵， $\vec{\mu_{i}}$ 表示均值。 $K$ 表示卡尔曼增益。

(2)卡尔曼滤波
假定现有两个分布，一个是预测分布和观测分布：
$\begin{aligned} (\mu_0,\Sigma_{0})&=(H_{k}\hat{x_{k}}，H_kP_kH_{k}^{K})\\ (\mu_1,\Sigma_1)&=(\vec{z_{k}},R_{k}) \end{aligned}\tag6$
结合公式5，可得如下：
$\begin{aligned} H_k\vec{x_k{'}}&=H_k\vec{x_k}+K(\vec{z_k}-H_k \hat {x_k})\\ H_kP_k{'}H_k^{T}&=H_kP_kH_k^{T}-KH_kP_kH_k^{T} \end{aligned}\tag7$

卡尔曼增益如下：
$K=H_kP_kH_k^{T}(H_kP_kH_k^{T}+R_k)^{-1}\tag8$

式(7)、式(8)等式左右约去 $H_k$ ，注意约去 $K$ 中隐藏的 $H_k$ ，可将式(7)、式(8)写为如下：

$\begin{aligned} \vec{x}_k{'}&=\vec{x_k}+K{'}(\vec{z_k}-H_k \hat {x_k})\\ P_k{'}&=P_k-K{'}H_kP_k\\ K&=P_kH_k^{T}(H_kP_kH_k^{T}+R_k)^{-1} \end{aligned}\tag9$

扫描二维码关注公众号，回复： 10368876 查看本文章

式(9)便给了我们完整的更新步骤。

同时，我们附上一下的两个公式：
$\begin{aligned} \hat x_{k}&=F_k\hat{x}_{k-1}+B_k\vec\mu_k\\ P_k&=F_kP_{k-1}F_k^{T}+ Q_k \end{aligned}\tag{10}$

以上便是经典卡尔曼滤波中相关的5个公式。式(9)和式(10)中部分定义如下：

$H_k$ 为转换矩阵
$B_k$ 为控制矩阵
$R_k$ 为测量误差
$Q_k$ 为系统误差
$\vec\mu_k$ 为控制向量

流程图如下：

推导思路一细节参见：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-update

推导思路二：
由博文可知有预测方程和测量方程如下：
$\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k=Hx_k+v_k \end{cases}\tag1$

公式1中， $A$ 表示状态转换矩阵， $B$ 表示控制矩阵， $H$ 也表示转换矩阵， $x_k$ 表示预测值， $z_k$ 表示测量值， $u_k$ 表示控制向量， $w_{k-1}$ 表示系统噪声， $v_k$ 表示测量噪声。

针对小车加速运行问题，上式可表示为：

$\begin{cases} \begin{bmatrix} x_t\\ \dot x_t\end{bmatrix}=\begin{bmatrix}1&\Delta t \\0&1\end{bmatrix}\begin{bmatrix} x_{t-1} \\ \dot x_{t-1}\end{bmatrix}+\begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix}\times \alpha\\ z_k=\begin{bmatrix} 1&0\end{bmatrix}\begin{bmatrix} x_k\\ \dot x_k\end{bmatrix} \end{cases}\tag2$

公式2中 $\alpha$ 表示加速度。

假定系统噪声 $w_k$ 和测量噪声 $v_k$ 皆服从于高斯分布，即 $p(w)\sim N(0,Q)$ 和 $p(v)\sim N(0,R)$ 。以 $Q=\begin{bmatrix}0&0\\0&0.01\end{bmatrix}$ 为例， $Q$ 表明系统误差的协方差速度的方差为0.01，位移上标准差为0，速度和方差之间无关联。

现有 $\hat{x_k}{'}$ 为预测值(先验)， $\hat{x}_k$ 为估计值， $\hat z_k$ 为观测值(后验)。由一般的反馈思想有：

$\begin{aligned} \hat x_k&=\hat x_k{'}+K_k(z_k-\hat z_k)\\&=\hat x_k{'}+K_k(z_k-H\hat x_k{'}) \end{aligned}\tag3$

其中 $z_k$ 为真实值， $(z_k-H\hat x_k{'})$ 为测量值与真实值之间的残差，其中求取 $K_k$ 为关键。

假定估计值与真实值之间的协方差为：

$\begin{aligned} P_k&=E[e_ke_k^{T}]=E[(x_k-\hat x_k)(x_k-\hat x_k)^T]\\ &=\begin{bmatrix}E(S_{err}S_{err}^T)&E(S_{err}V_{err}^T)\\ E(V_{err}S_{err}^T)&E(V_{err}V_{err}^T)\end{bmatrix} \end{aligned}\tag4$

上式中 $S_{err}$ 表示位移误差， $V_{err}$ 表示速度误差。

将式(3)代入式(4)得：
$P_k=[(I-K_kH)(x_k-\hat x_k{'})-K_kv_k][(I-K_kH)(x_k-\hat x_k{'})-K_kv_k]^{T}\tag{5}$

同理得到预测值与真实值之间的协方差。

$P_k{'}=E[e_k{'}e_k{'}^{T}]=E[(x_k-\hat x_k{'})(x_k-\hat x_k{'})^{T}]\tag6$

注意系统状态 $x_k$ 与测量噪声 $v_k$ 之间是相互独立的。

将式(5)展开可得：

$\begin{aligned} P_k&=(I-K_kH)E[(x_k-\hat x_k{'})(x_k-\hat x_k{'})^{T}](I-K_kH)^{T}+K_kE[v_kv_k^{T}]K_k^{T}\\ &=(I-K_kH)P_k{'}(I-K_kH)^{T}+K_kRK_k^{T}\\ &=P_k{'}-K_kHP_k{'}-P_k{'}H^{T}K_k^{T}+K_k(HP_k{'}H^T+R)K_k^{T} \end{aligned}\tag7$

结合均方差的意义，利用矩阵的迹对式(7)进行操作得：

$\begin{aligned} T[P_k]=T[P_k{'}]-2T[K_kHP_k{'}]+T[K_k(HP_k{'}H^T+R)K_k^{T}] \end{aligned}\tag8$

最小均方差，对 $K_k$ 进行求导，令导函数为0。则有下式：

$\begin{aligned} \frac{dT[P_k]}{dK_k}&=-2(HP_k{'})^{T}+2K_k(HP_k^{'}H^T+R)=0\\ \therefore K_k&=P_k{'}H^{T}(HP_{k}'H^{T}+R)^{-1} \end{aligned}\tag9$

其中 $R$ 为测量噪声协方差矩阵。假定上面的所有维度都为 $1\times1$ 维，令 $H=1$ ，且 $P_{k}{'}\neq0$ ，则公式(9)可以简化如下：
$K_k=\frac{P_k{'}}{P_k{'}+R}=\frac{1}{1+\frac{R}{P_k{'}}}$

分析上式可得以下结论：

若 $K_k$ 随着 $P_k{'}$ 增大而增大，说明卡尔曼增益越大，越重视反馈。
若 $P_k{'}=0$ ，说明预测值等于真实值。
若 $K_k=0$ ，说明估计值等于预测值。
注意， $P_k$ 为估计值与真实值之间的协方差， $P_k{'}$ 为预测值与真实值之间的误差。

将计算出的 $K_k$ 反代入公式 7中,化简可得：

$\begin{aligned} P_k&=P_k{'}-P_k{'}H^T(HP_k{'}H^{T}+R^{-1})HP_k{'}\\ &=P_k{'}-K_kHP_k{'}\\ &=(I-K_kH)P_k{'} \end{aligned}\tag{10}$

上式中 $P_k{'}$ 的递推计算如下，注意 $P_k{'}$ 为预测值与真实值之间的协方差矩阵。

首先有预测值的递推形式： $\hat x{'}_{k+1}=A\hat{x_k}+Bu_k$ ，结合公式(1)可得：

$\begin{aligned} P{'}_{k+1}&=E[e{'}_{k+1}e{'}_{k+1}^{T}]=E[(x_{k+1}-\hat{x}{'}_{k+1})(x_{k+1}-\hat{x}{'}_{k+1})^{T}]\\ &=E[[A(x_k-\hat x_k)+w_k][A(x_k-\hat x_k)+w_k]^T] \end{aligned}\tag{11}$