% kalman filtering
% 用一個線性隨機微分方程來描述離散控制過程系統(tǒng)：X(k)=A X(k-1)+B U(k)+W(k) 
% 系統(tǒng)的測量值：Z(k)=H X(k)+V(k) 
% X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數，對于多模型系統(tǒng)，他們?yōu)榫仃嚒?% Z(k)是k時刻的測量值，H 是測量系統(tǒng)的參數，對于多測量系統(tǒng)，H為矩陣。
% W(k)和V(k)分別表示過程和測量的噪聲。 他們被假設成高斯白噪聲，他們的協(xié)方差矩陣分別是Q，R（假設不隨系統(tǒng)狀態(tài)變化而變化）。
% 算法過程
% 1.更新預測值X(k|k-1):X(k|k-1)=A*X(k-1|k-1)+B*U(k)
% 2.更新X(k|k-1)狀態(tài)的協(xié)方差矩陣:P(k|k-1)=A*P(k-1|k-1)*A+Q
% 3.結合預測值和測量值，得到現在狀態(tài)X(k)的最佳估計值X(k|k): X(k|k)= X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1)) 
% 其中卡爾曼增益 Kg(k)= P(k|k-1) H'*inv(H P(k|k-1) H’ + R)
% 4.更新X(k|k)狀態(tài)的斜方差矩陣:P(k|k)=（I-Kg(k) H）P(k|k-1) 
 
load initial_track  y s; % y:initial data,s:data with noise
clc;
T=0.1;

% yp denotes the sample value of position
% yv denotes the sample value of velocity
% Y=[yp(n);yv(n)];
% error deviation caused by the random acceleration 
% known data
Y=zeros(2,200);
Y0=[0;1];            %零時刻初始值 X(0|0)
Y(:,1)=Y0;
A=[1 T
    0 1];          
B=[1/2*(T)^2 T]';     %激勵轉移矩陣
H=[1 0];

C0=[10 0
    0 1];             %初始誤差協(xié)方差矩陣賦值 P(0|0)=P(0)
C=[C0 zeros(2,2*199)];
Q=(0.1)^2;            %過程噪聲方差
R=(0.1)^2;            %測量噪聲方差


% kalman algorithm ieration
for n=1:199
    i=(n-1)*2+1;
    Y(:,n+1)=A*Y(:,n);                            %更新預測值X(k|k-1):X(k|k-1)=A*X(k-1|k-1)+B*U(k)
    
    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B' ;                 %更新X(k|k-1)狀態(tài)的協(xié)方差矩陣:P(k|k-1)=A*P(k-1|k-1)*A+Q
    
    K=C(:,i+2:i+3)*H'*inv(H*C(:,i+2:i+3)*H'+R);   %卡爾曼增益Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    
    Y(:,n+1)=Y(:,n+1)+K*(s(:,n+1)-H*Y(:,n+1));    %結合預測值和測量值，得到現在狀態(tài)X(k)的最佳估計值X(k|k)
                                                  %X(k|k)= X(k|k-1)+Kg(k)*(Z(k)-H X(k|k-1)) 
                                                  
    C(:,i+2:i+3)=(eye(2,2)-K*H)*C(:,i+2:i+3);     %更新X(k|k)狀態(tài)的斜方差矩陣:P(k|k)=（I-Kg(k) H）P(k|k-1) 
           
end

% the diagram of position after filtering
subplot(221)
t=0:0.1:20-0.1;
yp=Y(1,:);
plot(t,yp,'+');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');

% the diagram of velocity after filtering
subplot(222)
yv=Y(2,:);
plot(t,yv,'+');
xlabel('time');
ylabel('yv velocity');
title('the velocity caused by random acceleration');

subplot(223);
plot(t,yp-y);
title('位置跟蹤誤差');

subplot(224);
plot(t,yv-0.5);
title('速度跟蹤誤差');