卡尔曼滤波算法它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态,实践中得到广泛应用,场景包括控制系统参数估计,移动预测等等,在深度学习中也常常用来配合神经网络输出做后处理,应用于目标跟踪等场景, 自动化专业在讲现代控制理论的时候会讲。
测试代码在:
代码下载后,编译运行步骤如下。
1.安装eigen3
eigen3是一套高效的线性代数运行库,安装命令如下:
sudo apt-get install libeigen3-dev
具体用法可以参考下面这篇博客
ubuntu18.04安装eigen算法库_papaofdoudou的博客-CSDN博客_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