%Step1 X_kAnd1_k = A * X_k_k; %Step2 P_kAnd1_k = A* P_k_k * A' + Q_k; %Step3 K_kAnd1 = P_kAnd1_k * C' * ( C * P_kAnd1_k * C' + R_kAnd1)^-1; %Step4 X_kAnd1_kAnd1 = X_kAnd1_k + K_kAnd1 * (Z_kAnd1 - C * X_kAnd1_k); %Step5 P_kAnd1_kAnd1 = (I - K_kAnd1 * C) * P_kAnd1_k;