卡尔曼滤波matlab 代码

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 xg1(k)=xk(1); xg2(k)=xk(2);

z(k)=C*xk+wgn(1,1,10);

P=A*P*A'+Q; %KF Kk=P*C'/(C*P*C'+R); %KF x=xk+Kk*(z(k)-C*xk); %KF 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 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)的估计值和真实值曲线')

方程1

方程2 方程3 方程4

方程5