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

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

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

案例1：室内温度测量

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

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

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

$X(k)=X(k-1)+W(k-1)$

$Q(k)=0.01$

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

$Z(k)=X(k)+V(k)$

$R(k)=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*Xkfvoid StateUpdate(Matrix &X);          //状态转移后还加入噪声，这里就是X=H*X+wMatrix 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）保存到文件里。

来看看效果吧