I think both of them should use Pm = (I-KH)*P_w_m...
Code: Select all
void kalmanCoreScalarUpdate()
// ====== COVARIANCE UPDATE ======
mat_mult(&Km, Hm, &tmpNN1m); // KH
for (int i=0; i<KC_STATE_DIM; i++) { tmpNN1d[KC_STATE_DIM*i+i] -= 1; } // KH - I
mat_trans(&tmpNN1m, &tmpNN2m); // (KH - I)'
mat_mult(&tmpNN1m, &this->Pm, &tmpNN3m); // (KH - I)*P
mat_mult(&tmpNN3m, &tmpNN2m, &this->Pm); // (KH - I)*P*(KH - I)'
Code: Select all
void kalmanCoreUpdateWithPKE()
// ====== COVARIANCE UPDATE ====== //
mat_mult(Km, Hm, &tmpNN1m); // KH, the Kalman Gain and H are the updated Kalman Gain and H
mat_scale(&tmpNN1m, -1.0f, &tmpNN1m); // I-KH
for (int i=0; i<KC_STATE_DIM; i++) { tmpNN1d[i][i] = 1.0f + tmpNN1d[i][i]; }
float Ppo[KC_STATE_DIM][KC_STATE_DIM]={0};
arm_matrix_instance_f32 Ppom = {KC_STATE_DIM, KC_STATE_DIM, (float *)Ppo};
mat_mult(&tmpNN1m, P_w_m, &Ppom); // Pm = (I-KH)*P_w_m