Bitcraze Forums

Posted: **Tue May 11, 2021 2:35 am**

Dose any one konw why function kalmanCoreScalarUpdate and function kalmanCoreUpdateWithPKE in "kalman_core.c" just use different covariance update formula.
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

Posted: **Tue May 11, 2021 2:06 pm**

Hello WingViewWR,

The original EKF written by Mark Muller (ETH) uses the Joseph’s form covariance update, which is the one in function kalmanCoreScalarUpdate().
This general Joseph's form covariance update can maintain the symmetry and positive definiteness of the covariance matrix at the expense of computation burden. A derivation for Joseph's form covariance update can be found on Wikipedia (https://en.wikipedia.org/wiki/Kalman_filter).

The function kalmanCoreUpdateWithPKE() is a new function mainly used for the M-estimation-based robust Kalman filter for UWB localization (mm_distance_robust.c, mm_tdoa_robust.c). It updates the covariance based on the prior covariance P, Kalman gain K, and innovation error E. I implemented the conventional covariance update mainly considering the computation costs. After the covariance updates, the posteriori covariance is enforced to be symmetric by:

Code: Select all

 float p = 0.5f*this->P[i][j] + 0.5f*this->P[j][i];

Joseph's form should also work with a slightly increased computation expense.

Hope this answers your questions.

Posted: **Wed May 12, 2021 5:28 am**

Thank you very much!
I learned a lot from it.

Bitcraze Forums

Extended Kalman Filter

Extended Kalman Filter

Re: Extended Kalman Filter

Re: Extended Kalman Filter