卡尔曼滤波matlab-代码

kalman滤波matlab代码%kalmanfilter卡尔曼滤波clearclcA=[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);fork=2:20xk=A*x+B*u;%KF方程1xg1(k)=xk(1);xg2(k)=xk(2);z(k)=C*xk+wgn(1,1,10);P=A*P*A'+Q;%KF方程2Kk=P*C'/(C*P*C'+R);%KF方程3x=xk+Kk*(z(k)-C*xk);%KF方程4x1(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方程5endk=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的观测值曲线')