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

最新推荐文章于 2025-11-12 15:42:20 发布

原创最新推荐文章于 2025-11-12 15:42:20 发布 · 809 阅读

4 ·

CC 4.0 BY-SA版权

3D_detector 专栏收录该内容

4 篇文章

订阅专栏

本文深入解析卡尔曼滤波算法，阐述其预测和更新模块原理，通过行人状态估计实例，展示恒速度模型应用及卡尔曼增益计算。适用于信号处理、机器人定位等领域。

1.卡尔曼滤波是什么？

为什么要对事物状态进行估计？由于我们无法准确知道物体的当前状态，为了获得事物状态我们需要测量。但是测量值并不是准确的，总会存在噪声。卡尔曼滤波是一种结合预测（先验分布）和测量更新（似然）的状态估计算法。
预测模块就是对物体的运动建立运动模型，本文栗子中的行人状态估计我们采用恒速度模型（CV），通过对上一时刻的最优估计进行运动模型转换，得到当前时刻的估计状态，以及预测误差。
更新模块是根据当前时刻的测量值和预测状态得到当前时刻的状态最优估计。

2.卡尔曼公式

Predict：
$k−1+BUk+wk{{X}_{k}}=F{{\overset{\hat{\ }}{\mathop{X}}\,}_{k-1}}+B{{U}_{k}}+{{w}_{k}}$ ${{P}_{k}}=F{{P}_{k-1}}F+{{Q}_{k}}$
其中 ${{X}_{k}}$ 为k时刻的状态预测值，F为状态转移矩阵， $k−1{{\overset{\hat{\ }}{\mathop{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}}$ $=XK+KK(ZK−HXK)\overset{\hat{\ }}{\mathop{{{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}}$ 其实是行人加减速引起的。公式如下：
$XK=[10△t0010△t00100001][pxpyvxvy]k−1+[12ax△t212ay△t2ax△tay△t]k−1{{X}_{K}}=\left[ \begin{matrix} 1 & 0 & \vartriangle t & 0 \\ 0 & 1 & 0 & \vartriangle t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]{{\left[ \begin{matrix} {{p}_{x}} \\ {{p}_{y}} \\ {{v}_{x}} \\ {{v}_{y}} \\ \end{matrix} \right]}_{k-1}}+{{\left[ \begin{matrix} \tfrac{1}{2}{{a}_{x}}\vartriangle {{t}^{2}} \\ \tfrac{1}{2}{{a}_{y}}\vartriangle {{t}^{2}} \\ {{a}_{x}}\vartriangle t \\ {{a}_{y}}\vartriangle t \\ \end{matrix} \right]}_{k-1}}$ ${{P}_{k}}=F{{P}_{k-1}}F+{{Q}_{k}}$
其中P第一次应该初始化为？？？由于w（过程激励噪声）是随机代入的，所以为高斯白噪声，满足 $~N(0,Q)p(w)\tilde{\ }N(0,Q)$ ,Q即过程噪声协方差矩阵，表示如下：
$Q=[σpx2σpxpyσpxvxσpxvyσpypxσpy2σpyvxσpyvyσvxpxσvxpyσvx2σvxvyσvypxσvypyσvyvxσvy2]Q=\left[ \begin{matrix} \sigma _{{{p}_{x}}}^{2} & {{\sigma }_{{{p}_{x}}{{p}_{y}}}} & {{\sigma }_{{{p}_{x}}{{v}_{x}}}} & {{\sigma }_{{{p}_{x}}{{v}_{y}}}} \\ {{\sigma }_{{{p}_{y}}{{p}_{x}}}} & \sigma _{{{p}_{y}}}^{2} & {{\sigma }_{{{p}_{y}}{{v}_{x}}}} & {{\sigma }_{{{p}_{y}}{{v}_{y}}}} \\ {{\sigma }_{{{v}_{x}}{{p}_{x}}}} & {{\sigma }_{{{v}_{x}}{{p}_{y}}}} & \sigma _{{{v}_{x}}}^{2} & {{\sigma }_{{{v}_{x}}{{v}_{y}}}} \\ {{\sigma }_{{{v}_{y}}{{p}_{x}}}} & {{\sigma }_{{{v}_{y}}{{p}_{y}}}} & {{\sigma }_{{{v}_{y}}{{v}_{x}}}} & \sigma _{{{v}_{y}}}^{2} \\ \end{matrix} \right]$
Q表示的是噪声 $[12ax△t212ay△t2ax△tay△t]T{{\left[ \begin{matrix} \tfrac{1}{2}{{a}_{x}}\vartriangle {{t}^{2}} & \tfrac{1}{2}{{a}_{y}}\vartriangle {{t}^{2}} & {{a}_{x}}\vartriangle t & {{a}_{y}}\vartriangle t \\ \end{matrix} \right]}^{T}}$ 各个变量之间的协方差，假设 $~N(0,a)p(ax)\tilde{\ }N(0,a)$ , $~N(0,b)p(ay)\tilde{\ }N(0,b)$ ,则此时Q表示如下：
$Q=[14ax△t4012ax△t30014ay△t4012ay△t312ax△t30ax△t20012ay△t30ay△t2]Q=\left[ \begin{matrix} \tfrac{1}{\text{4}}{{a}_{x}}\vartriangle {{t}^{\text{4}}} & \text{0} & \tfrac{1}{\text{2}}{{a}_{x}}\vartriangle {{t}^{\text{3}}} & \text{0} \\ \text{0} & \tfrac{1}{\text{4}}{{a}_{y}}\vartriangle {{t}^{\text{4}}} & 0 & \tfrac{1}{\text{2}}{{a}_{y}}\vartriangle {{t}^{\text{3}}} \\ \tfrac{1}{\text{2}}{{a}_{x}}\vartriangle {{t}^{\text{3}}} & 0 & {{a}_{x}}\vartriangle {{t}^{\text{2}}} & 0 \\ 0 & \tfrac{1}{\text{2}}{{a}_{y}}\vartriangle {{t}^{\text{3}}} & 0 & {{a}_{y}}\vartriangle {{t}^{\text{2}}} \\ \end{matrix} \right]$

②update

假设此时测量值来自于里程计，即测量设备是底层光电编码器，则 ${{v}_{x}}$ ${{v}_{y}}$ 是可观测的,观测矩阵H表示如下：
$H=[00100001]H=\left[ \begin{matrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]$
注意：状态观测矩阵，与建立的运动模型和传感器都有关系，在该例子中CV模型下，如果传感器是里程计，则观测值为速度；如果传感器是激光雷达则观测值应该为位置xy，如果是毫米波雷达的话，由于毫米波雷达的输出结果是极坐标下的结果不是x,y,vx,vy,转换公式为非线性的，所以观测矩阵应该计算其雅可比矩阵。

测量噪声R表示如下：
$R=[σVX200σVY2]R=\left[ \begin{matrix} \sigma _{{{V}_{X}}}^{2} & 0 \\ 0 & \sigma _{{{V}_{Y}}}^{2} \\ \end{matrix} \right]$
测量噪声是测量传感器的性能指标，一般可由生产厂家提供。