卡尔曼(Kalman)滤波 C++

最新推荐文章于 2025-05-28 20:52:42 发布

初中就开始混世的大魔王

最新推荐文章于 2025-05-28 20:52:42 发布

阅读量1.4k

点赞数 2

CC 4.0 BY-SA版权

分类专栏：算法文章标签：算法

本文链接：https://blog.csdn.net/weixin_38799245/article/details/128192518

算法专栏收录该内容

1 篇文章

订阅专栏

本文通过一个具体的行人跟踪场景介绍了卡尔曼滤波的基本原理及应用。利用恒定速度模型预测行人的位置变化，并通过实际传感器数据不断修正预测结果，展示了卡尔曼滤波算法在状态估计中的强大能力。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

关于卡尔曼滤波介绍请看：卡尔曼滤波_随风张幔的博客-CSDN博客_卡尔曼滤波

一、卡尔曼滤波实例：

例如有这样的一个场景：无人车正常前行时前方突然出现一个行人，无人车就需要对前方行人的行为动作做一个预估和判断，以采取合适的避障或停车策略。如果想要估计一个行人的运动状态，首先需要建立被估计对象的状态方程表达式。人的状态可以用数学方程表示为 $x=(p,v)$ ，其中 $p$ 为行人的当前位置，而 $v$ 则是行人当前的速度。用一个向量来表示一个状态：

$x=(p_{x},p_{y},v_{x},v_{y})^{T}$

上述方程表示了行人的 $x,y$ 方向的位置分量 $p_{x},p_{y}$ ，以及速度分量 $v_{x},v_{y}$ 。在确定了想要估计的对象的状态后，还需要一个用于生成一个当前状态估计的过程模型。下面先以一个最简单的过程模型---恒定速度模型（即先假设行人速度是匀速的）来解释卡尔曼滤波算法的应用，假设过程模型为：

$x_{k+1}=Fx_{k}+v$

扩展为：

$x_{k+1}=\begin{pmatrix} 1 & 0& \Delta t& 0\\ 0 & 1& 0& \Delta t\\ 0 & 0& 1& 0\\ 0 & 0& 0& 1 \end{pmatrix}\cdot \begin{pmatrix} p_{x}\\ p_{y}\\ v_{x}\\ v_{y} \end{pmatrix}_{k}+v$

之所以称之为恒定速度模型，是因为将上面这个行列式展开可以得到：

$p_{x}^{k+1}=p_{x}^{k}+v_{x}^{k}\Delta t+v$

$p_{y}^{k+1}=p_{y}^{k}+v_{y}^{k}\Delta t+v$

$v_{x}^{k+1}=v_{x}^{k}+v$

$v_{y}^{k+1}=v_{y}^{k}+v$

恒定速度过程模型假定预测目标的运动规律是具有恒定的速度，在行人状态预测这个问题中，行人并不一定会以恒定的速度运动，所以过程模型还包含了一定的过程噪声，在这个问题中过程噪声也被考虑了近来，其中 $v$ 是这个模型的过程噪声。在行人状态估计中的过程噪声其实就是行人突然的加减速度，考虑到行人的加速度因素，那么原来的模型表达式就变成了：

$x_{k+1}=\begin{pmatrix} 1 & 0& \Delta t& 0\\ 0 & 1& 0& \Delta t\\ 0 & 0& 1& 0\\ 0 & 0& 0& 1 \end{pmatrix}\cdot \begin{pmatrix} p_{x}\\ p_{y}\\ v_{x}\\ v_{y} \end{pmatrix}_{k}+\begin{pmatrix} \frac{1}{2}a_{x}\Delta t^{2}\\ \frac{1}{2}a_{y}\Delta t^{2}\\ a_{x}\Delta t\\ a_{y}\Delta t \end{pmatrix}_{k}$

那么根据预测误差，第二步就变成了：

