卡尔曼滤波实战

理论

参考1视频 [参考2博客]( // https://zhuanlan.zhihu.com/p/195649092)

先验估计：基于数学模型根据上一时刻状态估计处下一时刻状态的一个结果；

后验估计：模型计算的数据+观测值修正后

每一帧都要执行下边的预测->更新的步骤，而且顺序不能变，且缺一不可。

预测：

前两个是状态与关系预测，预测状态(先验估计)以及状态之间的关系(协方差)。状态在更新，状态之间的关系也在更新。
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}^{-}+B{u}_{k-1}\text{\hspace{1em}(1)}$$
公式(1)就是根据上一时刻(k-1)的状态，依据数学模型，得到当前时刻的状态(k)，本质上要求把两个状态之间的状态转移矩阵写出来，具体矩阵形式可能会有不同，但等式形式类似。
$$P_k^{-}=A{P}_{k-1}{A}^T+Q\text{\hspace{1em}(2)}$$
公式(2)中$P$​​​​​​是协方差矩阵，A是状态转移矩阵，Q是噪音矩阵，服从高斯分布，代表一些不可控因素，也就是状态之间的转移除了状态转移矩阵的更新，还有外部条件的影响。因为你观测的状态之间必然是存在一个联系的，用协方差矩阵表示这个联系，一般是一个对角阵，对角线上是方差，非对角线上是两个状态之间的关系。

更新：
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\text{\hspace{1em}(3)}$$
公式(3)就是最核心的卡尔曼增益了，可以认为他就是一个权重项，他的目的就是当K取多少时，能让最优估计的不确定性最小。其中P是协方差，R是噪声矩阵（观测值的偏差），H是测量矩阵，（不用管它怎么推导出来的的，会用即可）。
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\text{\hspace{1em}(4)}$$
公式(4)就是计算后验估计的方法，其中${\hat{x}}_k$是后验估计输出结果，${\hat{x}}_k^{-}$是先验估计结果，$z_k$​​是观测值；也就是要计算观测值和先验估计之间的差异；我传感器的值你总得借鉴一下，那么借鉴多少呢？就是由$K$决定的
$$P_k=(I-K_{k}H)P_k^{-}\text{\hspace{1em}(5)}$$
公式(5)进行协方差矩阵的状态更新。每一帧都不一样，都要更新啊。

实战-以FAST-Dynamic-Vision为例

1. 状态有位置$x,y$​，速度$v_x,v_y$​，加速度$a_x,a_y$​​，其中四个的计算方程如下

$$\begin{gathered} x=x+v_x{d}_t+\frac{1}{2}{a}_x\Delta t^2 \\ y=y+v_y{d}_t+\frac{1}{2}{a}_y\Delta t^2 \\ {v}_x=v_x+a_x\Delta t \\ {v}_x=v_x+a_x\Delta t \end{gathered}$$

2. 因此我们就可以得出他的状态转移方程如下，根据当前状态估计下一时刻状态，其中左侧为时刻t，右侧为时刻t-1，得出的结果叫做先验估计，由上一状态估计出了现在的状态。我们把他叫做$statu{s}_t$​

$${\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\\ a_x\\ a_y\end{array}\right]}_t=\left[\begin{array}{cccccc}1& 0& \Delta t& 0& \frac{1}{2}a\Delta t^2& 0\\ 0& 1& 0& \Delta t& 0& \frac{1}{2}a\Delta t^2\\ 0& 0& 1& 0& \Delta t& 0\\ 0& 0& 0& 1& 0& \Delta t\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\\ a_x\\ a_y\end{array}\right]}_{t-1}$$

可写为，这就是先验估计
$$statu{s}_k=F\ast statu{s}_{t-1}$$

其中F如下所示，被叫做状态转移矩阵
$$F=\left[\begin{array}{cccccc}1& 0& \Delta t& 0& \frac{1}{2}a\Delta t^2& 0\\ 0& 1& 0& \Delta t& 0& \frac{1}{2}a\Delta t^2\\ 0& 0& 1& 0& \Delta t& 0\\ 0& 0& 0& 1& 0& \Delta t\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$

3. 那么协方差矩阵P，两个状态之间的联系。一般是个对角阵，对角线单个状态的方差。非对角线上是不同参数之间的关系，它代表**状态之间关系的更新。**Q是一个高斯分布的噪声矩阵。关系分布中有一些不可控因素，一般会初始化一个表示影响，
   $$P_t=F{P}_{t-1}{F}^T+Q$$

   其中Q在代码中为dtq。dt为两个时刻之间的时间差
   $$\begin{gathered} dt=t-pr{e}_t \\ dtq=dt\ast 100\ast Q=dt\ast 100\ast \left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 2& 0& 0& 0\\ 0& 0& 0& 2& 0& 0\\ 0& 0& 0& 0& 4& 0\\ 0& 0& 0& 0& 0& 4\end{array}\right] \end{gathered}$$
   至此，预测部分就结束了。

4. 下面开始更新部分。首先是卡尔曼增益的更新，他在其中把这个公式拆成了两部分

   原公式为：

$$K=(P{C}^T)(CP{C}^T+R{)}^{-1}$$

​ 在代码中的表现形式为:
$$\begin{gathered} S=HP{H}^T+R \\ K=P{H}^T\ast S^{-1} \end{gathered}$$
​ 其中:
KaTeX parse error: Undefined control sequence: \ at position 73: …atrix} \\ H= \̲ ̲\begin{bmatrix}…
也就是说该卡尔曼增益公式可被展开为如下样式，P的内容见上方，最后卡尔曼增益得到的是一个6*2的矩阵。
$$K=(P{H}^T)(HP{H}^T+R{)}^{-1}=(P\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right])(\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 2& 0& 0& 0\\ 0& 0& 0& 2& 0& 0\\ 0& 0& 0& 0& 4& 0\\ 0& 0& 0& 0& 0& 4\end{array}\right]\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]+\left[\begin{array}{cc}0.5& 0\\ 0& 0.5\end{array}\right]{)}^{-}1$$

5. 然后是状态的更新。
   $${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Y-H{\hat{x}}_k^{-})$$
   这个公式中，$Y$是观测到的xy坐标信息(来自相机的测量结果)，可以明显的看出，$H{\hat{x}}_k^{-}$恰好取出了上一状态中的xy坐标信息，得到一个列向量。同样$Y$也是个列向量，做差之后得到2*1的矩阵。再和卡尔曼增益相乘后得到6*1矩阵，就是相应的状态调整值了。然后即可得到后验估计的值。

6. 最后是协方差矩阵的更新。
   $$P=(I-KH)P$$
   在该公式中，$I$是一个6*6的单位矩阵。

预测：预测状态以及状态之间的关系

卡尔曼增益用来更新状态以及协方差矩阵

卡尔曼增益K：就是个权重项，可大可小可计算