kalman滤波器C++设计仿真案例

        很多同学看了我之前的文章，都对kalman滤波器的原理有了理解，但我发现，在具体工程设计过程中，还是很多人都感觉到无从下手，一些参数也不知道如何选取。

        这样吧。我这里举一些简单的例子，并用C++来一步一步的进行设计。然后，咱们一起看看最后实现的效果到底如何好吧。

案例1：室内温度测量

        要求用kalman滤波器对一个房间的温度进行测量估计。这个房间的温度大概是25℃，而由于空气流动、光照等影响，室内温度会有小幅的随机波动。这个波动的方差嘛，就暂定为0.01好了。然后我们 用温度计每分钟进行一次测量，温度计的测量误差大概是0.5℃，也就是方差为0.25。

        这是一个非常简单的案例，过程中所有的矩阵都是一维，也就是标量。

        先定义状态量，就是房间温度。房间温度有波动，所以会有系统噪声。根据题意，的协方差矩阵Q就等于0.01。于是状态方程就是：

再看量测方程。由于是直接测量，所以H阵等于1，而量测方差阵R根据题意，就等于0.25了。

        由于涉及矩阵计算，所以这里用到了我自己用C++开发的矩阵类。需要的可以到我的空间，或者下面这个链接去下载：

https://download.csdn.net/download/weixin_38898944/90305952

        开始C++程序设计了啊。嗯，注意是C++11，否则我的有些写法可能会报错。

        先设计一个kalman滤波器的类。为了代码复用程度高，我把之前文章里的提到的基本型kalman滤波器拆分成两个类。一个是模型Model类，一个是kalman类。这样如果换一个需求场景，只要更改模型类就好了，kalman类不用变。先看kalman类。头文件内容：

#ifndef KALMAN_H
#define KALMAN_H

#include "matrix.h"
#include "model.h"
class Kalman
{
    size_t m_ns;    //状态维数
    size_t m_nm;    //量测维数

    Matrix Xkf;     //状态估计值
    Matrix Xpre;    //状态预测值
    Matrix Z;       //量测
    Matrix K;       //增益
    Matrix P;       //状态估计的协方差阵
    Matrix Ppre;    //状态预测的协方差阵

    Model *mp;      //模型，包含了状态方程、量测方程等
    Kalman();       //默认构造函数，私有化，不能调用
public:
    Kalman(size_t ns, size_t nm);           //构造函数，需要确定状态维数和量测维数
    void Init(const Matrix &X0, const Matrix &P0, Model *mp0);//初始化，初始化X0和P0，并与模型对象挂钩
    void SetMeasur(const Matrix &Zk);       //量测量进入滤波器
    void Iterator();                        //滤波器更新过程
    Matrix GetX() {return Xkf;}             //获取当前状态估计
};

#endif // KALMAN_H

源文件：

#include "kalman.h"

Kalman::Kalman()
{

}

Kalman::Kalman(size_t ns, size_t nm)
{
    m_ns = ns;
    m_nm = nm;
    K = Matrix(ns, nm);
    Xkf = Matrix(ns);
    P = Matrix(ns, ns);
    Ppre = Matrix(ns, ns);
    Z = Matrix(nm);
    Xpre = Matrix(ns);
}

void Kalman::Init(const Matrix &X0, const Matrix &P0, Model *mp0)
{
    Xkf = X0;
    P = P0;
    mp = mp0;
}

void Kalman::SetMeasur(const Matrix &Zk)
{
    Z = Zk;
}

void Kalman::Iterator()
{
    static Matrix I(Matrix::unit(m_ns));
    Matrix F(mp->GetF());
    Matrix H(mp->GetH());
    Matrix Q(mp->GetQ());
    Matrix R(mp->GetR());
    Xpre = mp->StateTrans(Xkf);
    Ppre = F*P*F.Trans() + Q;
    K = Ppre*H.Trans()*((H*Ppre*H.Trans() + R).Inv());
    Xkf = Xpre + K*(Z - H*Xpre);
    P = (I - K*H)*Ppre;
}

        这几个成员函数都非常直观，尤其注意那个Iterator()函数，滤波器运行需要的转移矩阵F，量测矩阵H以及状态协方差阵 Q和量测方差阵R都是从模型里获取的。所以模型变了该模型就好了，不需要改这个滤波器。一步预测那里并没有写成Xpre=H*X，而是让模型对象去更新，这样写是为了适应以后扩展成非线性kalman滤波器，也就是ekf的时候方便一些。

        下面就来设计模型类。头文件：

