clc;
clear all;
close all;
N = 350;
W = [ 1 0 ; 0 1 ]; % State-error
V = [ 1 0 ; 1 0 ]; % Measurement-error
H = [ 1 0 ; 0 0 ]; % Measurement-state
Q = [ 0.001 0 ; 0 0 ]; % Process noise covariance
R = [ 0.1 0 ; 0 0.01 ]; % Measurement noise covariance
x = zeros(2,N);
apriori_state = zeros(2,N);
aposteriori_state = zeros(2,N);
apriori_error = zeros(2,2,N);
aposteriori_error = zeros(2,2,N);
z = zeros(2,N);
K = zeros(2,2,N);
x(:,1) = [ 1 ; 3*pi/500 ];
aposteriori_state(:,1) = [ 1 ; 1*pi/500 ];
aposteriori_error(:,:,1) = [ 1 0 ; 0 1 ];
error2(:,1)=aposteriori_state(:,1)-x(:,1);
randn('state',2);
n1=sqrt(Q)*[randn;randn];
n2=sqrt(R)*[randn;randn];
for i = 2:N
x(:,i) = [sin(x(2,i-1)*(i-1));x(2,i-1)] + n1;
z(:,i) = x(:,i) + n2;
apriori_state(:,i) = [sin(aposteriori_state(2,i-1)*(i-1));aposteriori_state(2,i-1)];
Fi = [0 (i-1)*cos(aposteriori_state(2,i-1)*(i-1)) ; 0 1 ];
Qi = Q;
Ri = R;
apriori_error(:,:,i) = Fi*aposteriori_error(:,:,i-1)*Fi' + Qi;
K(:,:,i) = apriori_error(:,:,i)*H' / (H*apriori_error(:,:,i)*H'+Ri);
aposteriori_state(:,i) = apriori_state(:,i) + K(:,:,i) * (z(:,i) - apriori_state(:,i));
aposteriori_error(:,:,i) = (eye(2) - K(:,:,i)*H) * apriori_error(:,:,i);
error1(:,i)=apriori_state(:,i)-x(:,i);
error2(:,i)=aposteriori_state(:,i)-x(:,i);
end
figure(1);
t1=2:N;
subplot(3,1,1);
plot(t1,x(1,2:N));
ylim([-2 +2]);
title('Actual state');
xlabel('Time');
ylabel('Position');
grid on;
subplot(3,1,2);
plot(t1,apriori_state(1,2:N));
ylim([-2 +2]);
title('Predicted state');
xlabel('Time');
ylabel('Position');
grid on;
subplot(3,1,3);
plot(t1,aposteriori_state(1,2:N));
ylim([-2 +2]);
title('Updated state');
xlabel('Time');
ylabel('Position');
grid on;
figure(2);
t2=2:N;
subplot(2,1,1);
plot(t2,error1(1,2:N));
title('Error between actual value and predicted value');
xlabel('Time');
ylabel('Position');
grid on;
subplot(2,1,2);
plot(t2,error2(1,2:N));
title('Error between actual value and updated value');
xlabel('Time');
ylabel('Position');
grid on;