void simDoAccUpdate(float accX, float accY, float accZ) { float noise[3]; // measurement variance float y[3]; // measurement(s) float norm; // remove bias accX += UKF_ACC_BIAS_X; accY += UKF_ACC_BIAS_Y; accZ += UKF_ACC_BIAS_Z; // normalize vector norm = __sqrtf(accX*accX + accY*accY + accZ*accZ); y[0] = accX / norm; y[1] = accY / norm; y[2] = accZ / norm; noise[0] = UKF_ACC_N + fabsf(GRAVITY - norm) * UKF_DIST_N; if (!(supervisorData.state & STATE_FLYING)) { accX -= UKF_ACC_BIAS_X; accY -= UKF_ACC_BIAS_Y; noise[0] *= 0.001f; } noise[1] = noise[0]; noise[2] = noise[0]; srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfAccUpdate); }
void navUkfGpsVelUpdate(uint32_t gpsMicros, float velN, float velE, float velD, float sAcc) { float y[3]; float noise[3]; float velDelta[3]; int histIndex; y[0] = velN; y[1] = velE; y[2] = velD; // determine how far back this GPS velocity update came from histIndex = (timerMicros() - (gpsMicros + UKF_VEL_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP); histIndex = navUkfData.navHistIndex - histIndex; if (histIndex < 0) histIndex += UKF_HIST; if (histIndex < 0 || histIndex >= UKF_HIST) histIndex = 0; // calculate delta from current position velDelta[0] = UKF_VELN - navUkfData.velN[histIndex]; velDelta[1] = UKF_VELE - navUkfData.velE[histIndex]; velDelta[2] = UKF_VELD - navUkfData.velD[histIndex]; // set current position state to historic data UKF_VELN = navUkfData.velN[histIndex]; UKF_VELE = navUkfData.velE[histIndex]; UKF_VELD = navUkfData.velD[histIndex]; noise[0] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_VEL_M_N; noise[1] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_VEL_M_N; noise[2] = UKF_GPS_VD_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_VD_M_N; srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfVelUpdate); // add the historic position delta back to the current state UKF_VELN += velDelta[0]; UKF_VELE += velDelta[1]; UKF_VELD += velDelta[2]; #ifdef UKF_LOG_FNAME { float *log = (float *)&ukfLog[navUkfData.logPointer]; int i = 0; *(uint32_t *)&log[i++] = 0xffffffff; log[i++] = y[0]; log[i++] = y[1]; log[i++] = y[2]; log[i++] = noise[0]; log[i++] = noise[1]; log[i++] = noise[2]; log[i++] = velDelta[0]; log[i++] = velDelta[1]; log[i++] = velDelta[2]; navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE; filerSetHead(navUkfData.logHandle, navUkfData.logPointer); } #endif }
void paramsrcdkfUpdate(srcdkf_t *f, float32_t *u, float32_t *d) { int S = f->S; float32_t *Sx = f->Sx.pData; float32_t *Sv = f->Sv.pData; float32_t *rDiag = f->rDiag.pData; int i; srcdkfMeasurementUpdate(f, u, d, f->M, f->N, 0, f->map); if (f->rm) { // Robbins-Monro innovation estimation for Rr // Rr = (1 - SRCDKF_RM)*Rr + SRCDKF_RM * K * [dk - g(xk, wk)] * [dk - g(xk, wk)]^T * K^T arm_mat_trans_f32(&f->K, &f->KT); arm_mat_trans_f32(&f->inov, &f->inovT); // xUpdate == K*inov arm_mat_mult_f32(&f->xUpdate, &f->inovT, &f->K); arm_mat_mult_f32(&f->K, &f->KT, &f->Sv); for (i = 0; i < S; i++) { rDiag[i] = (1.0 - f->rm)*rDiag[i] + f->rm * Sv[i*S + i]; Sx[i*S + i] = sqrt(Sx[i*S + i] * Sx[i*S + i] + rDiag[i]); } } }
void simDoPresUpdate(float pres) { float noise[2]; // measurement variance float y[2]; // measurment(s) noise[0] = UKF_ALT_N; noise[1] = noise[0]; y[0] = navUkfPresToAlt(pres); y[1] = y[0]; // if GPS altitude data has been available, only update pressure altitude if (navUkfData.presAltOffset != 0.0f) srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 1, 1, noise, navUkfPresUpdate); // otherwise update pressure and GPS altitude from the single pressure reading else srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 2, 2, noise, navUkfPresGPSAltUpdate); }
void navUkfZeroRate(float rate, int axis) { float noise[1]; // measurement variance float y[1]; // measurment(s) float u[1]; // user data noise[0] = 0.00001f; y[0] = rate; u[0] = (float)axis; srcdkfMeasurementUpdate(navUkfData.kf, u, y, 1, 1, noise, navUkfRateUpdate); }
void simDoMagUpdate(float magX, float magY, float magZ) { float noise[3]; // measurement variance float y[3]; // measurement(s) float norm; noise[0] = UKF_MAG_N; if (!(supervisorData.state & STATE_FLYING)) noise[0] *= 0.001f; noise[1] = noise[0]; noise[2] = noise[0]; // normalize vector norm = 1.0f / __sqrtf(magX*magX + magY*magY + magZ*magZ); y[0] = magX * norm; y[1] = magY * norm; y[2] = magZ * norm; srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfMagUpdate); }
void navUkfZeroVel(void) { float y[3]; float noise[3]; y[0] = 0.0f; y[1] = 0.0f; y[2] = 0.0f; if (supervisorData.state & STATE_FLYING) { noise[0] = 5.0f; noise[1] = 5.0f; noise[2] = 2.0f; } else { noise[0] = 1e-7f; noise[1] = 1e-7f; noise[2] = 1e-7f; } srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfVelUpdate); }
void navUkfZeroPos(void) { float y[3]; float noise[3]; y[0] = 0.0f; y[1] = 0.0f; y[2] = navUkfPresToAlt(AQ_PRESSURE); if (supervisorData.state & STATE_FLYING) { noise[0] = 1e1f; noise[1] = 1e1f; noise[2] = 1e1f; } else { noise[0] = 1e-7f; noise[1] = 1e-7f; noise[2] = 1.0f; } srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfPosUpdate); }
/* We take raw pixel movement data reported by the sensor and combine it with our own estimates of ground height and rotational rates to calculate the x/y velocity estimates. Along with reported sonar altitudes, they are fed to the UKF as observations. */ void navUkfFlowUpdate(void) { static float oldPitch, oldRoll; float flowX, flowY; float xT, yT; float dt; float flowAlt = 0.0f; float y[3]; float noise[3]; // set default noise levels noise[0] = 0.001f; noise[1] = 0.001f; noise[2] = 100.0f; // valid altitudes? if (navUkfData.flowAltCount > 0) { flowAlt = navUkfData.flowSumAlt / navUkfData.flowAltCount; if (fabsf(navUkfData.flowAlt - flowAlt) < 1.0f) noise[2] = 0.0025f; navUkfData.flowAlt = flowAlt; } // first valid flow update ? if (navUkfData.flowInit == 0) { // only allow init if we have a valid altitude if (navUkfData.flowAltCount > 0) { UKFPressureAdjust(navUkfData.flowAlt); UKF_POSD = navUkfData.flowAlt; navUkfData.flowInit = 1; } } else { // scaled, average quality navUkfData.flowQuality = (float)navUkfData.flowSumQuality / navUkfData.flowCount * (1.0f / 255.0f); // first rotate sensor data to craft frame around Z axis flowX = navUkfData.flowSumX * navUkfData.flowRotCos + navUkfData.flowSumY * navUkfData.flowRotSin; flowY = navUkfData.flowSumY * navUkfData.flowRotCos - navUkfData.flowSumX * navUkfData.flowRotSin; // adjust for pitch/roll rotations during measurement flowX -= (AQ_PITCH - oldPitch) * DEG_TO_RAD * UKF_FOCAL_PX; flowY += (AQ_ROLL - oldRoll) * DEG_TO_RAD * UKF_FOCAL_PX; // next, rotate flow to world frame xT = flowX * navUkfData.yawCos - flowY * navUkfData.yawSin; yT = flowY * navUkfData.yawCos + flowX * navUkfData.yawSin; // convert to distance covered based on focal length and height above ground flowX = xT * (1.0f / UKF_FOCAL_PX) * UKF_POSD; flowY = yT * (1.0f / UKF_FOCAL_PX) * UKF_POSD; // integrate for absolute position navUkfData.flowPosN += flowX; navUkfData.flowPosE += flowY; // time delta - assume each count is from two readings @ 5ms each dt = (float)navUkfData.flowCount * (5.0f * 2.0f) / 1000.0f; // differentiate for velocity navUkfData.flowVelX = flowX / dt; navUkfData.flowVelY = flowY / dt; // TODO: properly estimate noise noise[0] = (UKF_GPS_POS_N + UKF_GPS_POS_M_N) * 0.5f; noise[1] = noise[0]; navUkfCalcLocalDistance(navUkfData.flowPosN, navUkfData.flowPosE, &y[0], &y[1]); y[2] = navUkfData.flowAlt; srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfOfPosUpdate); #ifdef UKF_LOG_FNAME { float *log = (float *)&ukfLog[navUkfData.logPointer]; int i = 0; *(uint32_t *)&log[i++] = 0xffffffff; log[i++] = flowX; log[i++] = flowY; log[i++] = navUkfData.flowVelX; log[i++] = navUkfData.flowVelY; log[i++] = navUkfData.flowPosN; log[i++] = navUkfData.flowPosE; log[i++] = flowAlt; log[i++] = UKF_POSN; log[i++] = UKF_POSE; log[i++] = UKF_POSD; log[i++] = UKF_PRES_ALT; log[i++] = UKF_VELD; log[i++] = navUkfData.flowQuality; log[i++] = (AQ_PITCH - oldPitch); log[i++] = (AQ_ROLL - oldRoll); log[i++] = dt; navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE; filerSetHead(navUkfData.logHandle, navUkfData.logPointer); } #endif } navUkfData.flowSumX = 0.0f; navUkfData.flowSumY = 0.0f; navUkfData.flowSumQuality = 0; navUkfData.flowSumAlt = 0.0f; navUkfData.flowCount = 0; navUkfData.flowAltCount = 0; oldRoll = AQ_ROLL; oldPitch = AQ_PITCH; }
void navUkfGpsPosUpdate(uint32_t gpsMicros, double lat, double lon, float alt, float hAcc, float vAcc) { float y[3]; float noise[3]; float posDelta[3]; int histIndex; if (navUkfData.holdLat == (double)0.0) { navUkfData.holdLat = lat; navUkfData.holdLon = lon; navUkfCalcEarthRadius(lat); navUkfSetGlobalPositionTarget(lat, lon); navUkfResetPosition(-UKF_POSN, -UKF_POSE, alt - UKF_POSD); } else { navUkfCalcGlobalDistance(lat, lon, &y[0], &y[1]); y[2] = alt; // determine how far back this GPS position update came from histIndex = (timerMicros() - (gpsMicros + UKF_POS_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP); histIndex = navUkfData.navHistIndex - histIndex; if (histIndex < 0) histIndex += UKF_HIST; if (histIndex < 0 || histIndex >= UKF_HIST) histIndex = 0; // calculate delta from current position posDelta[0] = UKF_POSN - navUkfData.posN[histIndex]; posDelta[1] = UKF_POSE - navUkfData.posE[histIndex]; posDelta[2] = UKF_POSD - navUkfData.posD[histIndex]; // set current position state to historic data UKF_POSN = navUkfData.posN[histIndex]; UKF_POSE = navUkfData.posE[histIndex]; UKF_POSD = navUkfData.posD[histIndex]; noise[0] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_POS_M_N; noise[1] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_POS_M_N; noise[2] = UKF_GPS_ALT_N + vAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_ALT_M_N; srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfPosUpdate); // add the historic position delta back to the current state UKF_POSN += posDelta[0]; UKF_POSE += posDelta[1]; UKF_POSD += posDelta[2]; #ifdef UKF_LOG_FNAME { float *log = (float *)&ukfLog[navUkfData.logPointer]; int i = 0; *(uint32_t *)&log[i++] = 0xffffffff; log[i++] = y[0]; log[i++] = y[1]; log[i++] = y[2]; log[i++] = noise[0]; log[i++] = noise[1]; log[i++] = noise[2]; log[i++] = posDelta[0]; log[i++] = posDelta[1]; log[i++] = posDelta[2]; navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE; filerSetHead(navUkfData.logHandle, navUkfData.logPointer); } #endif } }