dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #171 from PX4/fault_detection 

Attitude / position estimation and controller improvements
This commit is contained in:
px4dev 2013-01-19 13:17:28 -08:00
parent 7eb7836d2d effc3001f4
commit c1a1207b9a
7 changed files with 375 additions and 252 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
apps/commander/commander.c
View File

@ -1850,15 +1850,16 @@ int commander_thread_main(int argc, char *argv[])
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
/* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
/* enable guided mode */
update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
// /* check auto mode switch for correct mode */
// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
// /* enable guided mode */
// update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
// XXX hardcode to auto for now
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
}
// }
} else {
/* center stick position, set SAS for all vehicle types */

44
apps/examples/control_demo/params.c
View File

@ -14,50 +14,50 @@ PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
// stabilization mode
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
// psi -> phi -> p
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg
// velocity -> theta
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
// theta -> q
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
// h -> thr
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
// crosstrack
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain
// speed command
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)

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

@ -45,9 +45,9 @@
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float RSq = 4.0680631591e+13; // earth radius squared
static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
@ -55,6 +55,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
F(9, 9),
G(9, 6),
P(9, 9),
P0(9, 9),
V(6, 6),
// attitude measurement ekf matrices
HAtt(6, 9),
@ -66,7 +67,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
C_nb(),
q(),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
@ -74,17 +75,16 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_fastTimeStamp(hrt_absolute_time()),
_slowTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_missFast(0),
_missSlow(0),
// state
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
@ -95,42 +95,32 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL")
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
// initial state covariance matrix
P = Matrix::identity(9) * 1.0f;
// wait for gps lock
while (1) {
struct pollfd fds[1];
fds[0].fd = _gps.getHandle();
fds[0].events = POLLIN;
// poll 10 seconds for new data
int ret = poll(fds, 1, 10000);
if (ret > 0) {
updateSubscriptions();
if (_gps.fix_type > 2) break;
} else if (ret == 0) {
printf("[kalman_demo] waiting for gps lock\n");
}
}
P0 = Matrix::identity(9) * 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
@ -141,8 +131,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0f;
HPos(3, 7) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
// initialize all parameters
@ -153,18 +143,13 @@ void KalmanNav::update()
{
using namespace math;
struct pollfd fds[3];
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
fds[1].fd = _param_update.getHandle();
fds[1].events = POLLIN;
fds[2].fd = _gps.getHandle();
fds[2].events = POLLIN;
// poll 20 milliseconds for new data
int ret = poll(fds, 2, 20);
// poll for new data
int ret = poll(fds, 1, 1000);
// check return value
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
@ -186,39 +171,67 @@ void KalmanNav::update()
// this clears update flag
updateSubscriptions();
// abort update if no new data
if (!(sensorsUpdate || gpsUpdate)) return;
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate &&
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
// fast prediciton step
// note, using sensors timestamp so we can account
// for packet lag
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
_fastTimeStamp = _sensors.timestamp;
if (dtFast < 1.0f / 50) {
predictFast(dtFast);
// count fast frames
_navFrames += 1;
} else _missFast++;
// slow prediction step
float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
if (dtSlow > 1.0f / 100) { // 100 Hz
_slowTimeStamp = _sensors.timestamp;
if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
else _missSlow ++;
if (_attitudeInitCounter > 100) {
printf("[kalman_demo] initialized EKF attitude\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2 &&
_gps.counter_pos_valid > 10) {
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
_positionInitialized = true;
printf("[kalman_demo] initialized EKF state with GPS\n");
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);
}
// prediciton step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (gpsUpdate) {
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
@ -226,16 +239,17 @@ void KalmanNav::update()
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _miss / 10);
_navFrames = 0;
_missFast = 0;
_missSlow = 0;
_miss = 0;
}
}
@ -278,73 +292,87 @@ void KalmanNav::updatePublications()
_att.q_valid = true;
_att.counter = _navFrames;
// update publications
SuperBlock::updatePublications();
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized)
_pos.update();
if (_attitudeInitialized)
_att.update();
}
void KalmanNav::predictFast(float dt)
int KalmanNav::predictState(float dt)
{
using namespace math;
Vector3 w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
}
// C_nb update
C_nb = Dcm(q);
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// position update
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + g;
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// attitude prediction
if (_attitudeInitialized) {
Vector3 w(_sensors.gyro_rad_s);
// rectangular integration
//printf("dt: %8.4f\n", double(dt));
//printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
//printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
}
// C_nb update
C_nb = Dcm(q);
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
void KalmanNav::predictSlow(float dt)
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
@ -356,6 +384,7 @@ void KalmanNav::predictSlow(float dt)
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
@ -436,9 +465,11 @@ void KalmanNav::predictSlow(float dt)
// for discrte time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
return ret_ok;
}
void KalmanNav::correctAtt()
int KalmanNav::correctAtt()
{
using namespace math;
@ -452,12 +483,14 @@ void KalmanNav::correctAtt()
// mag measurement
Vector3 zMag(_sensors.magnetometer_ga);
//float magNorm = zMag.norm();
zMag = zMag.unit();
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level
static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
float bN = cosf(dip) * cosf(dec);
float bE = cosf(dip) * sinf(dec);
float bD = sinf(dip);
@ -472,7 +505,7 @@ void KalmanNav::correctAtt()
// ignore accel correction when accel mag not close to g
Matrix RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - g) > 1.1f;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(3, 3) = 1.0e10;
@ -483,12 +516,8 @@ void KalmanNav::correctAtt()
//printf("correcting attitude with accel\n");
}
// account for banked turn
// this would only work for fixed wing, so try to avoid
//Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi);
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit();
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
// combined measurement
Vector zAtt(6);
@ -553,9 +582,9 @@ 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
P = Matrix::identity(9) * 1.0f;
return;
// reset P matrix to P0
P = P0;
return ret_error;
}
}
@ -568,9 +597,11 @@ void KalmanNav::correctAtt()
psi += xCorrect(PSI);
// attitude also affects nav velocities
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@ -579,38 +610,33 @@ void KalmanNav::correctAtt()
// fault detection
float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) {
if (beta > _faultAtt.get()) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
//printf("y:\n"); y.print();
printf("y:\n"); y.print();
printf("zMagHat:\n"); zMagHat.print();
printf("zMag:\n"); zMag.print();
printf("bNav:\n"); bNav.print();
}
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
return ret_ok;
}
void KalmanNav::correctPos()
int KalmanNav::correctPos()
{
using namespace math;
Vector y(6);
// residual
Vector y(5);
y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE;
y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
y(4) = double(_gps.alt) / 1.0e3 - alt;
// update gps noise
float cosLSing = cosf(lat);
float R = R0 + float(alt);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
float noiseLat = _rGpsPos.get() / R;
float noiseLon = _rGpsPos.get() / (R * cosLSing);
RPos(2, 2) = noiseLat * noiseLat;
RPos(3, 3) = noiseLon * noiseLon;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
@ -631,9 +657,9 @@ void KalmanNav::correctPos()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to identity
P = Matrix::identity(9) * 1.0f;
return;
// reset P matrix to P0
P = P0;
return ret_error;
}
}
@ -652,10 +678,17 @@ void KalmanNav::correctPos()
// fault detetcion
float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) {
if (beta > _faultPos.get()) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
//printf("y:\n"); y.print();
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))));
}
return ret_ok;
}
void KalmanNav::updateParams()
@ -664,7 +697,6 @@ void KalmanNav::updateParams()
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
@ -676,31 +708,54 @@ void KalmanNav::updateParams()
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
RAtt(1, 1) = noiseMagSq;
RAtt(2, 2) = noiseMagSq;
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(3, 3) = noiseAccelSq; // normalized direction
RAtt(4, 4) = noiseAccelSq;
RAtt(5, 5) = noiseAccelSq;
// gps noise
float cosLSing = cosf(lat);
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLat = _rGpsPos.get() / R;
float noiseLon = _rGpsPos.get() / (R * cosLSing);
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseAlt = _rGpsAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseAlt < noiseMin) noiseAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLat * noiseLat; // lat
RPos(3, 3) = noiseLon * noiseLon; // lon
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseAlt * noiseAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}

