mykalmanfilter.m

各种滤波器

function X_estimate=myKalmanFilter(A,C,Q,R,X0,y)
% myKalmanFilter 对实测值y进行Kalman滤波,该系统为
% X(n+1)=A X(n))+v1(n);
% y(n)=C X(n)+v2(n);
% v1和v2是噪声,其方差分别为Q和R,初始条件为X0;

N=length(y);
m=length(C);
X_estimate=zeros(m,N);   % 对实际值的滤波估计
X_estimate(:,1)=X0;
P_estimate=R;            % 估计误差

for k=2:N
    X_predict=A*X_estimate(:,k-1);       % 一步预测
    P_predict=A*P_estimate*A'+Q;                % 预测误差
    K=P_predict*C'/(C*P_predict*C'+R);          % Kalman增益
    X_estimate(:,k)=X_predict+K*(y(k)-C*X_predict);    % 由实测修正滤波估计
    P_estimate=P_predict-K*C*P_predict;         % 滤波估计误差
end