前言
上一篇围绕卡尔曼滤波算法的参数选取问题展开,针对非线性对象的状态估计问题,阐述扩展卡尔曼滤波(EKF)与卡尔曼滤波的区别以及扩展卡尔曼滤波算法的核心步骤。本篇将结合实际案例进行详解,同时,提供一种扩展卡尔曼滤波算法的C++代码实现方案。
扩展卡尔曼滤波实例
这里以无人驾驶的测量障碍物的实际案例为例子展开,如下图所示,毫米波雷达能够测量障碍物在极坐标下离雷达的距离 、方向角 以及距离的变化率(径向速度) ,如下图所示,
预测更新以二维匀速运动为例,这里的状态量 为:
根据牛顿运动定理,经过 时间后的预测状态量为:
对于二维的匀速运动模型,加速度为0,并不会对预测后的状态造成影响,因此令
由此可得状态转移矩阵为
毫米波雷达测量障碍物在径向上的位置和速度相对准确,不确定度较低,因此可以对状态协方差矩阵P进行如下初始化:
由于Q对整个系统存在影响,但又不能太确定对系统的影响有多大。对于简单的模型来说,这里可以直接使用单位矩阵或空值进行计算,即
- 量测更新
由图1可知观测值z的数据维度为3×1,为了能够实现矩阵运算,y和Hx'的数据维度也都为3×1。
使用基本的数学运算可以完成预测值x'从笛卡尔坐标系到极坐标系的坐标转换,求得Hx',再与观测值z进行计算。需要注意的是,在计算差值y的这一步中,我们通过坐标转换的方式避开了未知的旋转矩阵H的计算,直接得到了Hx’的值。
为了简化表达,我们用px,py以及vx和vy表示预测后的位置及速度,如下所示:
这里,
毫米波雷达观测z是包含位置、角度和径向速度的3x1的列向量,状态向量x'是包含位置和速度信息的4x1的列向量,根据公式y=z-Hx'可知测量矩阵(Measurement Matrix)H的维度是3行4列。即:
此时,我们需要对H矩阵进行泰勒展开(线性化),得到其Jacobian矩阵,求解方法详解上一篇,这里不再赘述。
经过线性化,最终得到测量矩阵H为:
扩展卡尔曼滤波的C++实现方案
完成以上推导后,我们将推导的过程写成代码。这里采用矩阵运算库Eigen进行矩阵运算,其运算效率较高,与自定义的矩阵运算库相比也更省心。
- ExtendedKalmanFilter类的实现
-
#pragma once #include "Eigen/Dense" class ExtendedKalmanFilter{ public: //构造函数 ExtendedKalmanFilter(){ is_initialized = false; } //析构函数 ~ExtendedKalmanFilter(){} void Initialization(Eigen::VectorXd x_in){ x = x_in; is_initialized = true; } bool IsInitialized(){ return is_initialized; } //状态转移矩阵赋值 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 SetR(Eigen::MatrixXd R_in){ R = R_in; } //预测更新 void Prediction(){ x = F * x; Eigen::MatrixXd Ft = F.transpose(); P = F * P * Ft + Q; } void CalculateJacobianMatrix(){ MatrixXd Hj(3,4); //获取状态量 float px = x(0); float py = x(1); float vx = x(2); float xy = x(3); float c1 = px * px + py * py; float c2 = sqrt(c1); float c3 = (c1 * c2); //避免除0的情况 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; return; } void MeasurementUpdate(const 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; VectorXd h = VectorXd(3); h << rho, theta, rho_dot; VectorXd y = z - h; CalculateJacobianMatrix(); MatrixXd S = H * P * H.transpose() + R; MatrixXd K = P * H.transpose() * S.inverse(); x = x + (K * y); P = (I - K * H) * P; } private: //初始化标志位 bool is_initialized; //状态向量 Eigen::VectorXd x; //状态转移矩阵 Eigen::MatrixXd F; //状态协方差矩阵 Eigen::MatrixXd P; //过程噪声矩阵 Eigen::MatrixXd Q; //雅各比量测矩阵 Eigen::MatrixXd H; //量测噪声协方差矩阵 Eigen::MatrixXd R; }
- 扩展卡尔曼滤波器的调用
#include "extended_kalman_filter.h"
#include "math.h"
#include <iostream>
int main(){
double m_rho = 0.0f, m_theta = 0.0f, m_rho_dot = 0.0f;
doublt last_timestamp = 0.0f, now_timestamp = 0.0f;
ExtendedKalmanFilter ekf;
while(GetRadarData(&m_rho, &m_rho_dot, &now_timestamp)){
if(!ekf.IsInitialized(){
last_timestamp = now_timestamp;
Eigen::VectorXd x(4, 1);
x << m_rho * cos(m_theta), m_rho*sin(m_theta),
m_rho_dot * cos(m_theta), m_rho_dot * sin(m_theta);
ekf.Initialization(x);
//状态协方差矩阵初始化
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, 10.0,0.0,
0.0, 0.0, 0.0, 10.0;
ekf.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;
ekf.setP(P_in);
//量测噪声矩阵初始化
Eigen::MatrixXd R_in(3, 3);
R_in << 0.09, 0.0, 0.0,
0.0, 0.0009, 0.0.
0.0, 0.0, 0.09;
ekf.setR(R_in);
continue;
}
double 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;
ekf.setF(F_in);
ekf.Prediction();
Eigen::VectorXd z(3, 1);
z << m_rho, m_theta, m_rho_dot;
ekf.MeasurementUpdate(z);
Eigen::VectorXd x_out = ekf.GetX();
std::cout << "kalman output x : " << x_out(0) << " y : " << x_out(1) << std::endl;
}
}
总结
本篇围绕一个简单的案例展开,详解扩展卡尔曼滤波算法的实现,同时,提供了一种采用Eigen矩阵运算库的c++实现方案。下篇中,我们将围绕多传感器状态估计的最优估计问题展开论述。
评论(0)
您还未登录,请登录后发表或查看评论