%只有一個觀測結果，可同時獲得多個狀態變量


%噪音可以是時變的，同時改變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('系統狀態值','卡爾曼濾波值')