# 【老生谈算法】matlab实现卡尔曼滤波算法源码——卡尔曼滤波

## 1、卡尔曼滤波器原理

ξ（n） = ξ（n - 1）= ξ0

## 2、基于Matlab的卡尔曼滤波器的仿真

Matlab代码如下：

``````clear
N=200;
w(1)=0;
x(1)=5;
a=1;
c=1;
%过程噪声
Q1=randn(1,N)*1;
%测量噪声
Q2=randn(1,N);
%状态矩阵
for k=2:N;x(k)=a*x(k-1)+Q1(k-1);end
for k=1:N;Y(k)=c*x(k)+Q2(k);end
p(1)=10;
s(1)=1;
for t=2:N;
Rww = cov(Q1(1:t));
Rvv = cov(Q2(1:t));
p1(t)=a.^2*p(t-1)+Rww;
%kalman 增益
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end
FontSize=14;
LineWidth=3;
figure();
%画出温度计的测量值
plot(Y,'k+');
hold on;
%画出最优估计值
plot(s,'b-','LineWidth',LineWidth)
hold on;
%画出真实值
plot(x,'g-','LineWidth',LineWidth);
legend('观测值', '后验估计', '真实值');
xl=xlabel('');
yl=ylabel('');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
figure();
valid_iter = [2:N];
%画出最优估计值的方差
plot(valid_iter,p([valid_iter]),'LineWidth',LineWidth);
legend('后验估计的误差估计');
xl=xlabel('');
yl=ylabel('');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);
``````

Matlab仿真结果如下：

THE END

)">