function  out = cp_kalman(in)

% Ts and noise (sym specific data)
Ts = in(4);
rr = in(5:6);
qq = in(7:8);

% system parameters
m = 0.1;
L = 1;
g = 9.81;
M = 1;
d1 = 1;
d2 = 0.5;

% system information
q_ = (m+M) * g / (M*L);
A = [0,1,0,0 ; 0,-d1, -g*m/M,0 ; 0,0,0,1 ; 0,d1/L,q_,-d2] ;
B = [0, 1.0/M, 0., -1/(M*L)].' ; 

Ad = expm(A*Ts);
Bd = pinv(A)*(Ad-eye(4))*B;

H = [1,0,0,0;0,0,1,0];

Q = 10*diag([1e-10, qq(1), 1e-10, qq(2)]);
R = 100*diag([rr(1), rr(2)]);

% new u/y
u = in(1);
y = in(2:3);

% previous values
x_hat_z1 = in(9:12);
P_k_1 =  reshape( in(13:28), [4,4] );

%% Kalman Filter Equations

% 1: prediction
x_k_1 = Ad*x_hat_z1 + Bd*u;    % prediction
P_k = Ad*P_k_1*Ad'+Q;          % err cov. prediction

% 2: update
z = y - H*x_k_1;  % innovation
S = R + H*P_k*H'; % innovation covariance

K = P_k*H'/S;     % optimal Kalman gain

x_k = x_k_1 + K*z;      % a posteriori estimate
P_k = (eye(4)-K*H)*P_k; % a posteriori err cov. estimate

%%

out = [ x_k' , P_k(1,:) , P_k(2,:) , P_k(3,:) , P_k(4,:) ];