匀速运动-线性卡尔曼滤波-单目标跟踪 C++实现（附源码）

卡尔曼滤波的原理不多赘述，直接上代码（附件里也可以下载）

编译运行：

mkdir build
cd build
cmake ..
make
./kalman_test

• 头文件 — kalman_xv.h

#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include <eigen3/Eigen/Dense>

class LinearKalmanFilter
{
public:
    LinearKalmanFilter()
    {
        is_initialized_ = false;
    };
    ~LinearKalmanFilter();

	
    void Initialization(Eigen::VectorXd x_in, double process_noise_std, double delta_t)
    {
        x_ = x_in;

        Eigen::MatrixXd P_in(2, 2);
        P_in << 1.0, 0.0,
                0.0, 1.0;
        P_ = P_in;

        // 过程噪声矩阵
        double process_noise_std_ = pow(process_noise_std, 2);
        Eigen::MatrixXd Q_in(2, 2);
        Q_in << process_noise_std_, 0.0,
                0.0, process_noise_std_;

        Q_ = Q_in;

        // 观测矩阵
        Eigen::MatrixXd H_in(1, 2);
        H_in << 1.0, 0.0;
        H_ = H_in;

        // 观测噪声均值
        Eigen::MatrixXd R_in(1, 1);
        R_in << 1.0;

        R_ = R_in;

        // state transition matrix
        // 状态转移矩阵
        Eigen::MatrixXd F_in(2, 2);
        F_in << 1.0, delta_t,
                0.0, 1.0;
        F_ = F_in;

        this->is_initialized_ = true;
    }

    bool IsInitialized()
    {
        return this->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 SetH(Eigen::MatrixXd H_in)
    {
        H_ = H_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 MeasurementUpdate(const Eigen::VectorXd &z)
    {
        Eigen::VectorXd y = z - H_ * x_;
        Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
        Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse(); // 计算卡尔曼增益
        x_ = x_ + (K * y);                                     // 状态更新 当前时刻通过卡尔曼滤波得到的目标估计量
        int size = x_.size();
        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
        P_ = (I - K * H_) * P_; // 更新状态估计误差协方差
    }

    Eigen::VectorXd GetX()
    {
        return x_;
    }

private:
    bool is_initialized_; // flag of initialized
    Eigen::VectorXd x_;   // state vector 目标状态
    Eigen::MatrixXd F_;   // state transistion matrix 状态转移矩阵
    Eigen::MatrixXd P_;   // state covariance matrix 状态估计误差协方差
    Eigen::MatrixXd Q_;   // process covariance matrix 过程噪声矩阵
    Eigen::MatrixXd H_;   // mesurement matrix 测量/观测 矩阵
    Eigen::MatrixXd R_;   // mesurement covariance matrix 测量/观测 噪声均值
};
LinearKalmanFilter::~LinearKalmanFilter()
{
}
#endif

• 源文件 — kalman_xv.cpp

#include "kalman_xv.h"
#include <iostream>
#include <iomanip>
#include <string>
#include <stdio.h>
#include <vector>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <chrono>

using namespace std;


int main()
{
    double vel = 8.3;     // 速度，单位m/s, 8.3m/s约30km/h
    double delta_t = 0.1; // 时间间隔
    int sample_time = 200;
    double process_noise_std = 0.1; // 值越大越接近原始轨迹，平滑越弱
    

    srand((unsigned int)time(NULL));
    vector<double> xx;

    // 模拟匀速直线运动的轨迹xx
    for (int i = 0; i < sample_time; i++)
    {
        // 制造(-1~1)之间的随机误差
        double random_num = pow(-1, rand()) * (double)(rand() % 10) / 10.0;
        double tmp = i * vel * delta_t + random_num;

        xx.push_back(tmp);
    }

    LinearKalmanFilter KF;
    if (!KF.IsInitialized())
    {
        // cout << "*************** Before Initialize KF *************** KF.IsInitialized()=" << KF.IsInitialized() << endl;
        Eigen::VectorXd x_in(2, 1);
        x_in << xx[0], vel;

        KF.Initialization(x_in, process_noise_std, delta_t);

        // cout << "*************** After Initialize KF *************** KF.IsInitialized()=" << KF.IsInitialized() << endl;
    }

	// 将原始轨迹和预测轨迹保存成txt，后续可以用python或者matlab画对比曲线图
    FILE *LOC_FILE = fopen("loc.txt", "w");
    double m_x;
    for (int i = 0; i < xx.size(); i++)
    {
        m_x = xx[i];

        KF.Prediction();
        // measurement value
        Eigen::VectorXd z(1, 1);
        z << m_x;
        KF.MeasurementUpdate(z);
        // result
        Eigen::VectorXd x_out = KF.GetX();

        std::cout << "真实位置：" << m_x << ",";
        std::cout << "预测位置：" << x_out[0] << endl;
        fprintf(LOC_FILE , "%f %f\n", m_x, x_out(0));
    }
    fclose(LOC_FILE );
}

• CmakeLists.txt

cmake_minimum_required(VERSION 3.0)

project(kalman_test)

add_definitions(-std=c++11)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE Release)

include_directories("include/")

add_executable(kalman_test
               src/kalman_xv.cpp
               )