kalman_myself.m

各种滤波器

% kalman_myself
% 模拟简谐振子m=1,k=1的振动,初始状态 v0=10,x0=10;
% 由牛顿第二定律有 m*dv/dt=-kx ==> dv/dt=-x
% 由运动定律有 dx/dt=v ,所以
% (dx/dt=v    )
% (dv/dt=-x   )
% 离散化这个方程组得
% (x(n+1)=x(n)+Tv(n);   )
% (v(n+1)=v(n)-Tx(n);   )
% 令状态变量X=[x(n);v(n)],得状态方程及测量方程
% X(n+1)=AX(n)+V1(n);
% Y(n)=CX(n)+V2(n);  
% cov(V1)=Q;cov(V2)=R;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalman滤波算法
% ================ Time Update ================
% X_predict(k)=A*X_estimate(k-1)+B*u(k);
% P_predict(k)=A*P_estimate(k-1)*A'+Q;
% ============ Measurement Update =============
% K=P_predict(k)*C'/(C*P_predict(k)*C'+R);
% X_estimate(k)=X_predict(k)+K*(Y(k)-C*X_predict(k));
% P_estimate(k)=P_predict(k)-K*C*P_predict(k);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T=0.05;Tn=500;
t=0:T:Tn;N=length(t);
A=[1,T;-T,1];Q=T;
C=[1,0];R=200;

x0=1;v0=2;                            % Initial condition
Amp=sqrt(x0^2+v0^2);fai=atan(x0/v0);
xreal=[Amp*sin(t+fai);Amp*cos(t+fai)];  % Analytic solution
y=xreal(1,:)+sqrt(R)*randn(1,N);        % Measurement
X_estimate=[x0;v0];   % Filter the measurement values
P_estimate=R*eye(2);  % the Error Covariance of the Filter Estimate

for k=2:N
    X_predict=A*X_estimate(:,k-1);        % One step Prediction
    P_predict=A*P_estimate*A'+Q;          % the Error Covariance of the Prediction
    K=P_predict*C'/(C*P_predict*C'+R);    % Kalman Gain
    X_estimate(:,k)=X_predict+K*(y(k)-C*X_predict);    % Correct the Filter Estimate
    P_estimate=P_predict-K*C*P_predict;   % the Error Covariance of the Filter Estimate
end

% X_estimate=myKalmanFilter(A,C,Q,R,[x0;v0],y);

figure(1)
clf
subplot(211)
hold on
plot(t,y,'g')
plot(t,X_estimate(1,:),'b')
plot(t,xreal(1,:),'r--');
legend('Measurement','Kalman Filter','Analytic Curve');
title('Displacement');
axis([0 Tn -Amp-3*sqrt(R) Amp+3*sqrt(R)])
hold off
subplot(212)
hold on
plot(t,X_estimate(2,:),'b')
plot(t,xreal(2,:),'r--');
legend('Kalman Filter','Analytic Curve');
title('Velocity');
axis([0 Tn -Amp-sqrt(R) Amp+sqrt(R)])
hold off