void paramsrcdkfSetVariance(srcdkf_t *f, float32_t *v, float32_t *n) { float32_t *rDiag = f->rDiag.pData; int i; srcdkfSetVariance(f, v, v, n, f->N); for (i = 0; i < f->S; i++) rDiag[i] = 0.0; }
void navUkfInit(void) { float Q[SIM_S]; // state variance float V[SIM_V]; // process variance float mag[3]; memset((void *)&navUkfData, 0, sizeof(navUkfData)); navUkfData.v0a[0] = 0.0f; navUkfData.v0a[1] = 0.0f; navUkfData.v0a[2] = -1.0f; // calculate mag vector based on inclination mag[0] = cosf(p[IMU_MAG_INCL] * DEG_TO_RAD); mag[1] = 0.0f; mag[2] = -sinf(p[IMU_MAG_INCL] * DEG_TO_RAD); // rotate local mag vector to align with true north navUkfData.v0m[0] = mag[0] * cosf(p[IMU_MAG_DECL] * DEG_TO_RAD) - mag[1] * sinf(p[IMU_MAG_DECL] * DEG_TO_RAD); navUkfData.v0m[1] = mag[1] * cosf(p[IMU_MAG_DECL] * DEG_TO_RAD) + mag[0] * sinf(p[IMU_MAG_DECL] * DEG_TO_RAD); navUkfData.v0m[2] = mag[2]; navUkfData.kf = srcdkfInit(SIM_S, SIM_M, SIM_V, SIM_N, navUkfTimeUpdate); navUkfData.x = srcdkfGetState(navUkfData.kf); Q[0] = UKF_VEL_Q; Q[1] = UKF_VEL_Q; Q[2] = UKF_VEL_ALT_Q; Q[3] = UKF_POS_Q; Q[4] = UKF_POS_Q; Q[5] = UKF_POS_ALT_Q; Q[6] = UKF_ACC_BIAS_Q; Q[7] = UKF_ACC_BIAS_Q; Q[8] = UKF_ACC_BIAS_Q; Q[9] = UKF_GYO_BIAS_Q; Q[10] = UKF_GYO_BIAS_Q; Q[11] = UKF_GYO_BIAS_Q; Q[12] = UKF_QUAT_Q; Q[13] = UKF_QUAT_Q; Q[14] = UKF_QUAT_Q; Q[15] = UKF_QUAT_Q; Q[16] = UKF_PRES_ALT_Q; V[UKF_V_NOISE_ACC_BIAS_X] = UKF_ACC_BIAS_V; V[UKF_V_NOISE_ACC_BIAS_Y] = UKF_ACC_BIAS_V; V[UKF_V_NOISE_ACC_BIAS_Z] = UKF_ACC_BIAS_V; V[UKF_V_NOISE_GYO_BIAS_X] = UKF_GYO_BIAS_V; V[UKF_V_NOISE_GYO_BIAS_Y] = UKF_GYO_BIAS_V; V[UKF_V_NOISE_GYO_BIAS_Z] = UKF_GYO_BIAS_V; V[UKF_V_NOISE_RATE_X] = UKF_RATE_V; V[UKF_V_NOISE_RATE_Y] = UKF_RATE_V; V[UKF_V_NOISE_RATE_Z] = UKF_RATE_V; V[UKF_V_NOISE_VELN] = UKF_VEL_V; V[UKF_V_NOISE_VELE] = UKF_VEL_V; V[UKF_V_NOISE_VELD] = UKF_ALT_VEL_V; srcdkfSetVariance(navUkfData.kf, Q, V, 0, 0); navUkfInitState(); navUkfData.flowRotCos = cosf(UKF_FLOW_ROT * DEG_TO_RAD); navUkfData.flowRotSin = sinf(UKF_FLOW_ROT * DEG_TO_RAD); #ifdef UKF_LOG_FNAME navUkfData.logHandle = filerGetHandle(UKF_LOG_FNAME); filerStream(navUkfData.logHandle, ukfLog, UKF_LOG_BUF_SIZE); #endif }