fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added check for valid attitude data.

Browse Source
This commit is contained in:
James Goppert 2013-01-15 12:37:12 -05:00
parent 022c30ea4f
commit edf0a6bae7
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
apps/examples/kalman_demo/KalmanNav.cpp
View File

@ -177,7 +177,10 @@ void KalmanNav::update()
if (!(sensorsUpdate || gpsUpdate)) return;
// if received gps for first time, reset position to gps
if (!_positionInitialized && gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 5) {
if (!_positionInitialized &&
gpsUpdate &&
_gps.fix_type > 2 &&
_gps.counter_pos_valid > 10) {
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
@ -187,10 +190,10 @@ void KalmanNav::update()
_positionInitialized = true;
printf("[kalman_demo] initializing EKF state with GPS\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
double(phi), double(theta), double(psi));
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
double(vN), double(vE), double(vD),
lat, lon, alt);
}
// fast prediciton step
@ -340,9 +343,9 @@ void KalmanNav::predictFast(float dt)
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
//printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
//double(vNDot), double(vEDot), double(vDDot));
//double(vNDot), double(vEDot), double(vDDot));
//printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
//double(LDot), double(lDot), double(rotRate));
//double(LDot), double(lDot), double(rotRate));
// rectangular integration
//printf("dt: %8.4f\n", double(dt));
@ -455,6 +458,12 @@ void KalmanNav::correctAtt()
{
using namespace math;
// check for valid data, return if not ready
if (_sensors.accelerometer_counter < 10 ||
_sensors.gyro_counter < 10 ||
_sensors.magnetometer_counter < 10)
return;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
@ -566,7 +575,7 @@ void KalmanNav::correctAtt()
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in att correction\n");
// reset P matrix to identity
// reset P matrix to P0
P = P0;
return;
}
@ -649,7 +658,7 @@ void KalmanNav::correctPos()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to identity
// reset P matrix to P0
P = P0;
return;
}

4
apps/examples/kalman_demo/params.c
View File

@ -1,8 +1,8 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);