卡尔曼滤波实战
时间：2023-07-05 16:07:00
文章目录
- 理论
- 实战-以FAST-Dynamic-Vision为例
理论
参考1视频 [参考2博客]( // https://zhuanlan.zhihu.com/p/195649092)
先验估计：基于数学模型，根据上一刻状态估计下一刻状态的结果；
后验估计：模型计算的数据 修正观测值后
每一帧都执行下面的预测->更新步骤，顺序不能改变，必不可少。
预测：
前两种是状态和关系预测、预测状态(先验估计)和状态之间的关系(协议差)。状态在更新，状态之间的关系也在更新。
x ^ k ? = A x ^ k ? 1 ? B u k ? 1 (1) \hat{x}^-_{k}=A\hat{x}^-_{k-1} Bu_{k-1}\tag{1} x^k?=Ax^k?1? Buk?1(1)
公式(1)就是根据上一时刻(k-1)的状态，依据数学模型，得到当前时刻的状态(k)，本质上要求把两个状态之间的状态转移矩阵写出来，具体矩阵形式可能会有不同，但等式形式类似。
P k − = A P k − 1 A T + Q (2) P_{k}^-=AP_{k-1}A^T+Q\tag{2} Pk−=APk−1AT+Q(2)
公式(2)中 P P P是协方差矩阵，A是状态转移矩阵，Q是噪音矩阵，服从高斯分布，代表一些不可控因素，也就是状态之间的转移除了状态转移矩阵的更新，还有外部条件的影响。因为你观测的状态之间必然是存在一个联系的，用协方差矩阵表示这个联系，一般是一个对角阵，对角线上是方差，非对角线上是两个状态之间的关系。
更新：
K k = P k − H T H P k − H T + R (3) K_k=\frac{P^-_kH^T}{HP^-_{k}H^T+R}\tag{3} Kk=HPk−HT+RPk−HT(3)
公式(3)就是最核心的卡尔曼增益了，可以认为他就是一个权重项，他的目的就是当K取多少时，能让最优估计的不确定性最小。其中P是协方差，R是噪声矩阵（观测值的偏差），H是测量矩阵，（不用管它怎么推导出来的的，会用即可）。
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) (4) \hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)\tag{4} x^k=x^k−+Kk(zk−Hx^k−)(4)
公式(4)就是计算后验估计的方法，其中 x ^ k \hat{x}_k x^k是后验估计输出结果， x ^ k − \hat{x}^-_k x^k−是先验估计结果， z k z_k zk是观测值；也就是要计算观测值和先验估计之间的差异；我传感器的值你总得借鉴一下，那么借鉴多少呢？就是由 K K K决定的
P k = ( I − K k H ) P k − (5) P_k=(I-K_kH)P^-_k\tag{5} Pk=(I−KkH)Pk−(5)
公式(5)进行协方差矩阵的状态更新。每一帧都不一样，都要更新啊。
实战-以FAST-Dynamic-Vision为例
- 状态有位置 x , y x,y x,y，速度 v x , v y v_{x},v_{y} vx,vy，加速度 a x , a y a_{x},a_{y} ax,ay，其中四个的计算方程如下
x = x + v x d t + 1 2 a x Δ t 2 y = y + v y d t + 1 2 a y Δ t 2 v x = v x + a x Δ t v x = v x + a x Δ t 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 \\ x=x+vxdt+21axΔt2y=y+vydt+21ayΔt2vx=vx+axΔtvx=vx+axΔt
- 因此我们就可以得出他的状态转移方程如下，根据当前状态估计下一时刻状态，其中左侧为时刻t，右侧为时刻t-1，得出的结果叫做先验估计，由上一状态估计出了现在的状态。我们把他叫做 s t a t u s t status_t statust
[ x y v x v y a x a y ] t = [ 1 0 Δ t 0 1 2 a Δ t 2 0 0 1 0 Δ t 0 1 2 a Δ t 2 0 0 1 0 Δ t 0 0 0 0 1 0 Δ t 0 0 0 0 1 0 0 0 0 0 0 1 ] [ x y v x v y a x a y ] t − 1 \begin{bmatrix} x \\ y \\ v_x \\ v_y \\ a_x \\ a_y \\ \end{bmatrix}_t =\ \begin{bmatrix} 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{bmatrix} \begin{bmatrix} x \\ y \\ v_x \\ v_y \\ a_x \\ a_y \\ \end{bmatrix}_{t-1} \\ ⎣⎢⎢⎢⎢⎢⎢⎡xyvxvyaxay⎦⎥⎥⎥⎥⎥⎥⎤t= ⎣⎢⎢⎢⎢⎢⎢⎡100000010000Δt010000Δt0100元器件数据手册、IC替代型号，打造电子元器件IC百科大全！