134
apps/examples/kalman_demo/KalmanNav.hpp
View File

@ -68,52 +68,108 @@
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
void predictFast(float dt);
void predictSlow(float dt);
void correctAtt();
void correctPos();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
math::Matrix F;
math::Matrix G;
math::Matrix P;
math::Matrix V;
math::Matrix HAtt;
math::Matrix RAtt;
math::Matrix HPos;
math::Matrix RPos;
math::Dcm C_nb;
math::Quaternion q;
control::UOrbSubscription<sensor_combined_s> _sensors;
control::UOrbSubscription<vehicle_gps_position_s> _gps;
control::UOrbSubscription<parameter_update_s> _param_update;
control::UOrbPublication<vehicle_global_position_s> _pos;
control::UOrbPublication<vehicle_attitude_s> _att;
uint64_t _pubTimeStamp;
uint64_t _fastTimeStamp;
uint64_t _slowTimeStamp;
uint64_t _attTimeStamp;
uint64_t _outTimeStamp;
uint16_t _navFrames;
uint16_t _missFast;
uint16_t _missSlow;
float fN, fE, fD;
// kalman filter
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix G; /**< noise shaping matrix for gyro/accel */
math::Matrix P; /**< state covariance matrix */
math::Matrix P0; /**< initial state covariance matrix */
math::Matrix V; /**< gyro/ accel noise matrix */
math::Matrix HAtt; /**< attitude measurement matrix */
math::Matrix RAtt; /**< attitude measurement noise matrix */
math::Matrix HPos; /**< position measurement jacobian matrix */
math::Matrix RPos; /**< position measurement noise matrix */
// attitude
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
float phi, theta, psi;
float vN, vE, vD;
double lat, lon, alt;
control::BlockParam<float> _vGyro;
control::BlockParam<float> _vAccel;
control::BlockParam<float> _rMag;
control::BlockParam<float> _rGpsVel;
control::BlockParam<float> _rGpsPos;
control::BlockParam<float> _rGpsAlt;
control::BlockParam<float> _rAccel;
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon, alt; /**< lat, lon, alt, radians */
// parameters
control::BlockParam<float> _vGyro; /**< gyro process noise */
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParam<float> _g; /**< gravitational constant */
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }

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

