卡尔曼滤波与目标跟踪-1卡尔曼滤波使用

什么是卡尔曼滤波？

为什么要估计事物的状态？因为我们不能准确地知道物体的当前状态，所以我们需要测量来获得物体的状态。但测量值不准确，总是有噪音。卡尔曼滤波器是一种结合预测（先验分布）和测量更新（似乎）的状态估计算法。
预测模块是为物体的运动建立运动模型。据估计，栗子中的行人状态采用恒速模型（CV），运动模型通过转换上一刻的最佳估计，得到当前时刻的估计状态和预测误差。
根据当前时刻的测量值和预测状态，对当前时刻的状态进行最优估计。

2.卡尔曼公式

Predict：
$$X_k=F{\hat{X}}_{k?1}B{U}_k+w_k$$$$P_k=F{P}_{k-1}F+Q_k$$
其中$X_k$为k时刻的状态预测值，F为状态转移矩阵，${\hat{X}}_{k-1}$为k-1时刻的最优估计。$B{U}_k$为外部输入，$w_k$为过程激励噪声。P为预测误差，$P_k$表示为k次先验估计协方差矩阵，$P_{k-1}$为k-1次后验估计协方差矩阵。$Q_k$为过程激励噪声协方差矩阵。
Update：
$$K_K=P_K{H}^T{(H{P}_K{H}^T+R)}^{-1}$$$$\widehat{X_K}=X_K+K_K(Z_K-H{X}_K)$$$$P_K=(1-K_{K}H)P_K$$
其中K为卡尔曼增益，H为观测矩阵，R为测量噪声协方差矩阵，$Z_K$为k时刻测量值。

3.以行人状态估计举个栗子

①predict:

先对行人状态建立一个运动模型，我们在此选择恒速度运动模型，$X={(p_x,p_y,v_x,v_y)}^T$,此时$B{U}_k$为0，过程噪声$w_k$其实是行人加减速引起的。公式如下：
$$X_K=\left[\begin{array}{cccc}1& 0& △t& 0\\ 0& 1& 0& △t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}{p}_x\\ p_y\\ v_x\\ v_y\end{array}\right]}_{k-1}+{\left[\begin{array}{c}\frac{1}{2}{a}_x△t^2\\ \frac{1}{2}{a}_y△t^2\\ a_x△t\\ a_y△t\end{array}\right]}_{k-1}$$