// parameters, outputs, output noise, map func
srcdkf_t *paramsrcdkfInit(int w, int d, int n, SRCDKFMeasurementUpdate_t *map) {
srcdkf_t *f;
f = srcdkfInit(w, d, w, n, (void(*)(float32_t*, float32_t*, float32_t*, float32_t*, float32_t))0);
f->M = d;
f->N = n;
f->map = map;
f->rm = SRCDKF_RM;
matrixInit(&f->KT, d, w);
matrixInit(&f->inovT, 1, d);
matrixInit(&f->rDiag, w, 1);
return f;
}
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
}
