X_0 = [0; 0; 0];
Y = 1;

A = [0.5 0 0; 0 0.5 0; 0.5 0.5 0];
C = [0 0 1];
P_0 = [4 0 0; 0 1 0; 0 0 5];
Q = [4 0 0; 0 1 0; 0 0 0];

%%
X_1_0 = A*X_0;

P_1_0 = A*P_0*A' + Q;

K = P_1_0*C' *(C*P_1_0*C')^(-1);

X_1_1 = X_1_0 + K*(Y-C*X_1_0)