很多同学看了我之前的文章，都对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滤波仿真。
