卡尔曼滤波与目标跟踪算法实现

[TOC]

代码实现：https://github.com/lovodllt/kalman_track

(这里用的是ros的工作空间，需要使用的记得修改cmakelist)

对于短暂被遮挡物体的识别效果

卡尔曼滤波（KF）

在线性条件下，通过过程与观测两个方面，对输入值进行最优估计

原理

（方差模拟不确定性）

公式部分：

观测（observation）

$$ \Large z_t=Hx_t+v_k $$

各参数含义： $$ \large \begin{align*} &z_t:当前时刻(t)的观测值\ &H : 观测矩阵H\ &x_t:当前时刻(t)的真实状态向量\ &v_k:观测噪声 \end{align*} $$

预测（Prediction）

状态预测公式

$$ \Large \hat{x}_t^{-}=F\hat{x}_{t - 1}+Bu_{t - 1} $$

根据上一时刻（t - 1）的状态估计值来预测当前时刻（ t ）的状态

推导：

各参数含义：

$$ \large \begin{align*} &\hat{x}_t^-:当前时刻(t)的先验姿态估计(预测值)\\ &F:状态转移矩阵 F\\ &\hat{x}_{t-1}:上一时刻(t-1)的后验状态估计(最优估计值)\\ &B:控制矩阵 B\\ &u_{t-1}:上一时刻（t - 1）的控制输入 \end{align*} $$

协方差预测公式

$$ \Large P_t^−=FP_{t−1} F^T+Q $$

衡量变量的总体误差

推导：

根据上一时刻（t - 1）的状态协方差矩阵来预测当前时刻的状态协方差

各参数含义： $$ \large \begin{align*} &P_t^-:当前时刻(t)的先验估计协方差矩阵(预测值)\ &F:状态转移矩阵\ &P_{t-1}:上一时刻(t-1)的后验估计协方差矩阵(最优估计值)\ &F^T : 状态转移矩阵 F 的转置\ &Q:过程噪声协方差矩阵 \end{align*} $$

更新（Update）

卡尔曼增益公式

$$ \Large K_t=P_t^−H^T(HP_t^−H^T+R)^{−1} $$

用于确定在更新状态估计时，测量值所占的比重。

各参数含义：

$$ \large \begin{align*} &K_t: 当前时刻(t)的卡尔曼增益(估计值权重)\\ &P_t^−: 当前时刻(t)的先验状态协方差(预测值)\\ &H : 观测矩阵H\\ &R : 测量噪声协方差矩阵R \end{align*} $$

状态更新公式

$$ \Large \hat{x}_t= \hat{x}_t^−+K_t(z_t−H \hat{x}_t^−) $$

结合预测状态和当前时刻的测量值来得到当前时刻的最优状态估计

各参数含义： $$ \large \begin{align*} &\hat{x}_t: 当前时刻(t)的后验状态估计(最优估计值)\ &\hat{x}_t^-: 当前时刻(t)的先验状态估计(预测值)\ &K_t: 当前时刻(t)的卡尔曼增益\ &z_t: 当前时刻(t)的测量值\ &H: 观测矩阵H\ \end{align*} $$

协方差更新公式

$$ \Large P_t=(I−K_tH)P_t^− $$ 根据卡尔曼增益和先验状态协方差来更新当前时刻的状态协方差

各参数含义： $$ \large \begin{align*} &P_t:当前时刻(t)的后验状态协方差(最优值)\ &I:单位矩阵\ &K_t: 当前时刻(t)的卡尔曼增益(估计值权重)\ &H: 观测矩阵\ &P_t^-:当前时刻(t)的先验状态协方差(预测值) \end{align*} $$

实例

需要考虑的状态：

均值：x = [cx, cy, r, h, vx, vy, vr, vh]

--中心坐标（cx, cy），高宽比r，高h，以及各自的速度变化值

协方差矩阵：由需要控制变量的数量决定，此处为8*8的矩阵

代码实现

class KalmanFilter_ {
/* x = [x,
 *      y,
 *      w,
 *      h,
 *      vx,
 *      vy,
 *      vw,
 *      vh]
 */
private:
    Vector8f x;//状态向量
    Matrix8f F;//状态转移矩阵
    Matrix8f P;//误差协方差矩阵
    Matrix8f Q;//过程噪声协方差矩阵
    Eigen::Matrix4f R;//观测噪声协方差矩阵
    Eigen::Matrix<float,8,4> K;//卡尔曼增益
    Eigen::Matrix<float,4,8> H;//观测矩阵
    float dt;
    bool is_init = false;

public:
    KalmanFilter_() : dt(0.01)
    {
        //初始化状态向量
        x << 0, 0, 0, 0, 0, 0, 0, 0;

        //初始化状态转移矩阵
        F = Matrix8f::Identity();

        //初始化过程噪声协方差矩阵
        Q = Matrix8f::Zero(8,8);
        Q.diagonal() << 1e-2, 1e-2, 1e-2, 2e-2, 5e-2, 5e-2, 1e-4, 4e-2;

        //初始化观测噪声协方差矩阵
        R = Eigen::Matrix4f::Identity() * (1e-2);

        //初始化误差协方差
        P = Matrix8f::Identity() * 10;

        //初始化观测矩阵
        H << 1, 0, 0, 0, 0, 0, 0, 0,
             0, 1, 0, 0, 0, 0, 0, 0,
             0, 0, 1, 0, 0, 0, 0, 0,
             0, 0, 0, 1, 0, 0, 0, 0;
    }

