卡尔曼滤波(利用C++编写，ROS下实现)

匍匐的狗仔

已于 2022-11-19 15:00:57 修改

阅读量6.5k

点赞数 13

文章标签： c++ 开发语言算法 ubuntu linux

于 2022-05-27 20:21:59 首次发布

本文链接：https://blog.csdn.net/er_dan_love/article/details/125007912

版权

提示：利用C++来编写卡尔曼滤波，更能比较简单，主要是提供一个思路，大家可以在这个上面进行修改。附上一个小例题，这个卡尔曼滤波适合这个小例题。

前言

利用C++编写卡尔曼滤波，并加上例题。

一、卡尔曼滤波

首先是介绍卡尔曼滤波的原理，下面是卡尔曼滤波的五大公式主要分为两部分。

第一部分是预测，第二部分是更新。如果想要了解卡尔曼滤波公式的推导可以看一下我另一篇博客又简单的介绍。这里就不做重复的介绍了。

卡尔曼滤波_er_dan_love的博客-CSDN博客

一. 卡尔曼滤波器开发实践之一: 五大公式详解_okgwf的博客-CSDN博客_卡尔曼五大公式

二、编程部分

这是我的github上的内容，在linux中的ros系统下实现的卡尔曼滤波：https://github.com/Zheng-jia-shuo/Kalman-Filter

1.头文件的编写

利用类的知识，编写了卡尔曼滤波的五大公式。这里面主要是定义了几个公式中用到的变量。

#include<iostream>
#include<Eigen/Dense>
using namespace std;

class Kalman
{
public:
    int m_StateSize;
    int m_MeaSize;
    int m_USize;
    Eigen::VectorXd m_x;
    Eigen::VectorXd m_u;
    Eigen::VectorXd m_z;
    Eigen::MatrixXd m_A;
    Eigen::MatrixXd m_B;
    Eigen::MatrixXd m_P;
    Eigen::MatrixXd m_H;
    Eigen::MatrixXd m_R;
    Eigen::MatrixXd m_Q;
    Eigen::MatrixXd m_iden_mat;
public:
    Kalman()
    {
        cout<<"Kalman construct function......"<<endl;
    }
    Kalman(int statesize,int measize,int usize):m_StateSize(statesize),m_MeaSize(measize),m_USize(usize)
{
    if(m_StateSize==0&&m_MeaSize==0)
    {
        cout<<"Init........."<<endl;
    }

    m_x.resize(statesize);
    m_x.setZero();

    m_u.resize(usize);
    m_u.setZero();

    m_z.resize(measize);
    m_z.setZero();

    m_A.resize(statesize,statesize);
    m_A.setIdentity();

    m_B.resize(statesize,usize);
    m_B.setZero();

    m_P.resize(statesize,statesize);
    m_P.setIdentity();

    m_H.resize(measize,statesize);
    m_H.setZero();

    m_R.resize(measize,measize);
    m_R.setZero();

    m_Q.resize(statesize,statesize);
    m_Q.setZero();

    m_iden_mat.resize(statesize,statesize);
    m_iden_mat.setIdentity();
}
    // void Init(Eigen::Matrix<double,6,1> &x,Eigen::Matrix<double,6,6> &P,Eigen::Matrix2d &R,Eigen::Matrix<double,6,6> &Q);
    // Eigen::VectorXd predict(Eigen::Matrix<double,6,6> &A);
    // Eigen::VectorXd predict(Eigen::Matrix<double,6,6> &A,Eigen::MatrixXd &B,Eigen::VectorXd &u);
    // void Update(Eigen::Matrix<double,2,6> &H,Eigen::Matrix<double,2,1> z_meas);

    void Init_Par(Eigen::VectorXd& x,Eigen::MatrixXd& P,Eigen::MatrixXd& R,Eigen::MatrixXd& Q,Eigen::MatrixXd& A,Eigen::MatrixXd& B,Eigen::MatrixXd& H,Eigen::VectorXd& u);
    void Predict_State();
    void Predict_Cov();
    Eigen::VectorXd Mea_Resd(Eigen::VectorXd& z);
    Eigen::MatrixXd Cal_Gain();
    void Update_State();
    void Update_Cov();
};

2.程序的实现

对应头文件中的各个函数的实现。

#include"../include/kalman.h"