@ -1,10 +1,16 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
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);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);

19
apps/mavlink/mavlink_receiver.c
View File

@ -318,9 +318,11 @@ handle_message(mavlink_message_t *msg)
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = imu.time_usec;
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.timestamp = timestamp;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro;
hil_sensors.gyro_raw[1] = imu.ygyro;
@ -367,8 +369,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 1000000) {
printf("receiving hil imu at %d hz\n", hil_frames);
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil imu at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
@ -412,8 +414,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 1000000) {
printf("receiving hil gps at %d hz\n", hil_frames);
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil gps at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}
@ -429,6 +431,9 @@ handle_message(mavlink_message_t *msg)
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = press.time_usec;
/* baro */
/* TODO, set ground_press/ temp during calib */
static const float ground_press = 1013.25f; // mbar
@ -454,8 +459,8 @@ handle_message(mavlink_message_t *msg)
hil_frames += 1 ;
// output
if ((timestamp - old_timestamp) > 1000000) {
printf("receiving hil pressure at %d hz\n", hil_frames);
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil pressure at %d hz\n", hil_frames/10);
old_timestamp = timestamp;
hil_frames = 0;
}

2
apps/mavlink/orb_listener.c
View File

@ -718,7 +718,7 @@ uorb_receive_start(void)
/* --- GLOBAL POS VALUE --- */
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */
orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */
/* --- LOCAL POS VALUE --- */
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));