卡尔曼滤波matlab 代码
- 格式:pdf
- 大小:171.02 KB
- 文档页数:3
kalman滤波matlab代码%kalman filter卡尔曼滤波
clear
clc
A = [1,1;0,1];
B = [1/2,1]';
C = [1,0];
x1(1)= 100; %初始化
x2(1)= 10;
x = [x1(1),x2(1)]';
z=C*x;
P = [1,0;0,1];
Q=[2,0;0,1]
R = 10;
g=0.98;
u=-g;
I=eye(2);
for k=2:20
xk=A*x+B*u; %KF 方程1 xg1(k)=xk(1);
xg2(k)=xk(2);
z(k)=C*xk+wgn(1,1,10);
P=A*P*A'+Q; %KF 方程2 Kk=P*C'/(C*P*C'+R); %KF 方程3 x=xk+Kk*(z(k)-C*xk); %KF 方程4 x1(k)=x(1);
x2(k)=x(2);
e1(k)=x1(k)-xg1(k);
e2(k)= x2(k)-xg2(k);
P=(I-Kk*C)*P; %KF 方程5 end
k=1:20;
figure(1)
plot(k,x1,k,xg1)
title('x1(k)的估计值和真实值曲线')
legend('x1(k)的估计值','x1(k)的真实值')
figure(2);
plot(k,x2,k,xg2')
title('x2(k)的估计值和真实值曲线')
legend('x2(k)的估计值','x2(k)的真实值') figure (3)
subplot(2,1,1)
plot(k,e1)
title('x1(k)的估计值和真实值误差曲线') subplot(2,1,2)
plot(k,e2)
title('x2(k)的估计值和真实值误差曲线')
figure (4)
plot(k,z)
title('输出z的观测值曲线')