#ifndef MODEL_H
#define MODEL_H

#include "matrix.h"
class Model
{
    Matrix Q;
    Matrix R;
public:
    Model(): Q(Matrix::unit(1)*0.01), R(Matrix::unit(1)*0.25){}
    Matrix StateTrans(Matrix &X);         //状态转移，这里就是Xpre=H*Xkf
    void StateUpdate(Matrix &X);          //状态转移后还加入噪声，这里就是X=H*X+w
    Matrix MeasurPre(const Matrix &X);    //量测预测，没有加入量测噪声
    Matrix Measur(const Matrix &X);       //量测，加入量测噪声

    Matrix GetF();                        //获取状态转移矩阵，如果是ekf，可以改写这个函数，
                                          //获取状态方程的雅各比矩阵

    Matrix GetH();                        //获取量测矩阵，如果是ekf，改写这个函数，
                                          //获取量测方程的雅各比矩阵

    Matrix GetQ() {return Q;}                //获取模型的状态方程协方差阵
    Matrix GetR() {return R;}                //获取模型的量测方程的协方差阵
};


#endif // MODEL_H

        Q和R都是一维矩阵，且值分别是0.01和0.25。然后是源文件：

#include "model.h"


Matrix Model::StateTrans(Matrix &X)
{
    Matrix F(Matrix::ones(1, 1));
    return F*X;
}

void Model::StateUpdate(Matrix &X)
{
    X = StateTrans(X);
    X = X + Sqrtm(Q)*Matrix::randn(1, 1);
}

Matrix Model::MeasurPre(const Matrix &X)
{
    Matrix H(Matrix::ones(1, 1));
    return  H*X;
}

Matrix Model::Measur(const Matrix &X)
{
    return MeasurPre(X) + Sqrtm(R)*Matrix::randn(1, 1);
}

Matrix Model::GetF()
{
    return Matrix::ones(1, 1);
}

Matrix Model::GetH()
{
    return Matrix::ones(1, 1);
}

        我的矩阵类里已经实现了矩阵的各类运算，包括矩阵加减乘，矩阵与标量的加减乘以及矩阵转置、矩阵求逆、矩阵元素开根号。并能够生成全零矩阵、全1矩阵、单位矩阵、（0,1）正态分布的随机矩阵等。所以以上代码看上去还是很好理解的。

        有了这两个类，主程序就非常简单了：

#include "model.h"
#include "kalman.h"
#include <stdio.h>
int main()
{
    FILE *fp;
    fp = fopen("F:/data/data.txt", "w");
    Model M;
    Kalman kf(1, 1);
    Matrix X(Matrix::ones(1, 1)*25.0);
    Matrix Z(1);

    Matrix P0(Matrix::ones(1, 1)*0.01);
    kf.Init(X, P0, &M);
    for(size_t i = 0; i < 100; i++)
    {
        M.StateUpdate(X);
        Z = M.Measur(X);
        kf.SetMeasur(Z);
        kf.Iterator();
        fprintf(fp, "%lf,%lf,%lf\n", X(0), kf.GetX()(0), Z(0));
    }
    fclose(fp);
    return 0;
}

        初值就设定为25℃，P的初值也取的跟Q一样。用kf.Init()函数初始化，并与模型对象挂钩。

        主程序在运行kalman的同时也对室内温度实际值进行了仿真，并将温度实际值（X），kalman滤波值（kf.GetX()），和量测值（Z）保存到文件里。

        来看看效果吧

        蓝线为实际温度，绿线为测量值，红线为滤波输出。喏，看出效果了吧。

        先这样吧。最近有些忙，回头我再写一个状态变化情况的kalman滤波仿真。