void Kalman::Init_Par(Eigen::VectorXd& x,Eigen::MatrixXd& P,Eigen::MatrixXd& R,Eigen::MatrixXd& Q,Eigen::MatrixXd& A,Eigen::MatrixXd& B,Eigen::MatrixXd& H,Eigen::VectorXd& u)
{
    m_x=x;
    m_P=P;
    m_R=R;
    m_Q=Q;
    m_A=A;
    m_B=B;
    m_H=H;
    m_u=u;
}

void Kalman::Predict_State()
{
    Eigen::VectorXd tmp_state=m_A*m_x+m_B*m_u;
    m_x=tmp_state;
}

void Kalman::Predict_Cov()
{
    Eigen::MatrixXd tmp_cov=m_A*m_P*m_A.transpose()+m_Q;
    m_P=tmp_cov;
}

Eigen::VectorXd Kalman::Mea_Resd(Eigen::VectorXd& z)
{
    m_z=z;
    Eigen::VectorXd tmp_res=m_z-m_H*m_x;
    return tmp_res;
}

Eigen::MatrixXd Kalman::Cal_Gain()
{
    Eigen::MatrixXd tmp_gain=m_P*m_H.transpose()*(m_H*m_P*m_H.transpose()+m_R).inverse();
    return tmp_gain;
}

void Kalman::Update_State()
{
    Eigen::MatrixXd kal_gain=Cal_Gain();
    Eigen::VectorXd mea_res=Mea_Resd(m_z);
    m_x=m_x+kal_gain*mea_res;
}

void Kalman::Update_Cov()
{
    Eigen::MatrixXd kal_gain=Cal_Gain();
    Eigen::MatrixXd tmp_mat=kal_gain*m_H;
    m_P=(m_iden_mat-tmp_mat)*m_P;
}

3、例题

题目：

同志们我实在是不晓得怎么旋转图片，有条件的话大家自己旋转着看吧~

#include"../include/kalman.h"
#define gravity 9.8
#define ARR_NUM 20

int main(int argc, char *argv[])
{
    Eigen::VectorXd x;
    Eigen::MatrixXd P0;
    Eigen::MatrixXd Q;
    Eigen::MatrixXd R;
    Eigen::MatrixXd A;
    Eigen::MatrixXd B;
    Eigen::MatrixXd H;
    Eigen::VectorXd z0;
    Eigen::VectorXd u_v;
    x.resize(2);
    x(0)=1900;
    x(1)=10;

    P0.resize(2,2);
    P0.setZero();
    P0(0,0)=100;
    P0(1,1)=2;

    Q.resize(2,2);
    Q.setZero();

    R.resize(1,1);
    R.setIdentity();

    A.resize(2,2);
    A.setIdentity();

    B.resize(2,2);
    B.setIdentity();

    H.resize(1,2);
    H(0,0)=1;
    H(0,1)=0;

    z0.resize(1);
    //z0(0)=1994.5;

    u_v.resize(2,1);
    u_v(0)=-0.5*gravity;
    u_v(1)=gravity;

    double arr[ARR_NUM]={1994.5,1979.4,1955.4,1921.4,1877.7,1825.0,1759.8,1686.7,1603.6,1509.2,1407.6,1294.4,1172.4,1039.9,898.0,745.5,585.0,412.5,231.8,39.9};
    Kalman ka(2,1,2);
    ka.Init_Par(x,P0,R,Q,A,B,H,u_v);

    for(int i=0;i<ARR_NUM;i++)
    {
        z0(0)=arr[i];
        cout<<"the "<<(i+1)<<" th time predict"<<endl;
        ka.Predict_State();
        ka.Predict_Cov();
        ka.Mea_Resd(z0);
        ka.Cal_Gain();
        ka.Update_State();
        ka.Update_Cov();
        cout<<ka.m_x<<endl;
        cout<<"*************"<<endl;
    }

    // cout<<ka.m_x<<endl;
    // cout<<ka.m_A<<endl;
    // cout<<ka.m_B<<endl;
    // cout<<ka.m_H<<endl;
    return 0;
}

4、CMakeLists.txt编写

cmake_minimum_required(VERSION 2.8)
project(kalman)

set( CMAKE_BUILD_TYPE "Release")
set( CMAKE_CXX_FLAGS "-O3")

include_directories("usr/include/eigen3")

add_library(kalman kalman.cpp)

add_executable(test_kal test_kal.cpp)
target_link_libraries(test_kal kalman)