Extended Kalman Filter

Firmware/software/electronics/mechanics
Post Reply
WingViewWR
Beginner
Posts: 4
Joined: Sat Dec 12, 2020 12:18 pm

Extended Kalman Filter

Post by WingViewWR »

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
WendaZ
Beginner
Posts: 14
Joined: Sun Jul 14, 2019 10:35 pm

Re: Extended Kalman Filter

Post by WendaZ »

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.
WingViewWR
Beginner
Posts: 4
Joined: Sat Dec 12, 2020 12:18 pm

Re: Extended Kalman Filter

Post by WingViewWR »

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