两变量卡尔曼跟踪状态.m

4个卡尔曼滤波程序

%只有一个观测结果，可同时获得多个状态变量


%噪音可以是时变的，同时改变R Q看结果的不同


clc;                     
clear;
n=50;                     % 采样点                   
I=eye(2);                  % 单位阵
Q=[0 0;0 1];               % 系统噪声协方差阵
A=[1 1;0 1];               % 状态转移矩阵
B=[1 0];                  % 观测系数矩阵
X0=zeros(2,1)             % 状态向量初值
X1=X0;      
P0=I*10;                  % 估计误差协方差阵初值
Xk_predict=zeros(2,1);      % 为预测估值分配空间
Pk_predict=zeros(2,2);      % 为最优预测估值误差协方差阵分配空间
PK=zeros(2,2);            % 为最优滤波估值误差协方差阵分配空间
K=zeros(2,1);             % 为增益矩阵分配空间
X2=zeros(2,1);            % 为卡尔曼滤波值分配空间
Z(1)=0;                   % 为观测值分配空间
for k=1:1:n;
    W=1*sqrt(Q)*randn(2,1);                      % 系统的动态噪声
    %R=6;   
     R=19+(-1)^k;  
     RR=R;
    %RR=R*0.1;
    %% 观测噪声协方差阵
    V=sqrt(R)*randn(1,1);                      % 观测噪声
    X=A*X1+W;                                 % 状态方程
    Z(k)=B*X+V;                                % 观测系统的观测值
    X1=X;                                     % 状态矩阵更新
    C(k)=X(1,1);                           % 将一步预测矩阵第一个元素赋值给数组    
D(k)=X(2,1); 
    Xk_predict=A*X0;                       % 一步预测估值
    Pk_predict=A*P0*A'+Q;                  % 最优预测估值误差协方差阵
    K=Pk_predict*B'*inv(B*Pk_predict*B'+RR);    % 增益矩阵
    X2=Xk_predict+K*(Z(k)-B*Xk_predict);       % 卡尔曼滤波值
    E(k)=X2(1,1);                         % 将卡尔曼滤波值第一个元素赋值给数组
    F(k)=X2(2,1);                         % 将卡尔曼滤波值第二个元素赋值给数组
    Pk=(I-K*B)*Pk_predict;                 % 最优滤波估值误差协方差阵
    P0=Pk;                               % 估计误差协方差阵更新
    X0=X2;                              % 将一步预测矩阵第二个元素赋值给数组
end
k=1:1:n;
figure(3);
plot(C,'r');                                 % 系统状态值曲线(第一个状态输出结果，只有噪音)
hold on;
plot(Z,'b');                                 % 系统观测值曲线                              
hold on;

plot(E,'g');                                 % 卡尔曼滤波值曲线
grid on;
xlabel('采样点')
ylabel('各信号幅值')
title('卡尔曼滤波结果显示')
legend('系统状态值','系统观测值','卡尔曼滤波值')
figure(4);
plot(D,'r');                                 % 系统状态值曲线
hold on;
plot(F,'b');                                 % 卡尔曼滤波值曲线
grid on;
xlabel('采样点')
ylabel('各信号幅值')
title('卡尔曼滤波结果显示')
legend('系统状态值','卡尔曼滤波值')