多元信息融合,噪声相关的kalman滤波

%假设X的第一维为位移,第二维是速度
%相关噪声S是个2*2的符合高斯分布的矩阵.
%创造出一个2行,200列的符合高斯分布,方差是S0的大矩阵,
%再用mat2cell命令分成100个2*2个元胞数组,再用大括号{}提取出元胞数组的元素
%编程如下：
clear all;
close all;
clc
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];
S0=[0.39,0;0,0.39];
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);
A=sqrt(S0)*randn(2,200);
S=mat2cell(A,ones(1,1)*2,ones(100,1)*2);%矩阵分块,创建100个 2*2的元胞数组
%编程如下:
    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;%X_pre 是一个2*1 不断更新的列向量
        P_pre(:,:,k)=F*P0*F'+Q;  %P_pre 是一个2*2 不断更新的2*2矩阵,用三维矩阵更新二维矩阵
        K(:,:,k)=(P_pre(:,:,k)*H'+S{:,1})*inv(H*P_pre(:,:,k)*H'+R+H*S{:,1}+S{:,1}'*H');  %S{:,1}提取元胞数组中的内容
        X_est(:,k)=X_pre(:,k)+K(:,:,k)*(Y(:,k)-H*X_pre(:,k));%X_est 是一个2*1 不断更新的列向量
        P_est(:,:,k)=P_pre(:,:,k)-K(:,:,k)*(P_pre(:,:,k)*H'+S{:,1})';%P_est 是一个2*2 不断更新的2*2矩阵,用三维矩阵更新二维矩阵
    else
         X_pre(:,k)=F*X_est(:,k-1);
        P_pre(:,:,k)=F*P_est(:,:,k-1)*F'+Q;
        K(:,:,k)=(P_pre(:,:,k)*H'+S{:,k})*inv(H*P_pre(:,:,k)*H'+R+H*S{:,k}+S{:,k}'*H'); %S{:,k}提取元胞数组中的内容
        X_est(:,k)=X_pre(:,k)+K(:,:,k)*(Y(:,k)-H*X_pre(:,k));
        P_est(:,:,k)=P_pre(:,:,k)-K(:,:,k)*(P_pre(:,:,k)*H'+S{:,k})';
    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');

%假设X的第一维为位移,第二维是速度
%相关噪声S是个22的符合高斯分布的矩阵.
%创造出一个2行,200列的符合高斯分布,方差是S0的大矩阵,
%再用mat2cell命令分成100个22个元胞数组,再用大括号{}提取出元胞数组的元素