一、卡尔曼滤波

---

前言

最近看到MPU6050模块有dmp处理功能，并且相关资料上表示接入第三方磁力计，可以使用dmp的九轴融合，但是发现只有dmp六轴融合的固件，没有九轴融合的固件，也就是说InvenSense的手册上声称“我们的dmp支持九轴融合”，但实际上InvenSense又不给出用于MPU6050九轴融合的固件，因此实际上说了等于没有说，这个问题刚开始一直在心中疑惑不解，但最后想来应该就是这样把。
反正6050也没有办法用dmp来做九轴融合，只能在单片机上融合了，实际上这反而给我们学习AHRS的动力，这次希望通过学习总结有所进步。
通过网上查看资料，AHRS又有很多算法，这里准备选择学习扩展卡尔曼，很多博主的AHRS讲解直接默认我们会各种前置知识，但实际上我并不会，所以这个系列作为学习笔记，我会把要用到的内容逐一记录下来。

---

一、卡尔曼滤波的作用

首先，我认为卡尔曼滤波其实并不是滤波器，其实它是一个估计器，什么意思呢？按我的理解就是我们已知一个系统的状态方程，以及一些测量结果，由于干扰和误差的存在，系统不可能完全按照状态方程运行，我们需要结合测量结果的信息来给出更好的结果。但是通常测量结果也有误差，那么如何利用状态方程和测量结果来做出最优估计呢？这就是卡尔曼滤波要做的事情。

二、卡尔曼滤波的思路

首先系统状态方程为：
$${\mathbf{x}}_k={\mathbf{\Phi }}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}$$

然后测量方程为：
$${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$$
其中：${\mathbf{w}}_{k-1}$是状态方程中的噪声，而${\mathbf{v}}_k$是测量方程中的噪声。
由于噪声的存在，不管是状态方程还是测量方程，我们都没有办法得到精确的状态，怎么办啊？我们退而求其次，我们来设定一个目标每次都去获得一个最佳估计。

假设第$k-1$次的最佳估计状态记为${\hat{\mathbf{x}}}_{k-1}$，现在一个的核心问题是怎么得到${\hat{\mathbf{x}}}_k$呢？

首先我们想到状态方程好像可以递推出${\hat{\mathbf{x}}}_k$，但是直接用好像又没有使用测量结果的信息，先不管了，我们先令：
$${\hat{\mathbf{x}}}_k^{-}={\mathbf{\Phi }}_{k-1}{\hat{\mathbf{x}}}_{k-1}$$
噪声嘛就直接丢掉好了，然后嘛，由于没有加入测量信息，所以等号左边那个带负号的递推值通常叫做先验估计值了，意思就是事先估计值了，响应的${\hat{\mathbf{x}}}_k$可以叫做后验估计，有点事后诸葛亮的意思在里面。

然后我们加入测量的结果企图得到最优的${\hat{\mathbf{x}}}_k$，这里我们不如这样考虑，${\hat{\mathbf{x}}}_k$有两部分组成，一部分是事先估计值${\hat{\mathbf{x}}}_k^{-}$，第二部分和测量相关，准确的说是这样的当测量值与事先估计值的差别越大，那么第二部分的比重就越大。因此我们用${\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-})$，来表示第二部分。因此${\hat{\mathbf{x}}}_k^{-}\to {\hat{\mathbf{x}}}_k$的问题就变成了这样子：
$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-})$$

卡尔曼估计的核心问题就是如何找到${\mathbf{K}}_k$是的我们的后验估计最优。

三、卡尔曼滤波的推导

