void EKF_New(EKF_Filter* ekf)
{
float32_t *H = ekf->H_f32;
float32_t *P = ekf->P_f32;
float32_t *Q = ekf->Q_f32;
float32_t *R = ekf->R_f32;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->P, EKF_STATE_DIM, EKF_STATE_DIM, ekf->P_f32);
arm_mat_zero_f32(&ekf->P);
P[0] = P[8] = P[16] = P[24] = EKF_PQ_INITIAL;
P[32] = P[40] = P[48] = EKF_PW_INITIAL;
//
arm_mat_init_f32(&ekf->Q, EKF_STATE_DIM, EKF_STATE_DIM, ekf->Q_f32);
arm_mat_zero_f32(&ekf->Q);
Q[0] = Q[8] = Q[16] = Q[24] = EKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = EKF_QW_INITIAL;
//
arm_mat_init_f32(&ekf->R, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->R_f32);
arm_mat_zero_f32(&ekf->R);
R[0] = R[14] = R[28] = R[42] = EKF_RQ_INITIAL;
R[56] = R[70] = R[84] = EKF_RA_INITIAL;
R[98] = R[112] = R[126] = EKF_RW_INITIAL;
R[140] = R[154] = R[168] = EKF_RM_INITIAL;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->K, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->K_f32);
arm_mat_init_f32(&ekf->KT, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, ekf->KT_f32);
arm_mat_init_f32(&ekf->S, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->S_f32);
//
arm_mat_init_f32(&ekf->F, EKF_STATE_DIM, EKF_STATE_DIM, ekf->F_f32);
arm_mat_zero_f32(&ekf->F);
arm_mat_identity_f32(&ekf->F, 1.0f);
//
arm_mat_init_f32(&ekf->FT, EKF_STATE_DIM, EKF_STATE_DIM, ekf->FT_f32);
//
arm_mat_init_f32(&ekf->H, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, H);
arm_mat_zero_f32(&ekf->H);
H[0] = H[8] = H[16] = H[24] = 1.0f; //q row 0~3, col 0~3
H[53] = H[61] = H[69] = 1.0f; //w row 7~9, col 4~6
//
arm_mat_init_f32(&ekf->HT, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->HT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->I, EKF_STATE_DIM, EKF_STATE_DIM, ekf->I_f32);
arm_mat_zero_f32(&ekf->I);
arm_mat_identity_f32(&ekf->I, 1.0f);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->X, EKF_STATE_DIM, 1, ekf->X_f32);
arm_mat_zero_f32(&ekf->X);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->Y, EKF_MEASUREMENT_DIM, 1, ekf->Y_f32);
arm_mat_zero_f32(&ekf->Y);
//////////////////////////////////////////////////////////////////////////
//
arm_mat_init_f32(&ekf->tmpP, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpP_f32);
arm_mat_init_f32(&ekf->tmpX, EKF_STATE_DIM, 1, ekf->tmpX_f32);
arm_mat_init_f32(&ekf->tmpYX, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, ekf->tmpYX_f32);
arm_mat_init_f32(&ekf->tmpXY, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->tmpXY_f32);
arm_mat_init_f32(&ekf->tmpXX, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpXX_f32);
arm_mat_init_f32(&ekf->tmpXXT, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpXXT_f32);
arm_mat_init_f32(&ekf->tmpS, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->tmpS_f32);
//////////////////////////////////////////////////////////////////////////
}
void UKF_New(UKF_Filter* ukf)
{
float32_t *P = ukf->P_f32;
float32_t *Q = ukf->Q_f32;
float32_t *R = ukf->R_f32;
float32_t *Wc = ukf->Wc_f32;
//scaling factor
float32_t lambda = UKF_alpha * UKF_alpha *((float32_t)UKF_STATE_DIM + UKF_kappa) - (float32_t)UKF_STATE_DIM;
float32_t gamma = (float32_t)UKF_STATE_DIM + lambda;
//weights for means
ukf->Wm0 = lambda / gamma;
ukf->Wmi = 0.5f / gamma;
//weights for covariance
arm_mat_init_f32(&ukf->Wc, UKF_SP_POINTS, UKF_SP_POINTS, ukf->Wc_f32);
arm_mat_identity_f32(&ukf->Wc, ukf->Wmi);
Wc[0] = ukf->Wm0 + (1.0f - UKF_alpha * UKF_alpha + UKF_beta);
//scaling factor need to tune!
arm_sqrt_f32(gamma, &ukf->gamma);
//////////////////////////////////////////////////////////////////////////
//initialise P
arm_mat_init_f32(&ukf->P, UKF_STATE_DIM, UKF_STATE_DIM, ukf->P_f32);
arm_mat_identity_f32(&ukf->P, 1.0f);
P[0] = P[8] = P[16] = P[24] = UKF_PQ_INITIAL;
P[32] = P[40] = P[48] = UKF_PW_INITIAL;
arm_mat_init_f32(&ukf->PX, UKF_STATE_DIM, UKF_STATE_DIM, ukf->PX_f32);
arm_mat_init_f32(&ukf->PY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->PY_f32);
arm_mat_init_f32(&ukf->tmpPY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->tmpPY_f32);
arm_mat_init_f32(&ukf->PXY, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->PXY_f32);
arm_mat_init_f32(&ukf->PXYT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->PXYT_f32);
//initialise Q
arm_mat_init_f32(&ukf->Q, UKF_STATE_DIM, UKF_STATE_DIM, ukf->Q_f32);
Q[0] = Q[8] = Q[16] = Q[24] = UKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = UKF_QW_INITIAL;
//initialise R
arm_mat_init_f32(&ukf->R, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->R_f32);
R[0] = R[14] = R[28] = R[42] = UKF_RQ_INITIAL;
R[56] = R[70] = R[84] = UKF_RA_INITIAL;
R[98] = R[112] = R[126] = UKF_RW_INITIAL;
R[140] = R[154] = R[168] = UKF_RM_INITIAL;
arm_mat_init_f32(&ukf->XSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->XSP_f32);
arm_mat_init_f32(&ukf->tmpXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpXSP_f32);
arm_mat_init_f32(&ukf->tmpXSPT, UKF_SP_POINTS, UKF_STATE_DIM, ukf->tmpXSPT_f32);
arm_mat_init_f32(&ukf->tmpWcXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpWcXSP_f32);
arm_mat_init_f32(&ukf->tmpWcYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpWcYSP_f32);
arm_mat_init_f32(&ukf->YSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->YSP_f32);
arm_mat_init_f32(&ukf->tmpYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpYSP_f32);
arm_mat_init_f32(&ukf->tmpYSPT, UKF_SP_POINTS, UKF_MEASUREMENT_DIM, ukf->tmpYSPT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->K, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->K_f32);
arm_mat_init_f32(&ukf->KT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->KT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->X, UKF_STATE_DIM, 1, ukf->X_f32);
arm_mat_zero_f32(&ukf->X);
arm_mat_init_f32(&ukf->tmpX, UKF_STATE_DIM, 1, ukf->tmpX_f32);
arm_mat_zero_f32(&ukf->tmpX);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->Y, UKF_MEASUREMENT_DIM, 1, ukf->Y_f32);
arm_mat_zero_f32(&ukf->Y);
arm_mat_init_f32(&ukf->tmpY, UKF_MEASUREMENT_DIM, 1, ukf->tmpY_f32);
arm_mat_zero_f32(&ukf->tmpY);
//////////////////////////////////////////////////////////////////////////
}
void SRCKF_New(SRCKF_Filter* srckf)
{
float32_t kesi;
arm_matrix_instance_f32 KesiPuls, KesiMinu;
float32_t KesiPuls_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM], KesiMinus_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM];
float32_t *S = srckf->S_f32;
float32_t *SQ = srckf->SQ_f32;
float32_t *SR = srckf->SR_f32;
//////////////////////////////////////////////////////////////////////////
//initialise weight
srckf->W = 1.0f / (float32_t)SRCKF_CP_POINTS;
arm_sqrt_f32(srckf->W, &srckf->SW);
//initialise kesi
//generate the cubature point
kesi = (float32_t)SRCKF_STATE_DIM;
arm_sqrt_f32(kesi, &kesi);
arm_mat_init_f32(&KesiPuls, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiPuls_f32);
arm_mat_init_f32(&KesiMinu, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiMinus_f32);
arm_mat_identity_f32(&KesiPuls, kesi);
arm_mat_identity_f32(&KesiMinu, -kesi);
arm_mat_init_f32(&srckf->Kesi, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->Kesi_f32);
arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiPuls, 0, 0);
arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiMinu, 0, SRCKF_STATE_DIM);
arm_mat_init_f32(&srckf->iKesi, SRCKF_STATE_DIM, 1, srckf->iKesi_f32);
//initialise state covariance
arm_mat_init_f32(&srckf->S, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->S_f32);
arm_mat_zero_f32(&srckf->S);
S[0] = S[8] = S[16] = S[24] = SRCKF_PQ_INITIAL;
S[32] = S[40] = S[48] = SRCKF_PW_INITIAL;
arm_mat_init_f32(&srckf->ST, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->ST_f32);
//initialise measurement covariance
arm_mat_init_f32(&srckf->SY, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SY_f32);
arm_mat_init_f32(&srckf->SYI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYI_f32);
arm_mat_init_f32(&srckf->SYT, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYT_f32);
arm_mat_init_f32(&srckf->SYTI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYTI_f32);
arm_mat_init_f32(&srckf->PXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->PXY_f32);
arm_mat_init_f32(&srckf->tmpPXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->tmpPXY_f32);
//initialise SQ
arm_mat_init_f32(&srckf->SQ, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->SQ_f32);
arm_mat_zero_f32(&srckf->SQ);
SQ[0] = SQ[8] = SQ[16] = SQ[24] = SRCKF_QQ_INITIAL;
SQ[32] = SQ[40] = SQ[48] = SRCKF_QW_INITIAL;
//initialise SR
arm_mat_init_f32(&srckf->SR, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SR_f32);
arm_mat_zero_f32(&srckf->SR);
SR[0] = SR[7] = SR[14] = SRCKF_RA_INITIAL;
SR[21] = SR[28] = SR[35] = SRCKF_RM_INITIAL;
//other stuff
//cubature points
arm_mat_init_f32(&srckf->XCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCP_f32);
//propagated cubature points
arm_mat_init_f32(&srckf->XPCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XPCP_f32);
arm_mat_init_f32(&srckf->YPCP, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YPCP_f32);
arm_mat_init_f32(&srckf->XCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCPCM_f32);
arm_mat_init_f32(&srckf->tmpXCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->tmpXCPCM_f32);
arm_mat_init_f32(&srckf->YCPCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YCPCM_f32);
arm_mat_init_f32(&srckf->YCPCMT, SRCKF_CP_POINTS, SRCKF_MEASUREMENT_DIM, srckf->YCPCMT_f32);
arm_mat_init_f32(&srckf->XCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_STATE_DIM, srckf->XCM_f32);
//initialise fill SQ
arm_mat_setsubmatrix_f32(&srckf->XCM, &srckf->SQ, 0, SRCKF_CP_POINTS);
arm_mat_init_f32(&srckf->YCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->YCM_f32);
//initialise fill SR
arm_mat_setsubmatrix_f32(&srckf->YCM, &srckf->SR, 0, SRCKF_CP_POINTS);
arm_mat_init_f32(&srckf->XYCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->XYCM_f32);
//Kalman gain
arm_mat_init_f32(&srckf->K, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->K_f32);
//////////////////////////////////////////////////////////////////////////
//state vector
arm_mat_init_f32(&srckf->X, SRCKF_STATE_DIM, 1, srckf->X_f32);
arm_mat_zero_f32(&srckf->X);
//measurement vector
arm_mat_init_f32(&srckf->Y, SRCKF_MEASUREMENT_DIM, 1, srckf->Y_f32);
arm_mat_zero_f32(&srckf->Y);
arm_mat_init_f32(&srckf->tmpY, SRCKF_MEASUREMENT_DIM, 1, srckf->tmpY_f32);
arm_mat_zero_f32(&srckf->tmpY);
}