$F^{'}=FPF^{T}+Q$

上述 $Q$ 是过程噪声的协方差矩阵。这是状态 $F$ 的更新过程，它本质上是估计状态概率分布的协方差矩阵。由于过程噪声是随机带入的，所以过程噪声 $v$ 本质上是一个高斯分布： $v~N(0,Q)$ ， $Q$ 的展开形式为：

$Q=\begin{pmatrix} \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{pmatrix}$

如果定义 $G=[0.5\Delta t^{2} , 0.5\Delta t^{2} , \Delta t , \Delta t]^{T}$ ，进而简化为：

$Q=G\cdot G^{T}\cdot \sigma _{v}^{2}$

其中， $\sigma _{v}^{2}$ 对于行人，假设其速度大约为 $0.5m/s^{2}$ 。

在测量步骤中，使用传感器可以直接测量行人的速度 $v_{x}$ 和 $v_{y}$ ，所以根据状态表达式，测量矩阵 $H$ 可以表示为：

$H=\begin{pmatrix} 0 & 0 & 1 & 0\\ 0 & 0& 0 & 1 \end{pmatrix}$

测量噪声的协方差矩阵 $R$ 为：

$R=\begin{pmatrix} \sigma _{v_{x}}^{2} & 0\\ 0 & \sigma _{v_{y}}^{2} \end{pmatrix}$

其中 $\sigma _{v_{x}}^{2}$ 和 $\sigma _{v_{y}}^{2}$ 描述了传感器的测量能够有多差，这是传感器故有的性质，所以往往由传感器厂商提供。最后，求得 $P_{k}=(I-K_{k}H)P_{k}^{'}$

二、卡尔曼滤波过程

三、代码实现

//
// Created by kangyu on 22-11-19.
//
#include "eigen3/Eigen/Eigen"
#include "eigen3/Eigen/SVD"
#include <iostream>

template<typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon =
                                                        std::numeric_limits<double>::epsilon())
{
  Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
  double tolerance = epsilon * std::max(a.cols(), a.rows()) *svd.singularValues().array().abs()(0);
  return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
}

int main(int argc, char **argv) {
  Eigen::MatrixXd x(4, 1); // 初始化行人状态，包括x，y方向位置和速度
  x << 0.0, 0.0, 0.0, 0.0;
  // 创建行人的不确定因素（先验估计协方差矩阵）
  Eigen::MatrixXd P(4 , 4);
  P << 1000.0 , 0 , 0 , 0 ,
      0 , 1000.0 , 0 , 0 ,
      0 , 0 , 1000.0 , 0 ,
      0 , 0 , 0 , 1000.0;
  // 测量时间间隔
  double dt = 0.1;
  // 创建状态转移矩阵
  Eigen::MatrixXd F(4, 4);
  F << 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
      1.0;
  // 创建观测矩阵
  Eigen::MatrixXd H(2, 4);
  H << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0;

  // 创建噪声协方差矩阵
  double ra = 0.09;
  Eigen::MatrixXd R(2, 2);
  R << ra, 0.0, 0.0, ra;

  // 创建过程噪声协方差矩阵
  double sv = 0.5; // 对于行人假设其速度大约为0.5m/s^2
  Eigen::MatrixXd G(4, 1);
  G << 0.5 * dt * dt, 0.5 * dt * dt, dt, dt;
  // 计算过程噪声协方差矩阵（即Px,Py,Vx,Vy协方差矩阵）
  Eigen::MatrixXd Q(4, 4);
  Q = G * G.transpose() * sv * sv;

  // 生成随机测量数据矩阵
  double mx[200] = {
      19.06138281, 20.7184095,  19.6003431,  20.64342422, 20.46436535,
      21.01724727, 19.87663378, 20.33254664, 20.83507762, 20.64791878,
      20.61129075, 20.55230186, 20.33422575, 20.81940445, 22.40115688,
      19.57007029, 18.85635776, 20.1101804,  21.45325898, 19.40465207,
      21.72706313, 20.69161843, 20.51639862, 20.54641995, 20.60180303,
      22.86223673, 19.44850942, 20.34627601, 21.12441095, 20.75323951,
      19.49980249, 20.24403407, 19.63073918, 21.57295219, 20.58523326,
      19.27400384, 19.47394214, 20.49479234, 19.1005685,  22.20494222,
      19.1945908,  19.01948288, 19.1725428,  19.33506097, 18.84559488,
      20.74153002, 19.15825133, 18.15083889, 20.01479679, 20.07979508,
      17.19799612, 19.68258375, 19.44984035, 19.2789717,  18.39087735,
      22.26248573, 20.63563033, 19.83847437, 18.5529162,  20.28583306,
      20.86017331, 19.7079787,  19.93148566, 22.2344179,  18.46744059,
      20.15576002, 21.5010283,  18.96322784, 18.12601612, 19.63885769,
      20.70539448, 19.30191813, 19.82980692, 17.72405939, 19.42412227,
      20.04811686, 20.81232539, 17.38993418, 18.52665261, 19.47803242,
      20.22105898, 20.68651507, 20.33884377, 20.84508251, 19.32011028,
      19.74006068, 19.26293954, 18.28897104, 18.87564706, 18.87217767,
      19.84896293, 20.25672685, 17.24549546, 20.83429221, 19.16013752,
      21.13353013, 18.96042591, 18.97720576, 20.64428335, 21.09215634,
      20.87725159, 19.74354243, 18.94466175, 20.09120071, 19.82496588,
      18.86031243, 19.38423827, 21.31653287, 19.7188618,  20.62047658,
      19.70550894, 19.01107295, 20.51021898, 19.68738768, 21.68756858,
      18.60478201, 18.67907061, 20.53768353, 20.06770123, 21.20523063,
      20.55177313, 20.78297782, 20.55402309, 18.85576377, 19.17892994,
      21.3855882,  18.52658166, 18.54339775, 20.70191295, 20.1528614,
      19.71426421, 19.07467611, 20.1777376,  19.557498,   21.56071554,
      21.19102938, 21.17970749, 18.83272093, 19.04331014, 20.02933802,
      20.03285167, 19.02457716, 20.00311467, 20.45059907, 20.55538089,
      21.22642523, 19.55811172, 18.45759434, 18.24662843, 19.18976387,
      20.17654908, 19.63656764, 19.61575094, 19.48319031, 19.82130519,
      19.33379757, 21.98405404, 19.72623731, 21.47471626, 20.50805392,
      20.44275725, 20.15125191, 18.64170151, 20.34954599, 20.98629904,
      20.28612748, 20.37581927, 19.28556512, 19.60249147, 19.3352152,
      20.38730756, 20.57569008, 20.52676791, 20.20728447, 20.41117994,
      20.68174744, 20.12918022, 19.09657057, 20.56260842, 19.81516068,
      22.27060708, 19.68444856, 21.6634146,  21.39108902, 21.24004727,
      17.89540798, 21.81263175, 18.81153216, 19.05879304, 20.59653657,
      20.17869052, 18.45946935, 18.95715967, 22.00879053, 21.00680329,
      19.97526522, 18.99965754, 21.08618178, 20.14911999, 18.95116079};
  double my[200] = {
      10.5470085,  10.40662542, 8.520953,    11.90872495, 9.47020574,
      9.56015845,  8.67267109,  8.86253219,  10.34010783, 10.02473774,
      8.22830326,  11.25676861, 7.83368774,  9.13774229,  11.31318839,
      8.75218424,  9.30711761,  11.18987146, 9.81326459,  9.88443584,
      10.68399146, 10.53767552, 8.39679905,  10.35833614, 9.12108005,
      11.40088701, 8.85493549,  10.26561276, 7.69215717,  10.31957316,
      9.04900644,  9.97648845,  10.75595865, 9.5445555,   10.92264117,
      10.51514597, 10.53558985, 8.80524405,  10.5543631,  8.92934421,
      9.42992494,  9.35971582,  10.58067664, 9.17799631,  9.10950372,
      10.94416336, 9.72518059,  9.56366617,  10.91082336, 10.7657096,
      8.83647039,  9.04842366,  8.91716678,  10.06001019, 10.20821141,
      10.27334634, 9.71745559,  10.58813907, 9.37547148,  10.81447524,
      10.24191296, 10.66409497, 8.49713991,  10.38355593, 9.2642231,
      9.61997387,  9.22882967,  9.85439277,  10.62010222, 9.96847195,
      10.58376835, 8.73555447,  8.64286605,  10.32287374, 10.63717923,
      11.70160519, 8.68427338,  9.79692825,  9.17883424,  11.68260586,
      10.78130577, 8.40547097,  9.09149288,  12.06141114, 9.98008944,
      9.43591795,  8.09034767,  9.28460981,  9.82941895,  9.61528355,
      10.23595969, 11.76445818, 11.58526395, 10.39227337, 10.09406473,
      10.77673885, 10.25269676, 9.51742757,  12.44971609, 9.3410574,
      10.5900303,  9.17397154,  10.75227525, 9.43715847,  9.94094229,
      10.6478532,  9.68270405,  10.24344286, 11.63711753, 10.10172998,
      9.64172328,  10.07657778, 10.3962835,  13.07077278, 9.16328842,
      9.46034924,  10.85302638, 11.32165567, 11.37931155, 9.42041489,
      9.78567032,  12.03355157, 9.62951601,  9.59950392,  9.61630805,
      11.14768071, 10.6712395,  10.08117998, 9.23451727,  9.17395926,
      11.26414313, 9.96424371,  10.48502564, 10.54096245, 7.67364863,
      9.77873417,  10.34831484, 9.78123532,  9.37860415,  8.48598973,
      9.49922156,  9.59036905,  12.9097515,  8.29802419,  11.01354666,
      9.0268258,   11.03561853, 10.68399364, 8.10421364,  6.32615615,
      8.80423588,  9.30618394,  10.52209332, 10.42287621, 10.51404334,
      11.19037181, 9.09831787,  11.93951518, 10.3906541,  9.28951338,
      10.26645749, 11.06035912, 10.23423786, 8.58025796,  9.69785568,
      12.25471631, 9.93013928,  9.53684647,  9.85494632,  10.18423726,
      11.15562792, 8.95057647,  12.15125943, 10.80864003, 9.56507666,
      8.86600217,  11.16982669, 10.8159668,  8.56132852,  8.6785302,
      8.14937,     11.32804185, 11.04300848, 9.85190524,  10.21779083,
      9.25806741,  8.38464321,  10.26177116, 10.97992672, 10.9080179,
      8.80363233,  9.46018494,  10.24589774, 10.35012901, 9.97076904,
      8.63556933,  11.3627808,  10.07084812, 10.59386879, 11.01464601};
  int len = sizeof(mx) / sizeof (double);
  for(int i = 0 ; i < len ; i++){
    // 预测未来状态
    x = F * x;
    // 预测误差协方差矩阵
    P = F * P * F.transpose() + Q;

    // 测量更新
    // 计算卡尔曼增益
    Eigen::MatrixXd S(2, 4);
    S = H * P * H.transpose() + R;
    Eigen::MatrixXd K(4 , 2);
    K = (P * H.transpose()) * pseudoInverse(S);

    // 更新观测值Z
    Eigen::MatrixXd Z(2 , 1);
    Z << mx[i] , my[i];
    Eigen::MatrixXd y(2 , 1);
    y = Z - (H * x);
    x = x + (K * y);
    //std::cout << x << std::endl;
    // 更新协方差矩阵
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(4 , 4);
    P = (I - (K * H)) * P;
    std::cout << x << std::endl;
    std::cout << "-------------------" << std::endl;
  }
}