kalman预测器

%二维系统.
%状态转移矩阵F=[1,1;0,1],观测矩阵H=[0.9,0;0,1.1],过程噪声方差Q=[0.01,0;0,0.01],R=[0.1,0;0,0.1]观测噪声方差R=1
%初始状态X0=[1;0.3],初始协方差P0=[0.1,0;0,0.1]
%状态方程X(k+1)=F(k+1,k)X(k)+w(k+1),假设X第一维度是位移,第二维度是速度
%观测方程Y(k+1)=H(k+1)X(k+1)+v(k+1)
%根据观测方程和状态方程的性质,如果X为n维,Y为m维.则F是n*n矩阵,H为m*n矩阵
%X_pre 是一个2*1 不断更新的列向量
%P_pre 是一个2*2 不断更新的2*2矩阵,用三维矩阵更新二维矩阵
%K 是一个2*2 不断更新的2*2矩阵,用三维矩阵更新二维矩阵
%X_est 是一个2*1 不断更新的列向量
%P_est 是一个2*2 不断更新的2*2矩阵,用三维矩阵更新二维矩阵
%编程如下：
F=[1,1;0,1];
H=[0.9,0;0,1.1];
Q=[0.01,0;0,0.01];
R=[0.1,0;0,0.1];
X0=[1;0.3];
P0=[0.1,0;0,0.1];
N=100;
W=sqrt(Q)*randn(2,N);
V=sqrt(R)*randn(2,N);
%模拟观测:
X(:,1)=F*X0+W(:,1);
for k=2:N
    X(:,k)=F*X(:,k-1)+W(:,k);
end
for k=1:N
    Y(:,k)=H*X(:,k)+V(:,k);
end
for k=1:N
    if k==1
        X_pre(:,k)=F*X0;
        Y_pre(:,k)=H*X0;
        P_pre(:,:,k)=F*P0*F'+Q;
        Pxy(:,:,k)=P_pre(:,:,k)*H';
        Pyy(:,:,k)=H*P_pre(:,:,k)*H'+R;
        K(:,:,k)=Pxy(:,:,k)*inv(Pyy(:,:,k));  
        X_est(:,k)=F*(X_pre(:,k)+K(:,:,k)*(Y(:,k)-Y_pre(:,k)));
        P_est(:,:,k)=F*(P_pre(:,:,k)-K(:,:,k)*H*P_pre(:,:,k))*F'+Q;
    else
        X_pre(:,k)=F*X_est(:,k-1);
        Y_pre(:,k)=H*X_est(:,k-1);
        P_pre(:,:,k)=F*P_est(:,:,k-1)*F'+Q;
        Pxy(:,:,k)=P_pre(:,:,k-1)*H';
        Pyy(:,:,k)=H*P_pre(:,:,k-1)*H'+R;
        K(:,:,k)=Pxy(:,:,k)*inv(Pyy(:,:,k));  
        X_est(:,k)=F*(X_pre(:,k)+K(:,:,k)*(Y(:,k)-Y_pre(:,k)));
        P_est(:,:,k)=F*(P_pre(:,:,k)-K(:,:,k)*H*P_pre(:,:,k))*F'+Q;
    end
end

e1=zeros(1,N);
e2=zeros(1,N);
for k=1:N
    e1(1,k)=abs(X(1,k)-X_est(1,k));
    e2(1,k)=abs(X(2,k)-X_est(2,k));
end
%绘图分析:
figure(1)
k=1:N;
plot(k,X(1,k),'-o',k,X_est(1,k),'-x');
xlabel('时间'),ylabel('状态值和估计值');
legend ('位移状态值','位移估计值');
title('位移的状态值和估计值曲线');

figure(2)
k=1:N;
plot(k,X(2,k),'-o',k,X_est(2,k),'-x');
xlabel('时间'),ylabel('状态值和估计值');
legend ('速度状态值','速度的估计值');
title('速度的状态值和估计值曲线');

figure(3)
subplot(2,1,1);
plot(e1,'b-');
xlabel('时间'),ylabel('误差绝对值');
title('位移状态值与估计值的误差绝对值');
subplot(2,1,2);
plot(e2,'k-');
xlabel('时间'),ylabel('误差绝对值');
title('速度状态值与估计值的误差绝对值');

figure(4)
k=1:N;
subplot(2,1,1);
plot(P_est(1,k),'b-');
xlabel('时间'),ylabel('估计误差协方差误差1');
title('估计误差协方差误差1');
subplot(2,1,2);
plot(P_est(2,k),'k-');
xlabel('时间'),ylabel('估计误差协方差误差2');
title('估计误差协方差误差2');