卡尔曼滤波工作原理
简单来讲，卡尔曼滤波器就是根据上一时刻的状态，预测当前时刻的状态，将预测的状态与当前时刻的测量值进行加权，加权后的结果才认为是当前的实际状态，而不是仅仅听信当前的测量值。
初始化（Initialization）
假设有个小车在道路上向右侧匀速运动，我们在左侧安装了一个测量小车距离和速度传感器，传感器每1秒测一次小车的位置s和速度v，如下图所示。
我们用向量xt来表示当前小车的状态，该向量也是最终的输出结果，被称作状态向量（state vector）：
由于测量误差的存在，传感器无法直接获取小车位置的真值，只能获取在真值附近的一个近似值，可以假设测量值在真值附近服从高斯分布。如下图所示，测量值分布在红色区域的左侧或右侧，真值则在红色区域的波峰处。
由于是第一次测量，没有小车的历史信息，我们认为小车在1秒时的状态x与测量值z相等，表示如下：
公式中的1表示第1秒。
预测（Prediction）
预测是卡尔曼滤波器中很重要的一步，这一步相当于使用历史信息对未来的位置进行推测。根据第1秒小车的位置和速度，我们可以推测第2秒时，小车所在的位置应该如下图所示。会发现，图中红色区域的范围变大了，这是因为预测时加入了速度估计的噪声，是一个放大不确定性的过程。
根据小车第一秒的状态进行预测，得到预测的状态xpre：
其中，pre是prediction的简称；时间间隔为1秒，所以预测位置为距离+速度*1；由于小车做的是匀速运动，因此速度保持不变。
观测（Measurement）
在第2秒时，传感器对小车的位置做了一次观测，我们认为小车在第2秒时观测值为z2，用向量表示第2秒时的观测结果为：
很显然，第二次观测的结果也是存在误差的，我们将预测的小车位置与实际观测到的小车位置放到一个图上，即可看到：
图中红色区域为预测的小车位置，蓝色区域为第2秒的观测结果。
很显然，这两个结果都在真值附近。为了得到尽可能接近真值的结果，我们将这两个区域的结果进行加权，取加权后的值作为第二秒的状态向量。
为了方便理解，可以将第2秒的状态向量写成：
其中，w1为预测结果的权值，w2为观测结果的权值。两个权值的计算是根据预测结果和观测结果的不确定性来的，这个不确定性就是高斯分布中的方差的大小，方差越大，波形分布越广，不确定性越高，这样一来给的权值就会越低。
加权后的状态向量的分布，可以用下图中绿色区域表示：
你会发现绿色区域的方差比红色区域和蓝色区域的小。这是因为进行加权运算时，需要将两个高斯分布进行乘法运算，得到的新的高斯分布的方差比两个做乘法的高斯分布都小。
两个不那么确定的分布，最终得到了一个相对确定的分布，这是卡尔曼滤波的一直被推崇的原因。
再预测，再观测（Prediction & Measurement）
第1秒的初始化以及第2秒的预测、观测，实现卡尔曼滤波的一个周期。
同样的，我们根据第2秒的状态向量做第3秒的预测，再与第3秒的观测结果进行加权，就得到了第3秒的状态向量；再根据第3秒的状态向量做第4秒的预测，再与第4秒的观测结果进行加权，就得到了第4秒的状态向量。以此往复，就实现了一个真正意义上的卡尔曼滤波器。
以上就是卡尔曼滤波器的感性分析过程，下面我们回归理性，谈谈如何将以上过程写成代码。
卡尔曼滤波器的理论
在这里就要祭出卡尔曼老先生给我们留下的宝贵财富了，下面7个公式就是卡尔曼滤波器的理性描述，使用下面7个公式，就能够实现一个完整的卡尔曼滤波器。现在看不懂这7个公式没关系，继续往下看，我会一个一个做解释。
代码实现
代码：初始化（Initialization）
在Initialization这一步，需要将各个变量初始化，对于不同的运动模型，其状态向量肯定是不一样的，比如前文小车的例子，只需要一个距离s和一个速度v就可以表示小车的状态；再比如在一个2维空间中的点，需要x方向上的距离和速度以及y方向上的距离和速度才能表示，这样的状态方程就有4个变量。因此我们使用Eigen库中非定长的数据结构，下图中的VerctorXd表示X维的列矩阵，其中的元素数据类型为double。
在这里，我们新建了一个KalmanFilter类，其中定义了一个叫做x_的变量，表示这个卡尔曼滤波器的状态向量。
代码：预测（Prediction）
完成初始化后，我们开始写Prediction部分的代码。首先是公式
这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x’。这里的F成为状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为
对于匀速运动模型，根据中学物理课本中的公式s1 = s0 + vt，经过时间△t后的预测状态向量应该是
由于假设当前运动为匀速运动，加速度为0，加速度不会对预测造成影响，即
如果换成加速或减速运动模型，就可以引入加速度a_x和a_y，根据s1 = s0 + vt + at^2/2这里的u会变成：
作为入门课程，这里不讨论太复杂的模型，因此公式
最终将写成
由于每次做预测时，△t的大小不固定，因此我们专门写一个函数SetF()
再看预测模块的第二个公式
该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；这里的Q表示过程噪声（process covariance matrix），即无法用x’=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的。
以激光雷达为例。激光雷达只能测量点的位置，无法测量点的速度，因此对于激光雷达的协方差矩阵来说，对于位置信息，其测量位置较准，不确定度较低；对于速度信息，不确定度较高。因此可以认为这里的P为：
由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。工程上，我们一般将Q设置为单位矩阵参与运算，即
根据以上内容和公式
我们就可以写出预测模块的代码了
代码：观测（Measurement）
观测的第一个公式是
这个公式计算的是实际观测到的测量值z与预测值x’之间差值y。不同传感器的测量值一般不同，比如激光雷达测量的位置信号为x方向和y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态向量左乘一个矩阵H，才能与测量值进行相应的运算，这个H被称为测量矩阵（Measurement Matrix）。
激光雷达的测量值为
其中xm和ym分别表示x方向上的测量（measurement）值。
由于x’是一个41的列向量，如果要与一个21的列向量z进行减运算，需要左乘一个2*4的矩阵才行，因此整个公式最终要写成：
即，对于激光雷达来说，这里的测量矩阵H为
求得y值后，对y值乘以一个加权量，再加到原来的预测量上去，就可以得到一个既考虑了测量值，又考虑了预测模型的位置的状态向量了。
那么y的这个权值该如何取呢？
再看接下里的两个公式
这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量，不要太在意。
看最后两个公式
这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。
将以上五个公式写成代码如下：
至此，一个卡尔曼滤波器的雏形就出来了。
完整代码
头文件
//
// Created by user1 on 2020/2/25.
//
#ifndef KALMA_KALMAT_H
#define KALMA_KALMAT_H
#include "Eigen/Dense"
class KalmanFilter{
public:
KalmanFilter()
{
is_initialized_=false;
}
~KalmanFilter();
void Initialization(Eigen::VectorXd x_in){//x_in 是一个x维的向量
x_=x_in;
is_initialized_=true;
}
bool IsInitialized()
{
return is_initialized_;
}
Eigen::VectorXd GetX()
{
return x_;
}
void SetF(Eigen::MatrixXd F_in)
{
F_=F_in;
}
void SetP(Eigen::MatrixXd P_in)
{
P_=P_in;
}
void SetQ(Eigen::MatrixXd Q_in)
{
Q_=Q_in;
}
void SetH(Eigen::MatrixXd H_in)
{
H_=H_in;
}
void SetR(Eigen::MatrixXd R_in)
{
R_=R_in;
}
void Prediction()
{
x_=F_*x_;
Eigen::MatrixXd Ft=F_.transpose();
P_=F_*P_*Ft+Q_;//P_是不确定性
}
void MeasurementUpdate(const Eigen::VectorXd &z)//z是测量值
{
Eigen::VectorXd y=z-H_*x_;
Eigen::MatrixXd S=H_*P_*H_.transpose()+R_;
Eigen::MatrixXd K=P_*H_.transpose()*S.inverse();
x_=x_+(K*y);//K相当于是权值
int size=x_.size();
Eigen::MatrixXd I=Eigen::MatrixXd::Identity(size,size);
P_=(I-K*H_)*P_;
}
private:
bool is_initialized_;
Eigen::VectorXd x_;//卡尔曼滤波器的状态向量 输入值 可能是[x,y,vx,vy]这种形式
Eigen::MatrixXd F_;//状态转移矩阵 预设值
Eigen::MatrixXd P_;//状态协方差矩阵 其实就是根据各个输入状态的测量信息的不确定性预设的矩阵 不确定性越大矩阵值越大
Eigen::MatrixXd Q_;//噪声矩阵 一般设为单位阵即可
Eigen::MatrixXd H_;//测量矩阵 用来把预测值x_的维度调整成和z一样，并有相同的物理意义
Eigen::MatrixXd R_;//测量噪声矩阵 传感器自带
};
#endif //KALMA_KALMAT_H
main函数
#include <iostream>
#include <Eigen/Dense>
#include "kalmat.h"
using namespace Eigen;
using namespace std;
int main()
{
double m_x=0.0,m_y=0.0;
double last_timestamp=0.0,now_timestamp=0.0;
KalmanFilter kf;
while (GetLidarData(m_x,m_y,now_timestamp))
{
if (!kf.IsInitialized())
{
last_timestamp=now_timestamp;
Eigen::VectorXd x_in(4,1);
x_in<<m_x,m_y,0,0,0,0;
kf.Initialization(x_in);
// 状态协方差矩阵
Eigen::MatrixXd P_in(4,4);
P_in<<1.0,0.0,0.0,0.0,
0.0,1.0,0.0,0.0,
0.0,0.0,100.0,0.0,
0.0,0.0,0.0,100.0;
kf.SetP(P_in);
Eigen::MatrixXd Q_in(4,4);
Q_in<<1.0,0.0,0.0,0.0,
0.0,1.0,0.0,0.0,
0.0,0.0,1.0,0.0,
0.0,0.0,0.0,1.0;
kf.SetQ(Q_in);
Eigen::MatrixXd H_in(2,4);
H_in<<1.0,0.0,0.0,0.0,
0.0,1.0,0.0,0.0;
kf.SetH(H_in);
Eigen::MatrixXd R_in(2,2);
R_in<<0.0225,0.0,
0.0,0.0225;
kf.SetR(R_in);
}
double_t delta_t=now_timestamp-last_timestamp;
last_timestamp=now_timestamp;
Eigen::MatrixXd F_in(4,4);
F_in<<1.0,0.0,delta_t,0.0,
0.0,1.0,0.0,delta_t,
0.0,0.0,1.0,0.0,
0.0,0.0,0.0,1.0;
kf.SetF(F_in);
kf.Prediction();
Eigen::VectorXd z(2,1);
z<<m_x,m_y;
kf.MeasurementUpdate(z);
//get result
Eigen::VectorXd x_out =kf.GetX();
std::cout<<"kalman output x:"<<x_out(0)<<" y: "<<x_out(1)<<std::endl;
}
}
其中GetLidarData函数除了获取点的位置信息m_x和m_y外，还获取了当前时刻的时间戳，用于计算前后两帧的时间差delta_t。
以上就是卡尔曼滤波器对于匀速运动物体跟踪的例子。在这个基础上，业内还有扩展卡尔曼滤波器和无迹卡尔曼滤波器，它们与经典卡尔曼滤波器的最大区别是状态转移矩阵F和测量矩阵H的不同，剩下的跟踪过程依然需要使用前面介绍的7个公式。
只要你能够写出某个模型的F、P、Q、H、R矩阵，任何状态跟踪的问题都将迎刃而解。
525
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
