在机器人定位中,KF或者EKF是常用的传感器融合算法,之前也总结过很多关于EKF的用法:
如何理解卡尔曼滤波(Kalman Filter)实现数据融合
通俗易懂理解扩展卡尔曼滤波EKF用于多源传感器融合
大部分都是基于MATLAB写的,所以正好有空简单的写一下基于C++的计算方法,并且和 bfl 进行对比。
简单的来说,EKF 分为两个过程,预测和更新,预测的部分一般使用的是数据频率比较高的传感器,例如IMU或者Odom数据,这里为了对比效果,使用了 bfl 例子中的仿真传感器数据,首先先对bfl中的例子,做一个简单的解析:
for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
{
auto start = std::chrono::steady_clock::now();
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);
// DO ONE MEASUREMEN
ColumnVector measurement = mobile_robot.Measure();
// UPDATE FILTER
// filter.Update(&sys_model,input,&meas_model,measurement);
filter.Update(&sys_model,input);
filter.Update(&meas_model,measurement);
measurement = mobile_robot.Measure();
x = filter.PostGet()->ExpectedValueGet();
covarLabelMeas = filter.PostGet()->CovarianceGet();
cout << " ExpectedValueGet = " << x <<" measurement = " << measurement << endl;
} // estimation loop
其中 mobile_robot.Move(input)
就是其中仿真得到传感器数据,模拟了前进的距离。而 mobile_robot.Measure()
便是得到了该时刻的一个观测值。
那么便可以直接调用 EKF 中定义的模型,其中 filter.Update(&sys_model,input)
为预测值更新, 而 filter.Update(&meas_model,measurement)
为观测值更新,其中区分的地方就是模型的概率分布,在bfl库中会对其功能进行重载。
更新完之后就可以通过 filter.PostGet()->ExpectedValueGet
得到 EKF中的最优状态。
下面对模型中的协方差分布进行介绍,主要包括:先验,系统模型,观测值。简单来说,就是初始位置是否准确,在预测的过程中会引入多少误差,由于传感器硬件问题是否会带来观测数据的误差。
先验协方差:
ColumnVector prior_Mu(2);
prior_Mu(1) = PRIOR_MU_X;
prior_Mu(2) = PRIOR_MU_Y;
SymmetricMatrix prior_Cov(2);
prior_Cov(1,1) = PRIOR_COV_X;
prior_Cov(1,2) = 0.0;
prior_Cov(2,1) = 0.0;
prior_Cov(2,2) = PRIOR_COV_Y;
Gaussian prior(prior_Mu,prior_Cov);
系统噪声:
// create gaussian
ColumnVector sysNoise_Mu(2);
sysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
sysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;
SymmetricMatrix sysNoise_Cov(2);
sysNoise_Cov = 0.0;
sysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
sysNoise_Cov(1,2) = 0.0;
sysNoise_Cov(2,1) = 0.0;
sysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
// create the model
LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
观测噪声:
// Construct the measurement noise (a scalar in this case)
ColumnVector measNoise_Mu(1);
measNoise_Mu(1) = MU_MEAS_NOISE;
SymmetricMatrix measNoise_Cov(1);
measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
// create the model
LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
最后就是 观测矩阵的构建,简单理解就是 所观测得到的数据,和 卡尔曼滤波 所需要的数据 之间的转换关系。
// create matrix H for linear measurement model
Matrix H(1,2);
double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
H = 0.0;
H(1,1) = wall_ct * RICO_WALL;
H(1,2) = 0 - wall_ct;
例如观测值是距离墙面的距离,但是 KF 中所维护的状态量是 坐标值 (x,y) ,因此需要将距离转换成坐标值,但是这里定义的 运动模型就是距离,所以这些转换相当于是在运动模型中做了。该例子的运动模型如下:
// Create the matrices A and B for the linear system model
Matrix A(2,2);
A(1,1) = 1.0;
A(1,2) = 0.0;
A(2,1) = 0.0;
A(2,2) = 1.0;
Matrix B(2,2);
B(1,1) = cos(0.8); // 0.69670670934717
B(1,2) = 0.0;
B(2,1) = sin(0.8); //0.71735609089952
B(2,2) = 0.0;
以上就是 bfl 库中所做的融合过程。那么下面就 根据公式 自己实现一个 KF 滤波器。
首先 运动方程还是保持和原来不变,用以下的方程进行预测更新:
x
k
=
A
k
x
k
−
1
+
B
k
u
k
+
w
k
x_k=A_kx_{k−1}+B_ku_k+w_k
xk=Akxk−1+Bkuk+wk
除了要更新 状态之外,还要更新该状态的一个协方差,也就是目前状态的一个可信度。也叫状态转移方程,其中
Q
k
Q_k
Qk 就是一个系统的噪声。
P
k
=
A
k
∗
P
k
−
1
∗
A
T
+
Q
k
P_{k} = A_k*P_{k-1}*A^T + Q_k
Pk=Ak∗Pk−1∗AT+Qk
以上预测的部分就完成了,下面便是更新的过程,也可以认为是融合的过程,两个高斯分布融合,其中首先要算卡尔曼增益,就是更相信谁的问题:
K
=
P
k
∗
H
H
∗
P
k
∗
H
T
+
R
k
K = {P_k*H \over H*P_k*H^T + R_k}
K=H∗Pk∗HT+RkPk∗H
其中
P
k
P_k
Pk 就是目前 KF 中所维护的状态的可信度,而
R
k
R_k
Rk 就是观测值的可信度,那么计算得到的
K
K
K 相当于是一个权重。
有了权重就可以知道更相信谁,那么如果更相信观测值,那么 KF 中所维护的状态 就要往观测值那边多靠一靠。
x
k
=
x
k
−
1
+
K
(
m
e
a
s
u
r
e
m
e
n
t
−
H
∗
x
k
)
x_k = x_{k-1} +K(measurement -H*x_k)
xk=xk−1+K(measurement−H∗xk)
这里也能看出来
H
H
H 观测矩阵的用法,使观测值和状态值保持一致。
既然观测值被更新了,那么对应的置信度也需要被更新:
P
k
=
(
I
−
K
∗
H
)
∗
P
k
−
1
P_k = (I - K*H)*P_{k-1}
Pk=(I−K∗H)∗Pk−1
那么更新的过程也完成了,接下来就是一个不断循环迭代的过程。最终计算得到的效果是一模一样的~
cout << "MAIN: Starting estimation" << endl;
ColumnVector X_k(2); // 状态量 (x,y)
X_k= prior_Mu; // 先验
Matrix P(2,2); // 先验协防差
P = prior_Cov;
Matrix Identity(2,2); // 用于数据格式转换
Identity(1,1) = 1;
Identity(1,2) = 0.0;
Identity(2,1) = 0.0;
Identity(2,2) = 1;
for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
{
auto start = std::chrono::steady_clock::now();
// DO ONE STEP WITH MOBILE ROBOT
mobile_robot.Move(input);
// DO ONE MEASUREMEN
ColumnVector measurement = mobile_robot.Measure();
cout << " \n "<< endl;
// 预测
// 1.更新状态量
X_k = A*X_k + B*input + sysNoise_Mu;
// 2.协防差传播
P = A*(P*A.transpose()) + sysNoise_Cov*Identity;
// 观测值更新
Matrix K,R;
R = measNoise_Cov;
// 1.计算增益矩阵
K = P*H.transpose() *( H*P*H.transpose() + R).inverse();
// 2.更新状态量
ColumnVector X_k_1(2);
X_k = X_k + K *(measurement - H*X_k);
// 3.协防差传播
P = (Identity - K*H)*P;
cout << " K:" << K <<endl;
cout << " Z - Hx:" << measurement - H*X_k <<endl;
cout << " K(Z - Hx):" << K *(measurement - H*X_k) <<endl;
cout << " x + K(Z - Hx):" << X_k <<endl;
} // estimation loop
具体的代码如上,后面会继续更新基于C++的非线性卡尔曼滤波,也就是考虑了转向角。