卡尔曼滤波算法的代码验证

卡尔曼滤波算法它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态，实践中得到广泛应用，场景包括控制系统参数估计，移动预测等等，在深度学习中也常常用来配合神经网络输出做后处理，应用于目标跟踪等场景, 自动化专业在讲现代控制理论的时候会讲。

测试代码在：

GitHub - 13824125580/kalmanfilter-cpp: A basic Kalman Filter implementation in C++ using the Eigen library.https://github.com/13824125580/kalmanfilter-cpp.git

代码下载后，编译运行步骤如下。

1.安装eigen3

eigen3是一套高效的线性代数运行库，安装命令如下：

sudo apt-get install libeigen3-dev

具体用法可以参考下面这篇博客

ubuntu18.04安装eigen算法库_papaofdoudou的博客-优快云博客_ubuntu18安装eigen

2.编译源码

源码下载后的目录结构如下图所示：

用例使用cmake编译，创建编译目录build,进入build目录，执行 cmake ../生成编译文件

之后执行make 编译，编译后，生成可执行测试程序kalman-test文件

测试

执行kalman-test用例，输出如下：

测试输出中的x_hat则是对下图中的测量值的经过卡尔曼滤波算法之后的输出值。

matlab kalman滤波实现

N=200;
w(1)=0;
w=randn(1,N)  % White Gaussian Noise of Prediction

x(1)=25;
a=1;  % prediction parameter
for k=2:N;
  x(k)=a*x(k-1)+w(k-1);
end

V=randn(1,N); % White Gaussian Noise of Measurement
q1=std(V);
Rvv=q1.^2; % covariance of Measurement
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2; % covariance of prediction
c=0.2;   % measurement parameter
Y=c*x+V;

p(1)=10;  % initial prediction result
s(1)=26;  % initial optimal result
for t=2:N;
  p1(t)=a.^2*p(t-1)+Rww;  % covariance of t-1
  b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); % kalman gain
  s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); % optimal result
  p(t)=p1(t)-c*b(t)*p1(t); % covariance of t
end

t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b',t,w,'m');

用OCTAVE验证：

---

参考资料：

卡尔曼滤波器｜Little Stone - Huan Li's Blog

结束