int main() { // open data files pImuFile = fopen ("IMU.txt","r"); pMagFile = fopen ("MAG.txt","r"); pGpsFile = fopen ("GPS.txt","r"); pAhrsFile = fopen ("ATT.txt","r"); pAdsFile = fopen ("NTUN.txt","r"); pTimeFile = fopen ("timing.txt","r"); pStateOutFile = fopen ("StateDataOut.txt","w"); pEulOutFile = fopen ("EulDataOut.txt","w"); pCovOutFile = fopen ("CovDataOut.txt","w"); pRefPosVelOutFile = fopen ("RefVelPosDataOut.txt","w"); pVelPosFuseFile = fopen ("VelPosFuse.txt","w"); pMagFuseFile = fopen ("MagFuse.txt","w"); pTasFuseFile = fopen ("TasFuse.txt","w"); pGpsRawINFile = fopen ("GPSraw.txt","r"); pGpsRawOUTFile = fopen ("GPSrawOut.txt","w"); memset(gpsRaw, 0, sizeof(gpsRaw)); // read test data from files for first timestamp readIMUData(); readGpsData(); readMagData(); readAirSpdData(); readAhrsData(); readTimingData(); while (!endOfData) { if ((IMUmsec >= msecStartTime) && (IMUmsec <= msecEndTime)) { // Initialise states, covariance and other data if ((IMUmsec > msecAlignTime) && !statesInitialised && (GPSstatus == 3)) { InitialiseFilter(); } // If valid IMU data and states initialised, predict states and covariances if (statesInitialised) { // Run the strapdown INS equations every IMU update UpdateStrapdownEquationsNED(); // debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4]; for (uint8_t j=0; j<=3; j++) tempQuat[j] = states[j]; quat2eul(eulerEst, tempQuat); for (uint8_t j=0; j<=2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; if (eulerDif[2] > pi) eulerDif[2] -= 2*pi; if (eulerDif[2] < -pi) eulerDif[2] += 2*pi; // store the predicted states for subsequent use by measurement fusion StoreStates(); // Check if on ground - status is used by covariance prediction OnGroundCheck(); // sum delta angles and time used by covariance prediction summedDelAng = summedDelAng + correctedDelAng; summedDelVel = summedDelVel + correctedDelVel; dt += dtIMU; // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { CovariancePrediction(); summedDelAng = summedDelAng.zero(); summedDelVel = summedDelVel.zero(); dt = 0.0f; } } // Fuse GPS Measurements if (newDataGps && statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); posNE[0] = posNED[0]; posNE[1] = posNED[1]; hgtMea = -posNED[2]; // set fusion flags fuseVelData = true; fusePosData = true; fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); // run the fusion step FuseVelposNED(); } else { fuseVelData = false; fusePosData = false; fuseHgtData = false; } // Fuse Magnetometer Measurements if (newDataMag && statesInitialised) { fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data } else { fuseMagData = false; } if (statesInitialised) FuseMagnetometer(); // Fuse Airspeed Measurements if (newAdsData && statesInitialised && VtasMeas > 8.0f) { fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); } else { fuseVtasData = false; } // debug output //printf("Euler Angle Difference = %3.1f , %3.1f , %3.1f deg\n", rad2deg*eulerDif[0],rad2deg*eulerDif[1],rad2deg*eulerDif[2]); WriteFilterOutput(); } // read test data from files for next timestamp readIMUData(); readGpsData(); readMagData(); readAirSpdData(); readAhrsData(); if (IMUmsec > msecEndTime) { CloseFiles(); endOfData = true; } } }
// Main get_nav filter function void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){ double tnow, imu_dt; double dq[4], quat_new[4]; // compute time-elapsed 'dt' // This compute the navigation state at the DAQ's Time Stamp tnow = sensorData_ptr->imuData_ptr->time; imu_dt = tnow - tprev; tprev = tnow; // ================== Time Update =================== // Temporary storage in Matrix form quat[0] = navData_ptr->quat[0]; quat[1] = navData_ptr->quat[1]; quat[2] = navData_ptr->quat[2]; quat[3] = navData_ptr->quat[3]; a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve; a_temp31[2][0] = navData_ptr->vd; b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon; b_temp31[2][0] = navData_ptr->alt; // AHRS Transformations C_N2B = quat2dcm(quat, C_N2B); C_B2N = mat_tran(C_N2B, C_B2N); // Attitude Update // ... Calculate Navigation Rate nr = navrate(a_temp31,b_temp31,nr); dq[0] = 1; dq[1] = 0.5*om_ib[0][0]*imu_dt; dq[2] = 0.5*om_ib[1][0]*imu_dt; dq[3] = 0.5*om_ib[2][0]*imu_dt; qmult(quat,dq,quat_new); quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); if(quat[0] < 0) { // Avoid quaternion flips sign quat[0] = -quat[0]; quat[1] = -quat[1]; quat[2] = -quat[2]; quat[3] = -quat[3]; } navData_ptr->quat[0] = quat[0]; navData_ptr->quat[1] = quat[1]; navData_ptr->quat[2] = quat[2]; navData_ptr->quat[3] = quat[3]; quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi)); // Velocity Update dx = mat_mul(C_B2N,f_b,dx); dx = mat_add(dx,grav,dx); navData_ptr->vn += imu_dt*dx[0][0]; navData_ptr->ve += imu_dt*dx[1][0]; navData_ptr->vd += imu_dt*dx[2][0]; // Position Update dx = llarate(a_temp31,b_temp31,dx); navData_ptr->lat += imu_dt*dx[0][0]; navData_ptr->lon += imu_dt*dx[1][0]; navData_ptr->alt += imu_dt*dx[2][0]; // JACOBIAN F = mat_fill(F, ZERO_MATRIX); // ... pos2gs F[0][3] = 1.0; F[1][4] = 1.0; F[2][5] = 1.0; // ... gs2pos F[5][2] = -2*g/EARTH_RADIUS; // ... gs2att temp33 = sk(f_b,temp33); atemp33 = mat_mul(C_B2N,temp33,atemp33); F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2]; F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2]; F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2]; // ... gs2acc F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2]; F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2]; F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2]; // ... att2att temp33 = sk(om_ib,temp33); F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2]; F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2]; F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2]; // ... att2gyr F[6][12] = -0.5; F[7][13] = -0.5; F[8][14] = -0.5; // ... Accel Markov Bias F[9][9] = -1.0/TAU_A; F[10][10] = -1.0/TAU_A; F[11][11] = -1.0/TAU_A; F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G; F[14][14] = -1.0/TAU_G; //fprintf(stderr,"Jacobian Created\n"); // State Transition Matrix: PHI = I15 + F*dt; temp1515 = mat_scalMul(F,imu_dt,temp1515); PHI = mat_add(I15,temp1515,PHI); // Process Noise G = mat_fill(G, ZERO_MATRIX); G[3][0] = -C_B2N[0][0]; G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2]; G[4][0] = -C_B2N[1][0]; G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2]; G[5][0] = -C_B2N[2][0]; G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2]; G[6][3] = -0.5; G[7][4] = -0.5; G[8][5] = -0.5; G[9][6] = 1.0; G[10][7] = 1.0; G[11][8] = 1.0; G[12][9] = 1.0; G[13][10] = 1.0; G[14][11] = 1.0; //fprintf(stderr,"Process Noise Matrix G is created\n"); // Discrete Process Noise temp1512 = mat_mul(G,Rw,temp1512); temp1515 = mat_transmul(temp1512,G,temp1515); // Qw = G*Rw*G' Qw = mat_scalMul(temp1515,imu_dt,Qw); // Qw = dt*G*Rw*G' Q = mat_mul(PHI,Qw,Q); // Q = (I+F*dt)*Qw temp1515 = mat_tran(Q,temp1515); Q = mat_add(Q,temp1515,Q); Q = mat_scalMul(Q,0.5,Q); // Q = 0.5*(Q+Q') //fprintf(stderr,"Discrete Process Noise is created\n"); // Covariance Time Update temp1515 = mat_mul(PHI,P,temp1515); P = mat_transmul(temp1515,PHI,P); // P = PHI*P*PHI' P = mat_add(P,Q,P); // P = PHI*P*PHI' + Q temp1515 = mat_tran(P, temp1515); P = mat_add(P,temp1515,P); P = mat_scalMul(P,0.5,P); // P = 0.5*(P+P') //fprintf(stderr,"Covariance Updated through Time Update\n"); navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2]; navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5]; navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8]; navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11]; navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14]; navData_ptr->err_type = TU_only; //fprintf(stderr,"Time Update Done\n"); // ================== DONE TU =================== if(sensorData_ptr->gpsData_ptr->newData){ // ================== GPS Update =================== sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag // Position, converted to NED a_temp31[0][0] = navData_ptr->lat; a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt; pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef); a_temp31[2][0] = 0.0; //pos_ref = lla2ecef(a_temp31,pos_ref); pos_ref = mat_copy(a_temp31,pos_ref); pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref); pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R; pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R; pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt; pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef); pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref); // Create Measurement: y y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0]; y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0]; y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0]; y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn; y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve; y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd; //fprintf(stderr,"Measurement Matrix, y, created\n"); // Kalman Gain temp615 = mat_mul(H,P,temp615); temp66 = mat_transmul(temp615,H,temp66); atemp66 = mat_add(temp66,R,atemp66); temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R) //fprintf(stderr,"inv(H*P*H'+R) Computed\n"); temp156 = mat_transmul(P,H,temp156); // P*H' //fprintf(stderr,"P*H' Computed\n"); K = mat_mul(temp156,temp66,K); // K = P*H'*inv(H*P*H'+R) //fprintf(stderr,"Kalman Gain Computed\n"); // Covariance Update temp1515 = mat_mul(K,H,temp1515); ImKH = mat_sub(I15,temp1515,ImKH); // ImKH = I - K*H temp615 = mat_transmul(R,K,temp615); KRKt = mat_mul(K,temp615,KRKt); // KRKt = K*R*K' temp1515 = mat_transmul(P,ImKH,temp1515); P = mat_mul(ImKH,temp1515,P); // ImKH*P*ImKH' temp1515 = mat_add(P,KRKt,temp1515); P = mat_copy(temp1515,P); // P = ImKH*P*ImKH' + KRKt //fprintf(stderr,"Covariance Updated through GPS Update\n"); navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2]; navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5]; navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8]; navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11]; navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14]; // State Update x = mat_mul(K,y,x); denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat))); denom = sqrt(denom*denom); Re = EARTH_RADIUS / sqrt(denom); Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom); navData_ptr->alt = navData_ptr->alt - x[2][0]; navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt); navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat); navData_ptr->vn = navData_ptr->vn + x[3][0]; navData_ptr->ve = navData_ptr->ve + x[4][0]; navData_ptr->vd = navData_ptr->vd + x[5][0]; quat[0] = navData_ptr->quat[0]; quat[1] = navData_ptr->quat[1]; quat[2] = navData_ptr->quat[2]; quat[3] = navData_ptr->quat[3]; // Attitude correction dq[0] = 1.0; dq[1] = x[6][0]; dq[2] = x[7][0]; dq[3] = x[8][0]; qmult(quat,dq,quat_new); quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]); navData_ptr->quat[0] = quat[0]; navData_ptr->quat[1] = quat[1]; navData_ptr->quat[2] = quat[2]; navData_ptr->quat[3] = quat[3]; quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi)); navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0]; navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0]; navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0]; navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0]; navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0]; navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0]; navData_ptr->err_type = gps_aided; //fprintf(stderr,"Measurement Update Done\n"); } // Remove current estimated biases from rate gyro and accels sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0]; sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1]; sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2]; sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0]; sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1]; sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2]; // Get the new Specific forces and Rotation Rate, // use in the next time update f_b[0][0] = sensorData_ptr->imuData_ptr->ax; f_b[1][0] = sensorData_ptr->imuData_ptr->ay; f_b[2][0] = sensorData_ptr->imuData_ptr->az; om_ib[0][0] = sensorData_ptr->imuData_ptr->p; om_ib[1][0] = sensorData_ptr->imuData_ptr->q; om_ib[2][0] = sensorData_ptr->imuData_ptr->r; }
void FixedwingEstimator::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); float dt = 0.0f; // time lapsed since last covariance prediction _filter_start_time = hrt_absolute_time(); if (!_ekf) { errx(1, "OUT OF MEM!"); } /* * do subscriptions */ _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); #ifndef SENSOR_COMBINED_SUB _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_gyro_sub, 4); #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ parameters_update(); Vector3f lastAngRate; Vector3f lastAccel; /* wakeup source(s) */ struct pollfd fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; #ifndef SENSOR_COMBINED_SUB fds[1].fd = _gyro_sub; fds[1].events = POLLIN; #else fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; #endif bool newDataGps = false; bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) uint64_t last_gps = 0; _gps.vel_n_m_s = 0.0f; _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("POLL ERR %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); /* update parameters from storage */ parameters_update(); } /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; bool mag_updated; perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); #else /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); #endif /* set sensors to de-initialized state */ _gyro_valid = false; _accel_valid = false; _mag_valid = false; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } /** * PART ONE: COLLECT ALL DATA **/ /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_check(_accel_sub, &accel_updated); if (accel_updated) { orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } _last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - _last_run) / 1e6f; _last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; if (isfinite(_gyro.x) && isfinite(_gyro.y) && isfinite(_gyro.z)) { _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; if (!_gyro_valid) { lastAngRate = _ekf->angRate; } _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; if (!_accel_valid) { lastAccel = _ekf->accel; } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; _ekf->lastAccel = accel; #else orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; static hrt_abstime last_mag = 0; if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; } else { accel_updated = false; } last_accel = _sensor_combined.accelerometer_timestamp; // Copy gyro and accel _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; if (isfinite(_sensor_combined.gyro_rad_s[0]) && isfinite(_sensor_combined.gyro_rad_s[1]) && isfinite(_sensor_combined.gyro_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; if (!_gyro_valid) { lastAngRate = _ekf->angRate; } _gyro_valid = true; perf_count(_perf_gyro); } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; if (!_accel_valid) { lastAccel = _ekf->accel; } _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; lastAccel = _ekf->accel; if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; newDataMag = true; } else { newDataMag = false; } last_mag = _sensor_combined.magnetometer_timestamp; #endif //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); _ekf->VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; } else { newAdsData = false; } bool gps_updated; orb_check(_gps_sub, &gps_updated); if (gps_updated) { last_gps = _gps.timestamp_position; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); if (_gps.fix_type < 3) { newDataGps = false; } else { /* store time of valid GPS measurement */ _gps_start_time = hrt_absolute_time(); /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } /* fuse GPS updates */ //_gps.timestamp / 1e3; _ekf->GPSstatus = _gps.fix_type; _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { // _ekf->posNeSigma = _parameters.posne_noise; // } // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; } } bool baro_updated; orb_check(_baro_sub, &baro_updated); if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _ekf->baroHgt = _baro.altitude; if (!_baro_init) { _baro_ref = _baro.altitude; _baro_init = true; warnx("ALT REF INIT"); } perf_count(_perf_baro); newHgtData = true; } else { newHgtData = false; } #ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); #endif if (mag_updated) { _mag_valid = true; perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. // 0.001f _ekf->magData.x = _mag.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = _mag.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = _mag.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #endif newDataMag = true; } else { newDataMag = false; } /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; } /** * PART TWO: EXECUTE THE FILTER * * We run the filter only once all data has been fetched **/ if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; initVelNED[0] = _gps.vel_n_m_s; initVelNED[1] = _gps.vel_e_m_s; initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; _ekf->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; _ekf->gpsHgt = gps_alt; // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; _local_pos.ref_lon = lon; _local_pos.ref_alt = gps_alt; _local_pos.ref_timestamp = _gps.timestamp_position; map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif _gps_initialized = true; } else if (!_ekf->statesInitialised) { initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } else if (_ekf->statesInitialised) { // We're apparently initialized in this case now int check = check_filter_state(); if (check) { // Let the system re-initialize itself continue; } // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); #if 0 // debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4]; for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; quat2eul(eulerEst, tempQuat); for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; #endif // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction _ekf->OnGroundCheck(); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; dt += _ekf->dtIMU; // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); _ekf->summedDelAng.zero(); _ekf->summedDelVel.zero(); dt = 0.0f; } // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; // Calculate acceleration predicted by GPS velocity change if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; } _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else if (_ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED _ekf->velNED[0] = 0.0f; _ekf->velNED[1] = 0.0f; _ekf->velNED[2] = 0.0f; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else { _ekf->fuseVelData = false; _ekf->fusePosData = false; } if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); } else { _ekf->fuseHgtData = false; } // Fuse Magnetometer Measurements if (newDataMag && _ekf->statesInitialised) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); } else { _ekf->fuseMagData = false; } // Fuse Airspeed Measurements if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { _ekf->fuseVtasData = false; } // Output results math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); _att.timestamp = _last_sensor_timestamp; _att.q[0] = _ekf->states[0]; _att.q[1] = _ekf->states[1]; _att.q[2] = _ekf->states[2]; _att.q[3] = _ekf->states[3]; _att.q_valid = true; _att.R_valid = true; _att.timestamp = _last_sensor_timestamp; _att.roll = euler(0); _att.pitch = euler(1); _att.yaw = euler(2); _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; // gyro offsets _att.rate_offsets[0] = _ekf->states[10]; _att.rate_offsets[1] = _ekf->states[11]; _att.rate_offsets[2] = _ekf->states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { /* advertise and publish */ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } if (_gps_initialized) { _local_pos.timestamp = _last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly _local_pos.z = _ekf->states[9] - _baro_ref_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; _local_pos.vz = _ekf->states[6]; _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; _local_pos.v_xy_valid = _gps_initialized; _local_pos.v_z_valid = true; _local_pos.xy_global = true; _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; /* crude land detector for fixedwing only, * TODO: adapt so that it works for both, maybe move to another location */ if (_velocity_xy_filtered < 5 && _velocity_z_filtered < 10 && _airspeed_filtered < 10) { _local_pos.landed = true; } else { _local_pos.landed = false; } _local_pos.z_global = false; _local_pos.yaw = _att.yaw; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); } else { /* advertise and publish */ _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } _global_pos.timestamp = _local_pos.timestamp; if (_local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; } else { _global_pos.vel_n = 0.0f; _global_pos.vel_e = 0.0f; } /* local pos alt is negative, change sign and add alt offsets */ _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; } _global_pos.yaw = _local_pos.yaw; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; _global_pos.timestamp = _local_pos.timestamp; /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); } else { /* advertise and publish */ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = 0.0f; // XXX get form filter _wind.covariance_east = 0.0f; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { /* publish the wind estimate */ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } } } } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; _wind.covariance_north = _ekf->P[14][14]; _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { /* publish the wind estimate */ orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); } else { /* advertise and publish */ _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); } } } } perf_end(_loop_perf); } warnx("exiting.\n"); _estimator_task = -1; _exit(0); }