前言
LZ最近在看Udacity的无人驾驶课程,该课程主要分为三部分,第一部分的课程主要使用Python实现的车道线识别、车牌识别等计算机视觉项目。由于我对定位、建图等方面有些知识储备,所以先从第二部分课程开始。
本节将用最简洁的话讲解卡尔曼滤波KF、非线性卡尔曼滤波EKF等知识点,并就此实现一个多传感器融合定位的小demo,后面会就粒子滤波PF专门开一个章节讲解。
对于匀速运动模型,KF和EKF的状态预测(Predict)过程是一样的;KF和EKF唯一区别的地方在于测量值更新(Update)这一步。在测量值更新中,KF使用的测量矩阵H是不变的,而EKF的测量矩阵H是Jacobian矩阵。
本文约束:匀速运动;固定位置的传感器对小车不断测量。
一、卡尔曼滤波 KF
1、引子
1)两个传感器测量同一个信号,为了减小误差我们可以采用取平均的方式,
2)进一步的我们采用加权平均(由方差大小分配),但加权平均是一种静态分配方式。3)方差是随外界环境而变的,加权值也应该随之改变,这就是卡尔曼滤波出现的原因,它是一种动态更新加权值,不断迭代的算法。
卡尔曼滤波器就是根据上一时刻x(k-1)的状态,预测当前时刻x(k)的状态,将预测的状态x^(k)与当前时刻的测量值z进行加权,加权后的结果才认为是当前的实际状态。
2、对象:线性高斯模型,这个模型最好的地方在于两个高斯分布的乘积仍然是一高斯分布,由此实现模型的动态迭代。
3、本质:参数化的贝叶斯模型。先验估计:对下一时刻系统初步状态估计;结合先验估计以及测量反馈,得到后验估计。
4、应用:最优化自回归数据处理算法、机器人导航、雷达系统、图像处理等
2、黄金公式及推导
直接上结论,下面这个公式分为两部分,上面的是预测,下面是更新。
具体公式推导,我竟然发现了我之前的笔记,下面直接上图解释。
为了避免推导晦涩难懂,采用小汽车的模型进行一步一步的推导。
1、建立模型
2、简单一维小车
假设有个小车匀速运动,我们在左侧安装了一个测量小车距离和速度传感器,每秒测一次小车位置s和速度v。我们用向量
x
t
x_{t}
xt状态向量。
由于测量误差的存在,无法获得真实值,我们可以以一个高斯分布来表示结果在各个地方出现的概率,如下图所示,在真值附近的概率最高。
x
t
−
1
=
[
s
t
−
1
v
t
−
1
]
x_{t-1}=\begin{bmatrix} s_{t-1}\\ v_{t-1} \end{bmatrix}
xt−1=[st−1vt−1]
接下来是使用历史信息对未来的位置进行推测,以速度v匀速行走
Δ
t
\Delta t
Δt后,小车位置的红色区域范围变大了,这是因为预测时加入了速度估计的噪声,放大了不确定性。
[
s
t
v
t
]
=
[
1
Δ
t
0
1
]
[
s
t
−
1
v
t
−
1
]
\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}
[stvt]=[10Δt1][st−1vt−1]
此时传感器对小车做了一次观测,结果为 z
上图蓝色区域为此时观测结果,红色为预测结果,那么最终的结果是怎样的呢?下图绿色部分给出了答案。
我们对预测和观测结果用不同的权重得到最终结果,两个权值的计算是根据预测结果和观测结果的不确定性来的,这个不确定性就是高斯分布中的方差的大小,方差越大,波形分布越广,不确定性越高,这样一来给的权值就会越低。
3、预测(Predict)
上面是一维小车,状态向量里只有它的位置和速度,当二维小车在平面时它的状态向量有分别是位置和速度的x、y方向(x,y,vx,vy)
x
=
[
x
y
v
x
v
y
]
x_{}=\begin{bmatrix}x\\ y\\vx\\vy\end{bmatrix}
x=⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
和上面的一维对应
[
s
t
v
t
]
=
[
1
Δ
t
0
1
]
[
s
t
−
1
v
t
−
1
]
\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}
[stvt]=[10Δt1][st−1vt−1]
这里的二维应该是
[
x
t
y
t
v
x
t
v
y
t
]
=
[
1
0
Δ
t
0
0
1
0
Δ
t
0
0
1
0
0
0
0
1
]
[
x
t
−
1
y
t
−
1
v
x
t
−
1
v
y
t
−
1
]
\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}=\begin{bmatrix} 1& 0 & \Delta t& 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1 \end{bmatrix}\begin{bmatrix}x_{t-1}\\ y_{t-1}\\vx_{t-1}\\vy_{t-1}\end{bmatrix}
⎣⎢⎢⎡xtytvxtvyt⎦⎥⎥⎤=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⎣⎢⎢⎡xt−1yt−1vxt−1vyt−1⎦⎥⎥⎤+Bu
接下来是预测的第二个模块,状态协方差矩阵P和过程噪声Q。P表示系统的不确定程度,卡尔曼滤波器初始化时很大,随着更多数据注入滤波器,不确定度会逐渐变小。Q暂时设为单位矩阵。
P
=
[
1
0
0
0
0
1
0
0
0
0
100
0
0
0
0
100
]
P=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 100 & 0\\ 0 & 0& 0 & 100 \end{bmatrix}
P=⎣⎢⎢⎡10000100001000000100⎦⎥⎥⎤
Q
=
[
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
]
Q=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1\end{bmatrix}
Q=⎣⎢⎢⎡1000010000100001⎦⎥⎥⎤
4、观测更新(measurement update)
先说第一个公式括号里面的z-Hx,z表示的是测量值,x是4维的状态向量,对于激光雷达测量的z就是位置(x,y),对于毫米波雷达测量的z是位置和角度。需要对状态向量x左乘一个矩阵H,才能将二者映射到同一个空间,直接进行加减运算。
以激光雷达的测量值z为:
z
=
[
x
m
y
m
]
z=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}
z=[xmym]
z-Hx扩展开为:
[
Δ
x
Δ
y
]
=
[
x
m
y
m
]
−
[
1
0
0
0
0
1
0
0
]
∗
[
x
t
y
t
v
x
t
v
y
t
]
\begin{bmatrix}\Delta x\\\Delta y\end{bmatrix}=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}-\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}*\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}
[ΔxΔy]=[xmym]−[10010000]∗⎣⎢⎢⎡xtytvxtvyt⎦⎥⎥⎤
测量矩阵 H 是一个2*4的矩阵
H
=
[
1
0
0
0
0
1
0
0
]
H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}
H=[10010000]
H_lidar_ = Eigen::MatrixXd(2, 4);
H_lidar_ << 1, 0, 0, 0,
0, 1, 0, 0;
接下来说K(z-Hx)中的权重K是如何取的,简单来说是和协方差矩阵相关。
R_lidar_ = Eigen::MatrixXd(2, 2);
R_lidar_ << 0.0225, 0,
0, 0.0225;
这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K(Kalman Gain),用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵(measurement covariance matrix),这个表示的是测量值与真值之间的差值,R应该是2*2的矩阵。一般情况下,传感器的厂家会提供该值。
求得K之后,当前时刻的x和P都可以求解出来了,第一个公式是完成了当前状态向量x的更新,不仅考虑了上一时刻的预测值,也考虑了测量值,和整个系统的噪声,第二个公式根据卡尔曼增益,更新了系统的不确定度P,用于下一个周期的运算。
二、非线性卡尔曼滤波EKF
KF中测量矩阵H是固定的,代码中直接赋值即可。
H
=
[
1
0
0
0
0
1
0
0
]
H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}
H=[10010000]
但是真实情况下,测量矩阵 H应该是需要求雅克比矩阵求出的。我们接下来以毫米波雷达为例
毫米波原理是多普勒效应,能够测量障碍物在极坐标系下雷达的距离ρ,方向角ϕ和距离的变化率(径向速度)ρ’。
已知量:状态向量x为4*1,
已知量:观测值z的数据维度为3*1.
观测值z和预测值x之间的差值y关系为:
但是如果转化为 y=Hx时,由于第二部分的转化是非线性的,无法找到一个常数矩阵H。
所以我们需要将上述非线性函数转化为近似的线性函数,用一阶泰勒展开。对状态向量x求偏导数,即雅克比Jacobian矩阵。
三、代码讲解
KF和EKF都在kalmanfilter.h/cpp文件中,头文件KalmanFilter类为:
class KalmanFilter {
public:
KalmanFilter();
~KalmanFilter();
void Initialization(Eigen::VectorXd x_in);
bool IsInitialized();
void SetF(Eigen::MatrixXd F_in);
void SetP(Eigen::MatrixXd P_in);
void SetQ(Eigen::MatrixXd Q_in);
void SetH(Eigen::MatrixXd H_in);
void SetR(Eigen::MatrixXd R_in);
void Prediction();
void KFUpdate(Eigen::VectorXd z);
void EKFUpdate(Eigen::VectorXd z);
Eigen::VectorXd GetX();
private:
void CalculateJacobianMatrix();
bool is_initialized_;
Eigen::VectorXd x_;
Eigen::MatrixXd P_;
Eigen::MatrixXd F_;
Eigen::MatrixXd Q_;
Eigen::MatrixXd H_;
Eigen::MatrixXd R_;
};
主要成员变量为:状态转移矩阵F、状态协方差矩阵P、测量矩阵H、过程噪声Q、测量噪声R、状态向量x
主要成员函数为:初始化Initialization()、5个矩阵赋值set()、预测prediction()、KF更新KFUpdate()、EKF更新EKFUpdate().
1、预测Prediction()
void KalmanFilter::Prediction()
{
x_ = F_ * x_;
Eigen::MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}
2、KF更新KFUpdate()
void KalmanFilter::KFUpdate(Eigen::VectorXd z)
{
Eigen::VectorXd y = z - H_ * x_;
Eigen::MatrixXd Ht = H_.transpose();
Eigen::MatrixXd S = H_ * P_ * Ht + R_;
Eigen::MatrixXd Si = S.inverse();
Eigen::MatrixXd K = P_ * Ht * Si;
x_ = x_ + (K * y);
int x_size = x_.size();
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
3、EKF更新EKFUpdate()
EKF的预测两个公式和之前一样,都是Prediction()函数,区别在于求解测量矩阵H矩阵。
首先是要求解测量值z和预测值x之间的偏差。y=z-Hx。
这里Hx用的就是将x从笛卡尔坐标系转化为极坐标系。
接下来的主要问题是在求解H矩阵。
void KalmanFilter::EKFUpdate(Eigen::VectorXd z)
{
double rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
double theta = atan2(x_(1), x_(0));
double rho_dot = (x_(0)*x_(2) + x_(1)*x_(3)) / rho;
Eigen::VectorXd h = Eigen::VectorXd(3);
h << rho, theta, rho_dot;
Eigen::VectorXd y = z - h;
CalculateJacobianMatrix();
Eigen::MatrixXd Ht = H_.transpose();
Eigen::MatrixXd S = H_ * P_ * Ht + R_;
Eigen::MatrixXd Si = S.inverse();
Eigen::MatrixXd K = P_ * Ht * Si;
x_ = x_ + (K * y);
int x_size = x_.size();
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
4、CalculateJacobianMatrix()函数
void KalmanFilter::CalculateJacobianMatrix()
{
Eigen::MatrixXd Hj(3, 4);
float px = x_(0);
float py = x_(1);
float vx = x_(2);
float vy = x_(3);
float c1 = px * px + py * py;
float c2 = sqrt(c1);
float c3 = (c1 * c2);
if(fabs(c1) < 0.0001){
H_ = Hj;
return;
}
Hj << (px/c2), (py/c2), 0, 0,
-(py/c1), (px/c1), 0, 0,
py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;
H_ = Hj;
}
5、main主函数
主函数基本流程为:
int main()
{
KalmanFilter kf;
while(getlint(x,y,时间戳)){
if(激光雷达尚未初始化){
kf.Initialization(x_in);
P<< 4*4;
Q<< 4*4;
H<< 2*4;
R<<2*2;
}
获取两帧时间差delta t;
F<< 4*4;
kf.Prediction();
kf.KFUpdate();
VectorXd x_out=kf.GetX();
}
}
参考:
https://zhuanlan.zhihu.com/p/45238681
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)