Example for Extended Kalman Filter - 2023.1 English

//Load F/B_mat/Uq_mat/Dq_mat/X0_mat/U0_mat/D0_mat

for(int iteration=0; iteration< count; iteration++)
{
    if(iteration ==0)
        model_fx(X0_mat, fx);// update fx using X0_mat
    else
model_fx(Xout_mat, fx); // update fx using Xout_mat

        unsigned char initFlag;
        if(iteration ==0)
            initFlag = INIT_EN;
        else
            initFlag = EKF_MEM_OPT+INIT_EN;

        //Initialization
KalmanFilter (F, B_mat, Uq_mat, Dq_mat, H, fx, U0_mat, D0_mat, R_mat, hx, y_mat, Xout_mat, Uout_mat, Dout_mat, initFlag);

        //Time Update
KalmanFilter (F, B_mat, Uq_mat, Dq_mat, H, fx, U0_mat, D0_mat, R_mat, hx, y_mat, Xout_mat, Uout_mat, Dout_mat, TIMEUPDATE_EN + XOUT_EN_TU + UDOUT_EN_TU);
        for(int index=0; index< M_MEAS; index++)
        {
if(iteration ==0)
// update hx/H using X0_mat for one measurement at a time
            model_hxH(X0_mat, hx, H, index);
        else
        //update hx/H using Xout_mat for one measurement at a time
model_hxH(Xout_mat, hx, H, index);

//Load R_mat
            R_mat.write_float(0,R_matrix[index][index]);

            //Load y_mat
            Y_mat.write_float(0,measurement_vector[index]);

//Measurement Update
KalmanFilter (F, B_mat, Uq_mat, Dq_mat, H, fx, U0_mat, D0_mat, R_mat, hx, y_mat, Xout_mat, Uout_mat, Dout_mat, MEASUPDATE_EN + XOUT_EN_MU + UDOUT_EN_MU);
        }
}