Hi Ron,
Thanks for your reply ! , "we" --- there are some people who take interest in AQ just like me .
In nav_ukf.c file .....
******************************************************************
void navUkfGpsPosUpate(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 (/*(supervisorData.state & STATE_FLYING) &&*/ hAcc < 20.0f && gpsData.tDOP != 0.0f) {
if (navUkfData.holdLat == 0.0) {
navUkfData.holdLat = lat;
navUkfData.holdLon = lon;
navUkfCalcEarthRadius(lat);
navUkfSetGlobalPositionTarget(lat, lon);
navUkfResetPosition(-UKF_POSN, -UKF_POSE, alt - UKF_POSD);
}
else {
navUkfCalcDistance(lat, lon, &y[0], &y[1]);
y[2] = alt;
...............
// 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] = p[UKF_GPS_POS_N] + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * p[UKF_GPS_POS_M_N];
noise[1] = p[UKF_GPS_POS_N] + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * p[UKF_GPS_POS_M_N];
noise[2] = p[UKF_GPS_ALT_N] + vAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * p[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];
..............................
******************************************************************
My question is... the UKF use for what ? .. are the position data(posN,E,D; velN,E,D) just come from GPS and smoothed by UKF filter OR it combines rate-gyro and accelerometer data (Inertial data) to estimate the position by UKF filter ?
Best Regards
Stone.