    void setF(float dt)
    {
        F << 1, 0, 0, 0, dt, 0,  0,  0,
             0, 1, 0, 0, 0,  dt, 0,  0,
             0, 0, 1, 0, 0,  0,  dt, 0,
             0, 0, 0, 1, 0,  0,  0,  dt,
             0, 0, 0, 0, 1,  0,  0,  0,
             0, 0, 0, 0, 0,  1,  0,  0,
             0, 0, 0, 0, 0,  0,  1,  0,
             0, 0, 0, 0, 0,  0,  0,  1;

        Vector8f new_x = x;
        x[0] = new_x[0] + new_x[4] * dt;
        x[1] = new_x[1] + new_x[5] * dt;
        x[2] = new_x[2] + new_x[6] * dt;
        x[3] = new_x[3] + new_x[7] * dt;
    }

    void init(const Eigen::Vector4f& z)
    {
        x.head<4>() = z;
        is_init = true;
    }

    // 预测
    void predict(float dt)
    {
        if(!is_init) return;

        setF(dt);
        x = F * x;
        P = F * P * F.transpose() + Q;
    }

    // 更新
    void update(const Eigen::Vector4f& z)
    {
        if(!is_init) init(z);

        auto y = z - H * x;
        auto S = H * P * H.transpose() + R;
        K = P * H.transpose() * S.inverse();

        x = x + K * y;
        P = (Matrix8f::Identity() - K * H) * P;
    }

    // 获取当前位置
    Eigen::Vector4f getPosition()
    {
        return x.head<4>();
    }
};typedef Eigen::Matrix<float, 8, 1> Vector8f;
typedef Eigen::Matrix<float, 8, 8> Matrix8f;

class KalmanFilter_ {
/* x = [x,
 *      y,
 *      w,
 *      h,
 *      vx,
 *      vy,
 *      vw,
 *      vh]
 */
private:
    Vector8f x;//状态向量
    Matrix8f F;//状态转移矩阵
    Matrix8f P;//误差协方差矩阵
    Matrix8f Q;//过程噪声协方差矩阵
    Eigen::Matrix4f R;//观测噪声协方差矩阵
    Eigen::Matrix<float,8,4> K;//卡尔曼增益
    Eigen::Matrix<float,4,8> H;//观测矩阵
    float dt;
    bool is_init = false;

public:
    KalmanFilter_() : dt(0.01)
    {
        //初始化状态向量
        x << 0, 0, 0, 0, 0, 0, 0, 0;

        //初始化状态转移矩阵
        F = Matrix8f::Identity();

        //初始化过程噪声协方差矩阵
        Q = Matrix8f::Zero(8,8);
        Q.diagonal() << 1.0, 1.0, 1.0, 1.0, 2.0, 2.0, 2.0, 2.0;

        //初始化观测噪声协方差矩阵
        R = Eigen::Matrix4f::Identity() * 0.01;

        //初始化误差协方差
        P = Matrix8f::Identity() * 10;

        //初始化观测矩阵
        H << 1, 0, 0, 0, 0, 0, 0, 0,
             0, 1, 0, 0, 0, 0, 0, 0,
             0, 0, 1, 0, 0, 0, 0, 0,
             0, 0, 0, 1, 0, 0, 0, 0;
    }

    void setF(float dt)
    {
        F << 1, 0, 0, 0, dt, 0,  0,  0,
             0, 1, 0, 0, 0,  dt, 0,  0,
             0, 0, 1, 0, 0,  0,  dt, 0,
             0, 0, 0, 1, 0,  0,  0,  dt,
             0, 0, 0, 0, 1,  0,  0,  0,
             0, 0, 0, 0, 0,  1,  0,  0,
             0, 0, 0, 0, 0,  0,  1,  0,
             0, 0, 0, 0, 0,  0,  0,  1;
    }

    void init(const Eigen::Vector4f& z)
    {
        x.head<4>() = z;
        is_init = true;
    }

    // 预测
    void predict(float dt)
    {
        if(!is_init) return;

        setF(dt);
        x = F * x;
        P = F * P * F.transpose() + Q;
    }

    // 更新
    void update(const Eigen::Vector4f& z)
    {
        if(!is_init) init(z);

        auto y = z - H * x;
        auto S = H * P * H.transpose() + R;
        K = P * H.transpose() * S.inverse();

        x = x + K * y;
        P = (Matrix8f::Identity() - K * H) * P;
    }

    // 获取当前位置
    Eigen::Vector4f getPosition()
    {
        return x.head<4>();
    }
};

扩展卡尔曼滤波（EKF）

可应用于非线性环境。相较于卡尔曼滤波，状态向量中可以引入加速度，旋转等非线性内容

通过对状态转移矩阵和观测矩阵进行求导降阶，实现对非线性函数进行线性化近拟