clear all
clc

%% 條件初值
measnoise=10;
accelnoise=0.5;
duration=100;
%%dt=0.2;
dt=0.1;

%% 系統模型
a=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
c=[1 0 0 0;0 1 0 0];
%% Q=accelnoise^2*[dt^4/4 dt^4/4 dt^3/2 dt^3/2;dt^4/4 dt^4/4 dt^3/2 dt^3/2;dt^3/2 dt^3/2 dt^2 dt^2;dt^3/2 dt^3/2 dt^2 dt^2];
 %% Q=0.0001*eye(4);
 %% Q=0.01*eye(4);
 
%% Q=[0.0001 0 0 0;0 0.0001 0 0;0 0 0.1 0;0 0 0 0.1];
%% R=measnoise^2*[dt^2 dt^2;dt^2 dt^2];
%% R=measnoise^2*eye(2);  %%為什么Q，R要取這樣的值？
R=10^4*eye(2);

%% 賦初值
x=[0;0;0;0];
xhat=x;
pest=Q;
%% Inn=[0;0];

%% 
position=[];
positionmeasure=[];
positionhat=[];
Innovation=[];
Gaintk=[];
pp=[];




%% 循環程序
for t=0:dt:duration
    x=[10*sin(t);0;0;0];
    xest=x;
    
    processnoise=accelnoise*[dt^2/2;dt^2/2;dt;dt]*randn;
    x=a*x+processnoise;
    
    measurenoise=measnoise*[randn;randn];
    z=c*x+measurenoise;
    
    Inn=z-c*xest;
    
    s=c*pest*c'+R;
    
    K=pest*c'*inv(s);
    
    xhat=xest+K*Inn;
    
    %% P=(eye(4)-K*c)*pest;
    P=pest-pest*c'*inv(s)*c*pest;
    pest=a*P*a'+Q;
    xest=a*xhat;


%% 輸出準備
position=[position;x(1)];
positionmeasure=[positionmeasure;z(1)];
positionhat=[positionhat;xhat(1)];
Innovation=[Innovation;Inn(1)];
Gaintk=[Gaintk;K(1)];
%% pp=[pp;P(1)];
end

%%輸出
t=0:dt:duration;

figure(1)
plot(t,position,'r',t,positionmeasure,'b',t,positionhat,'c')
grid on
title('kalman')
ylabel('position(m)');
xlabel('time(sec)');

figure(2)
plot(t,Innovation,'b');
grid on
ylabel('Innovation(m)');
xlabel('Time(sec)');

figure(3)
plot(t,Gaintk)
grid on
ylabel('Gaintk');
xlabel('Time(sec)');



figure(4)
plot(t,position)
grid on
ylabel('True position trajectory (m)');
xlabel('Time(sec)');

figure(5)
plot(t,positionmeasure)
grid on
ylabel('Measured Position Trajectory (m)');
xlabel('Time(sec)');

figure(6)
plot(t,positionhat)
grid on
ylabel('Estimated Position Trajectory (m)');
xlabel('Time(sec)');