卡尔曼滤波器的推导过程中会用到一些假设，不过不用担心这些假设都很自然。
为了使得我们的最优估计（也就是那个后验估计）${\hat{\mathbf{x}}}_k$与真实状态变量${\mathbf{x}}_k$尽量接近：
第一，我们可以设计我们的估计方法使得对每一个$k$都有期望：$E[{\hat{\mathbf{x}}}_k-{\mathbf{x}}_k]=0$，一般的说法是要求最优估计是无偏。
类似的，我们来看先验估计$E[{\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k]$，期望是否也为$0$呢？我们来看一下：
$$\begin{array}{rl}E[{\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k]& =E[{\mathbf{\Phi }}_{k-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{x}}_k]=E[{\mathbf{\Phi }}_{k-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{\Phi }}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{w}}_{k-1}]\\ & ={\mathbf{\Phi }}_{k-1}E[{\hat{\mathbf{x}}}_{k-1}-{\mathbf{x}}_{k-1}]-E[{\mathbf{w}}_{k-1}]=0\end{array}$$
所以先验估计自动满足了无偏的要求，非常nice！！！这样的话我们就非常有信心继续下去了。

然后我们要考虑些什么呢？考虑最优估计值和真实的状态之间的偏离越小越好，仿照最小二乘法的思想，那当然就是要求估计误差的方差最小了。
$$\begin{array}{rl}J& =E[({\hat{\mathbf{x}}}_k-{\mathbf{x}}_k{)}^T({\hat{\mathbf{x}}}_k-{\mathbf{x}}_k)]=E[({\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k{\mathbf{z}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k{)}^T({\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k{\mathbf{z}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)]\\ & =E[({\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{K}}_k{\mathbf{v}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k{)}^T({\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{K}}_k{\mathbf{v}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)]\\ & =E\{[(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)+{\mathbf{K}}_k{\mathbf{v}}_k{]}^T[(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)+{\mathbf{K}}_k{\mathbf{v}}_k]\}\end{array}$$
问题变成如何选择${\mathbf{K}}_k$使得$J$最小呢？
借鉴最小二乘法的经验，我们需要一个关于${\mathbf{K}}_k$的二次型，但是上面的$J$中期望括号中直接乘开的话，得到的形式不好用啊！！！原因在于直接乘开的话有关${\mathbf{K}}_k$的部分不是在二次型的外侧。如果相乘的顺序可以交换的话就好了：
$$\begin{gathered} [(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)+{\mathbf{K}}_k{\mathbf{v}}_k{]}^T[(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)+{\mathbf{K}}_k{\mathbf{v}}_k] \\ \to [(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)+{\mathbf{K}}_k{\mathbf{v}}_k][(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)+{\mathbf{K}}_k{\mathbf{v}}_k{]}^T \end{gathered}$$
？？？这样做我们确实可以得到关于${\mathbf{K}}_k$的二次型，但是两者并不相等，唉……
但是注意到一个特殊情况的事实，任意一个列向量$\mathbf{A}$，一定有${\mathbf{A}}^T\mathbf{A}=tr[\mathbf{A}{\mathbf{A}}^T]$，所以：
J = E { [ ( I − K k H k ) ( x ^ k − − x k ) + K k v k ] T [ ( I − K k H k ) ( x ^ k − − x k ) + K k v k ] } = E { t r { [ ( I − K k H k ) ( x ^ k − − x k ) + K k v k ] [ ( I − K k H k ) ( x ^ k − − x k ) + K k v k ] T } } = E { t r [ ( I − K k H k ) ( x ^ k − − x k ) ( x ^ k − − x k ) T ( I − K k H k ) T + ( I − K k H k ) ( x ^ k − − x k ) v k T K k T + K k v k ( x ^ k − − x k ) T ( I − K k H k ) T + K k v k v k T K k T ] } \begin{aligned} J=&E\{[(\bm I-\bm K_k\bm H_k)(\bm {\hat x}^-_k-\bm x_k)+\bm K_k\bm v_k]^T[(\bm I-\bm K_k\bm H_k)(\bm {\hat x}^-_k-\bm x_k)+\bm K_k\bm v_k]\}\\ =&E\{tr\{[(\bm I-\bm K_k\bm H_k)(\bm {\hat x}^-_k-\bm x_k)+\bm K_k\bm v_k][(\bm I-\bm K_k\bm H_k)(\bm {\hat x}^-_k-\bm x_k)+\bm K_k\bm v_k]^T\}\}\\ =&E\{tr[(\bm I-\bm K_k\bm H_k)(\bm {\hat x}^-_k-\bm x_k)(\bm {\hat x}^-_k-\bm x_k)^T(\bm I-\bm K_k\bm H_k)^T+(\bm I-\bm K_k\bm H_k)(\bm {\hat x}^-_k-\bm x_k)\bm v_k^T\bm K_k^T\\ &+\bm K_k\bm v_k(\bm {\hat x}^-_k-\bm x_k)^T(\bm I-\bm K_k\bm H_k)^T+\bm K_k\bm v_k\bm v_k^T\bm K_k^T]\} \end{aligned} J===​E{[(I−Kk​Hk​)(x^k−​−xk​)+Kk​vk​]T[(I−Kk​Hk​)(x^k−​−xk​)+Kk​vk​]}E{tr{[(I−Kk​Hk​)(x^k−​−xk​)+Kk​vk​][(I−Kk​Hk​)(x^k−​−xk​)+Kk​vk​]T}}E{tr[(I−Kk​Hk​)(x^k−​−xk​)(x^k−​−xk​)T(I−Kk​Hk​)T+(I−Kk​Hk​)(x^k−​−xk​)vkT​KkT​+Kk​vk​(x^k−​−xk​)T(I−Kk​Hk​)T+Kk​vk​vkT​KkT​]}​
一个很自然的假设就是状态和噪声无关，于是我们合理地假设${\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k$和${\mathbf{v}}_k$是两个不相关的随机变量。又由于这两个随机变量的期望也是$0$，所以把期望交换到内部一定会导致交叉项出现两个$0$相乘的结果，因此我们得到目标函数的新形式：
$$\begin{array}{rl}J=& tr\{(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k)E[({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k{)}^T](\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k{)}^T+{\mathbf{K}}_{k}E[{\mathbf{v}}_k{\mathbf{v}}_k^T]{\mathbf{K}}_k^T\}\end{array}$$
我们记${\mathbf{R}}_k=E[{\mathbf{v}}_k{\mathbf{v}}_k^T]$为测量噪声的协方差矩阵，记${\mathbf{P}}_k^{-}=E[({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k{)}^T]$为先验估计的偏差的协方差矩阵。
则最终我们把目标函数写成如下形式：
$$J=tr[(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k{)}^T+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^T]$$
这里再次注意一下，我们的目标函数是一个标量的形式，我们需要确定${\mathbf{K}}_k$为何值的时候目标函数最小。因此可以对${\mathbf{K}}_k$求导(实际上就是对${\mathbf{K}}_k$的每一个元素求偏导)，然后让导数为$0$。就可以得到我们需要的${\mathbf{K}}_k$了。
即要求对$\mathbf{K}$的任意元素$K_{ij}$，有：
$$\frac{\partial J}{\partial K_{ij}}=0$$
为了得到结果我们把$J$写成分量求和的形式，并且为了书写方便省略掉求和号（即使用求和约定）和下标$k$，如果不熟悉的话可自行写出求和符号：
∂ J ∂ K i j = ∂ { t r [ ( I − K H ) P − ( I − K H ) T + K R K T ] } ∂ K i j = ∂ [ ( δ r s − K r t H t s ) P s l − ( δ r l − K r n H n l ) + K r s R s l K r l ] ∂ K i j = − H j s P s l − ( δ i l − K i n H n l ) − ( δ i s − K i t H t s ) P s l − H j l + R j l K i l + K i s R s j \begin{aligned} \frac{\partial J}{\partial K_{ij}}=&\frac{\partial\{ tr[(\bm I-\bm K\bm H)\bm P^-(\bm I-\bm K\bm H)^T+\bm K\bm R\bm K^T]\}}{\partial K_{ij}}\\ =&\frac{\partial[(\delta_{rs}-K_{rt}H_{ts})P^-_{sl}(\delta_{rl}-K_{rn} H_{nl})+ K_{rs}R_{sl}K_{rl}]}{\partial K_{ij}}\\ =&-H_{js}P^-_{sl}(\delta_{il}-K_{in} H_{nl}) - (\delta_{is}-K_{it}H_{ts})P^-_{sl}H_{jl}+R_{jl}K_{il}+K_{is}R_{sj} \end{aligned} ∂Kij​∂J​===​∂Kij​∂{tr[(I−KH)P−(I−KH)T+KRKT]}​∂Kij​∂[(δrs​−Krt​Hts​)Psl−​(δrl​−Krn​Hnl​)+Krs​Rsl​Krl​]​−Hjs​Psl−​(δil​−Kin​Hnl​)−(δis​−Kit​Hts​)Psl−​Hjl​+Rjl​Kil​+Kis​Rsj​​
由于协方差矩阵${\mathbf{P}}^{-}$和$\mathbf{R}$一定是对称的，所以上面的式子可以写成更加简洁的样子：
$$\begin{array}{rl}\frac{\partial J}{\partial K_{ij}}=& -H_{js}{P}_{sl}^{-}({\delta }_{il}-K_{in}{H}_{nl})-({\delta }_{is}-K_{it}{H}_{ts})P_{sl}^{-}{H}_{jl}+R_{jl}{K}_{il}+K_{is}{R}_{sj}\\ =& -2({\delta }_{is}-K_{it}{H}_{ts})P_{sl}^{-}{H}_{jl}+2K_{il}{R}_{lj}\end{array}$$
回忆一下最优化条件：
$$\frac{\partial J}{\partial K_{ij}}=0$$
写成矩阵形式，并且恢复下标$k$：
$$\begin{array}{rl}\frac{\partial J}{\partial {\mathbf{K}}_k}=& -2(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+2{\mathbf{K}}_k{\mathbf{R}}_k=0\end{array}$$
因此我们得到最优估计需要使用的${\mathbf{K}}_k$，如下：
$$\begin{array}{r}{\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T[{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{]}^{-1}\end{array}$$

到这里，我们的问题基本上得到了问题的解决方案，但是还有一个问题：我们使用了${\mathbf{P}}_k^{-}$，但是我们没有它的递推公式！！！
因此需要详细研究一下${\mathbf{P}}_k^{-}$，其中各种不相关性以及无偏性的使用就直接用了哈：
$$\begin{array}{rl}{\mathbf{P}}_k^{-}=& E[({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k)({\hat{\mathbf{x}}}_k^{-}-{\mathbf{x}}_k{)}^T]\\ =& E[({\mathbf{\Phi }}_{k-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{\Phi }}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{w}}_{k-1})({\mathbf{\Phi }}_{k-1}{\hat{\mathbf{x}}}_{k-1}-{\mathbf{\Phi }}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{w}}_{k-1}{)}^T]\\ =& {\mathbf{\Phi }}_{k-1}E[({\hat{\mathbf{x}}}_{k-1}-{\mathbf{x}}_{k-1})({\hat{\mathbf{x}}}_{k-1}-{\mathbf{x}}_{k-1}{)}^T]{\mathbf{\Phi }}_{k-1}^T+E[{\mathbf{w}}_{k-1}{\mathbf{w}}_{k-1}^T]\end{array}$$
如果记：${\mathbf{P}}_{k-1}=E[({\hat{\mathbf{x}}}_{k-1}-{\mathbf{x}}_{k-1})({\hat{\mathbf{x}}}_{k-1}-{\mathbf{x}}_{k-1}{)}^T]$以及${\mathbf{Q}}_{k-1}=E[{\mathbf{w}}_{k-1}{\mathbf{w}}_{k-1}^T]$，则上式化为：
$$\begin{array}{r}{\mathbf{P}}_k^{-}={\mathbf{\Phi }}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k-1}^T+{\mathbf{Q}}_{k-1}\end{array}$$

现在，我们搞清楚了递推关系：$\mathbf{P}\to {\mathbf{P}}^{-}$，但是$\mathbf{P}$由怎么得到呢？
我们来试一下：
$$\begin{array}{rl}{\mathbf{P}}_k=& E[({\hat{\mathbf{x}}}_k-{\mathbf{x}}_k)({\hat{\mathbf{x}}}_k-{\mathbf{x}}_k{)}^T]\\ =& E\{[{\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-})-{\mathbf{x}}_k][{\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-})-{\mathbf{x}}_k{]}^T\}\end{array}$$
这样又有一大堆推导了，蛋疼得很，但是仔细观察我们之前$J=E[({\hat{\mathbf{x}}}_k-{\mathbf{x}}_k{)}^T({\hat{\mathbf{x}}}_k-{\mathbf{x}}_k)]$，仔细对比一下$J$的推导过程，我们发现原来${\mathbf{P}}_k$根本就不用重新推导，直接可以看出：
$$\begin{array}{r}{\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k{)}^T+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^T\end{array}$$
这个公式还可以化简：
$$\begin{array}{rl}{\mathbf{P}}_k=& (\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k{)}^T+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^T\\ =& (\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}+(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\mathbf{K}}_k^T+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^T\\ =& (\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}+{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\mathbf{K}}_k^T-{\mathbf{K}}_k({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k){\mathbf{K}}_k^T\end{array}$$
注意到：
$$\begin{array}{r}{\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T[{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{]}^{-1}\end{array}$$
上面公式的最后一项减号后面恰好是：${\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{\mathbf{K}}_k^T$，因此公式得到化简：
$$\begin{array}{r}{\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}\end{array}$$

到现在为止${\mathbf{P}}^{-}\to \mathbf{P}$也解决了，这样我们随着$k$的增加，我们发现可以使用套娃递推来解决$\mathbf{P}$的问题了：
${\mathbf{P}}_{k-1}^{-}\to {\mathbf{P}}_{k-1}\to {\mathbf{P}}_k^{-}\to {\mathbf{P}}_k\to {\mathbf{P}}_{k+1}^{-}\cdots$

四、卡尔曼滤波公式

我们把卡尔曼递推公式和使用步骤总结一下：

(1)系统建模：
$$\begin{array}{rl}{\mathbf{x}}_k=& {\mathbf{\Phi }}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}\\ {\mathbf{z}}_k=& {\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}$$
其中，各种噪声的期望都为$0$，并且两个时刻之间的噪声不相关，因此描述噪声相关性的协方差矩阵只有：
$$\begin{array}{rl}{\mathbf{Q}}_k=& E[{\mathbf{w}}_k{\mathbf{w}}_k^T]\\ {\mathbf{R}}_k=& E[{\mathbf{v}}_k{\mathbf{v}}_k^T]\end{array}$$

(2)初始条件
$$\begin{array}{rl}{\hat{\mathbf{x}}}_0^{-}=& E[{\mathbf{x}}_0]\\ {\mathbf{P}}_k^{-}=& E[({\hat{\mathbf{x}}}_0^{-}-{\mathbf{x}}_0)({\hat{\mathbf{x}}}_0^{-}-{\mathbf{x}}_0{)}^T]\end{array}$$

(3)卡尔曼滤波
第一步：状态预测(先验)
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{-}=& {\mathbf{\Phi }}_{k-1}{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{P}}_k^{-}=& {\mathbf{\Phi }}_{k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k-1}^T+{\mathbf{Q}}_{k-1}\end{array}$$
第二步：测量更新(后验)
$$\begin{array}{rl}{\mathbf{K}}_k=& {\mathbf{P}}_k^{-}{\mathbf{H}}_k^T[{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{R}}_k{]}^{-1}\\ {\hat{\mathbf{x}}}_k=& {\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-})\\ {\mathbf{P}}_k=& (\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_k^{-}\end{array}$$

---

总结

本文的目的是梳理一下卡尔曼滤波的设计思路，并从头推导了一遍滤波公式，对于一些假设不追求过分严谨，只要是直觉上合理就行，最后我们得到了卡尔曼滤波公式，这里得到了一组递推滤波公式，当然也有其他的一些等价形式，这里就不讨论了。