前言

上一篇围绕卡尔曼滤波算法的参数选取问题展开，针对非线性对象的状态估计问题，阐述扩展卡尔曼滤波（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++实现方案。下篇中，我们将围绕多传感器状态估计的最优估计问题展开论述。