kaerman.rar_kaerman_卡尔曼 参数_卡尔曼滤波器

ts=0.001; A1=[0,1;0,-a]; B1=[0;b]; C1=[1,0]; D1=0; [A,B,C,D]=c2dm(A1,B1,C1,D1,ts,'z'); %变连续系统为离散系统 x=[-0.5;-0.5]; r_1=0; r_2=0; Q=10; %Covariances of w Rm=10; %Covariances of v P=B*Q*B'; %Initial error covariance for k=1:1:2000 time(k)=k*ts; r(k)=0.5*sin(1*2*pi*k*ts); c=30;eq=150;q=300; Ce=[c,1]; %Using Waitui method dr(k)=(r(k)-r_1)/ts; dr_1=(r_1-r_2)/ts; r1(k)=2*r(k)-r_1; dr1(k)=2*dr(k)-dr_1; R=[r(k);dr(k)]; R1=[r1(k);dr1(k)]; E=R-x; e(k)=E(1); de(k)=E(2); s(k)=Ce*E; ds(k)=-eq*ts*sign(s(k))-q*ts*s(k); u(k)=inv(Ce*B)*(Ce*R1-Ce*A*x-s(k)-ds(k)); %inv距阵求逆 wn(k)=0.1*rands(1); %Process noise on u u(k)=u(k)+wn(k); x=A*x+B*u(k); v(k)=0.3*rands(1); %Measurment noise on y yv(k)=C*x+v(k); M=1 if M==1 % kalman filter Mn=P*C'/(C*P*C'+Rm); P=A*P*A'+B*Q*B'; P=(eye(2)-Mn*C)*P; x=A*x+Mn*(yv(k)-C*A*x); ye(k)=C*x; errcov(k)=C*P*C'; %Covariance of estimation error elseif M==2 %No filter ye(k)=yv(k); x(1)=ye(k); end %Update Parameters r_2=r_1; r_1=r(k); end