% Kalman filter example

clear all
clc
% Generating 50 measurement data with true value being -0.37727 and
% standard deviation of 0.1
n_iter = 50;
x_true = -0.37727;
y = x_true + 0.1.*randn(n_iter,1);
Q = 1e-5;
R = 0.1^2;
% intial guesses
xhat_k = 0.0;
P_k = 1;
% To save data
P = [];
xhat = [];
for k = 1:n_iter
% time update
xhatminus_k = xhat_k;
Pminus_k = P_k + Q;
% measurement update
K_k = Pminus_k*inv(Pminus_k + R);
xhat_k = xhatminus_k + K_k*(y(k)-xhatminus_k);
P_k = (1-K_k)*Pminus_k;
%Save data
P = [P;P_k];
xhat = [xhat,xhat_k];

end
figure(1)
plot(P)
xlabel('k');
ylabel('P');

figure(2)
plot(xhat,'*')
hold on
plot(y,'r*')
hold on
ind = [1:1:n_iter];
plot(ind,x_true,'k*');
xlabel('k');

% Kalman Q1
clear all
clc
% DC motor transfer function
s = tf('s');
G = 100/(s^2 + s);
% DC motor state space
%a = [0,1;-1,0];
a = [0,1;0,-1];
b = [0;100];
c = [1,0];
d = [0];
sys_c = ss(a,b,c,d);
% Discretization
Ts = 0.1;
sys_d = c2d(sys_c,Ts,'zoh');
A = sys_d.a;
B = sys_d.b;
C = sys_d.c;
D = sys_d.d;
% Noise covariences
Q = [0.5^2*0.2^2,0.5*0.2*1*0.2;0.5*0.2*1*0.2,1^1*0.2^2];
R = 1^2;
% Intial guesses
xhat_k = [0;0];
P_k = Q;
% IO data
y = [0,0.5,2,4.3,8,12.2,17.5,23.5,30,37.8,45.9];
u = 1;
% To save xhat and P
xhat = [];
P = [];
for k = 1:length(y)
% time update
xhatminus_k = A*xhat_k + B*u;
Pminus_k = A*P_k*A'+Q;
% measurement update
K_k = Pminus_k*C'*inv(C*Pminus_k*C'+R);
xhat_k = xhatminus_k + K_k*(y(k)-C*xhatminus_k);
P_k = (1-K_k*C)*Pminus_k;

end

% Save data
xhat = [xhat,xhat_k];
P = [P;P_k];

figure(1)
plot(xhat(1,:),'*')

hold on
plot(y,'r*')
legend('x1_{hat}','y')
figure(2)
plot(xhat(2,:),'k*')
xlabel('k');
figure(3)
plot(P(:,1),'k*')
hold on
plot(P(:,2),'r*')
xlabel('k');
ylabel('P')