卡尔曼滤波器（思路整理，算法推导）

卡尔曼滤波器（思路整理，算法推导）

知乎好文：卡尔曼滤波

• 首先定义好各个变量：

状态的最优估计值：${\hat{x}}_{t-1},{\hat{x}}_t$

状态的预测值：${\hat{x}}_t^{-}$

状态的真实值：$x_{t-1},x_t$

状态的观测值：$z_t$

状态转移矩阵：$F$

控制输入矩阵：$B$

控制变量：$u_t$

卡尔曼增益：$K$

状态观测矩阵：$H$

状态转移过程噪声：$w_t$遵循高斯分布$(0,Q)$

状态观测噪声：$v_t$遵循高斯分布$(0,R)$

• 先解释下卡尔曼滤波器完成的是一件什么事

默认：状态的真实值是无法直接得到的，只能尽可能估计。

最优估计：根据上一次状态的最优估计值${\hat{x}}_{t-1}$得到当前状态的预测值${\hat{x}}_t^{-}$，由于预测不一定准确，因此需要根据观察值加以修正，也就是说最优估计值是预测值与观测值的加权和，加权和即为滤波，此即卡尔曼滤波器：
$${\hat{x}}_t={\hat{x}}_t^{-}+K(z_t-H{\hat{x}}_t^{-})$$
以一维为例，一般而言，H为1，因此K等于1时，状态最优估计值完全等于观测值，K等于0时完全等于预测值。

• 公式推导

$${\hat{x}}_t^{-}=F{\hat{x}}_{t-1}+B{u}_t$$

$$x_t=F{x}_{t-1}+B{u}_t+w_t$$

因此真实值与预测值之差的方差为
$$\begin{gathered} P_t^{-}=cov(x_t-{\hat{x}}_t^{-})=cov(F(x_{t-1}-{\hat{x}}_{t-1})+w_t)= \\ Fcov(x_{t-1}-{\hat{x}}_{t-1})F^T+Q=F{P}_{t-1}{F}^T+Q \end{gathered}$$

$$z_t=H{x}_t+v_t$$

卡尔曼滤波器的目的：$min||{\hat{x}}_t-x_t|{|}^2$
$$\begin{gathered} {\hat{x}}_t={\hat{x}}_t^{-}+K(H{x}_t+v_t-H{\hat{x}}_t^{-}) \\ {\hat{x}}_t-x_t=(I-KH)({\hat{x}}_t^{-}-x_t)+K{v}_t \\ cov({\hat{x}}_t-x_t)=cov((I-KH)({\hat{x}}_t^{-}-x_t)+K{v}_t) \\ {P}_t=(I-KH)P_t^{-}(I-KH{)}^T+KR{K}^T \end{gathered}$$
要满足$min||{\hat{x}}_t-x_t|{|}^2$，需要$minTr(P_t)$，注意这里假设$x$为多变量，因此各个维度差值的方差和应尽可能小

接下来一步不太理解：

$P_t$对$K$求偏导应为0（这样就能保证迹最小？望大佬告知）：
$$\begin{gathered} \partial (P_t)/\partial (K)=-2(P_t^{-}{H}^T)+2K(H{P}_t^{-}{H}^T+R)=0 \\ K=(P_t^{-}{H}^T)(H{P}_t^{-}{H}^T+R{)}^{-1} \end{gathered}$$
由此可以得到
$$P_t=(I-KH)P_t^{-}$$
因此卡尔曼滤波的整个过程为：

1. 根据${\hat{x}}_{t-1}$得到预测值${\hat{x}}_t^{-}$，再通过观测得到观测值$z_t$
2. 根据上一时刻的方差$P_{t-1}$得到$P_t^{-}$，再根据$H,R$求得最佳卡尔曼增益$K$
3. 根据${\hat{x}}_t^{-},z_t,H,K$得到最优估计值${\hat{x}}_t$
4. 最后更新下一步需要用到的$P_t$

总结如下：

图中红色为输入，蓝色为输出，浅绿色与深绿色部分对应预测过程，白色、黄色、橙色代表更新过程。

Tips：习惯${\hat{x}}_0=0,P_0=1$，$Q,R$是可以调节的超参数