mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 01:43:33 +08:00
Rebase of changes to sensor_hil_fixedwing branch.
This commit is contained in:
James Goppert
@@ -42,10 +42,12 @@
|
||||
#include "KalmanNav.hpp"
|
||||
|
||||
// constants
|
||||
// Titterton pg. 52
|
||||
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
|
||||
static const float R = 6.371000e6f; // earth radius, m
|
||||
static const float RSq = 4.0589641e13f; // radius squared
|
||||
static const float g = 9.8f; // gravitational accel. m/s^2
|
||||
static const float R0 = 6378137.0f; // earth radius, m
|
||||
|
||||
static const float RSq = 4.0680631591e+13; // radius squared
|
||||
static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
|
||||
|
||||
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
SuperBlock(parent, name),
|
||||
@@ -57,15 +59,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
// attitude measurement ekf matrices
|
||||
HAtt(6, 9),
|
||||
RAtt(6, 6),
|
||||
// gps measurement ekf matrices
|
||||
HGps(6, 9),
|
||||
RGps(6, 6),
|
||||
// position measurement ekf matrices
|
||||
HPos(5, 9),
|
||||
RPos(5, 5),
|
||||
// attitude representations
|
||||
C_nb(),
|
||||
q(),
|
||||
// subscriptions
|
||||
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
|
||||
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
|
||||
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 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
|
||||
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
|
||||
@@ -78,6 +80,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
_outTimeStamp(hrt_absolute_time()),
|
||||
// frame count
|
||||
_navFrames(0),
|
||||
// miss counts
|
||||
_missFast(0),
|
||||
_missSlow(0),
|
||||
// state
|
||||
fN(0), fE(0), fD(0),
|
||||
phi(0), theta(0), psi(0),
|
||||
@@ -87,8 +92,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
_vGyro(this, "V_GYRO"),
|
||||
_vAccel(this, "V_ACCEL"),
|
||||
_rMag(this, "R_MAG"),
|
||||
_rGpsV(this, "R_GPS_V"),
|
||||
_rGpsGeo(this, "R_GPS_GEO"),
|
||||
_rGpsVel(this, "R_GPS_VEL"),
|
||||
_rGpsPos(this, "R_GPS_POS"),
|
||||
_rGpsAlt(this, "R_GPS_ALT"),
|
||||
_rAccel(this, "R_ACCEL")
|
||||
{
|
||||
@@ -99,12 +104,21 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
|
||||
// wait for gps lock
|
||||
while (1) {
|
||||
updateSubscriptions();
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = _gps.getHandle();
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
if (_gps.fix_type > 2) break;
|
||||
// poll 10 seconds for new data
|
||||
int ret = poll(fds, 1, 10000);
|
||||
|
||||
printf("[kalman_demo] waiting for gps lock\n");
|
||||
usleep(1000000);
|
||||
if (ret > 0) {
|
||||
updateSubscriptions();
|
||||
|
||||
if (_gps.fix_type > 2) break;
|
||||
|
||||
} else if (ret == 0) {
|
||||
printf("[kalman_demo] waiting for gps lock\n");
|
||||
}
|
||||
}
|
||||
|
||||
// initial state
|
||||
@@ -124,16 +138,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
// initialize dcm
|
||||
C_nb = Dcm(q);
|
||||
|
||||
// initialize F to identity
|
||||
F = Matrix::identity(9);
|
||||
|
||||
// HGps is constant
|
||||
HGps(0, 3) = 1.0f;
|
||||
HGps(1, 4) = 1.0f;
|
||||
HGps(2, 5) = 1.0f;
|
||||
HGps(3, 6) = 1.0f;
|
||||
HGps(4, 7) = 1.0f;
|
||||
HGps(5, 8) = 1.0f;
|
||||
// HPos is constant
|
||||
HPos(0, 3) = 1.0f;
|
||||
HPos(1, 4) = 1.0f;
|
||||
HPos(2, 6) = 1.0f;
|
||||
HPos(3, 7) = 1.0f;
|
||||
HPos(4, 8) = 1.0f;
|
||||
|
||||
// initialize all parameters
|
||||
updateParams();
|
||||
@@ -143,11 +153,13 @@ void KalmanNav::update()
|
||||
{
|
||||
using namespace math;
|
||||
|
||||
struct pollfd fds[2];
|
||||
struct pollfd fds[3];
|
||||
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);
|
||||
@@ -158,53 +170,55 @@ void KalmanNav::update()
|
||||
return;
|
||||
|
||||
} else if (ret == 0) { // timeout
|
||||
// run anyway
|
||||
} else if (ret > 0) {
|
||||
// update params when requested
|
||||
if (fds[1].revents & POLLIN) {
|
||||
printf("updating params\n");
|
||||
updateParams();
|
||||
}
|
||||
|
||||
// if no new sensor data, return
|
||||
if (!(fds[0].revents & POLLIN)) return;
|
||||
return;
|
||||
}
|
||||
|
||||
// get new timestamp
|
||||
uint64_t newTimeStamp = hrt_absolute_time();
|
||||
|
||||
// check updated subscriptions
|
||||
if (_param_update.updated()) updateParams();
|
||||
|
||||
bool gpsUpdate = _gps.updated();
|
||||
bool sensorsUpdate = _sensors.updated();
|
||||
|
||||
// get new information from subscriptions
|
||||
// this clears update flag
|
||||
updateSubscriptions();
|
||||
|
||||
// count fast frames
|
||||
_navFrames += 1;
|
||||
// abort update if no new data
|
||||
if (!(sensorsUpdate || gpsUpdate)) return;
|
||||
|
||||
// fast prediciton step
|
||||
// note, using sensors timestamp so we can account
|
||||
// for packet lag
|
||||
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
|
||||
_fastTimeStamp = _sensors.timestamp;
|
||||
predictFast(dtFast);
|
||||
|
||||
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 / 200) { // 200 Hz
|
||||
if (dtSlow > 1.0f / 100) { // 100 Hz
|
||||
_slowTimeStamp = _sensors.timestamp;
|
||||
predictSlow(dtSlow);
|
||||
|
||||
if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
|
||||
else _missSlow ++;
|
||||
}
|
||||
|
||||
// gps correction step
|
||||
if (gpsUpdate) {
|
||||
correctGps();
|
||||
correctPos();
|
||||
}
|
||||
|
||||
// attitude correction step
|
||||
if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
|
||||
if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
|
||||
_attTimeStamp = _sensors.timestamp;
|
||||
correctAtt();
|
||||
}
|
||||
@@ -218,8 +232,10 @@ void KalmanNav::update()
|
||||
// output
|
||||
if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
|
||||
_outTimeStamp = newTimeStamp;
|
||||
printf("nav: %4d Hz\n", _navFrames);
|
||||
printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
|
||||
_navFrames = 0;
|
||||
_missFast = 0;
|
||||
_missSlow = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -298,21 +314,28 @@ void KalmanNav::predictFast(float dt)
|
||||
// trig
|
||||
float sinL = sinf(lat);
|
||||
float cosL = cosf(lat);
|
||||
float cosLSing = cosf(lat);
|
||||
|
||||
if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
|
||||
|
||||
// position update
|
||||
// neglects angular deflections in local gravity
|
||||
// see Titerton pg. 70
|
||||
float LDot = vN / (R + float(alt));
|
||||
float lDot = vE / (cosL * (R + float(alt)));
|
||||
float vNDot = fN - vE * (2 * omega +
|
||||
lDot) * sinL +
|
||||
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 * (2 * omega + lDot) * cosL -
|
||||
float vDDot = fD - vE * rotRate * cosL -
|
||||
vN * LDot + g;
|
||||
float vEDot = fE + vN * (2 * omega + lDot) * sinL +
|
||||
vDDot * (2 * omega * cosL);
|
||||
float vEDot = fE + vN * rotRate * sinL +
|
||||
vDDot * rotRate * cosL;
|
||||
|
||||
// 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;
|
||||
@@ -331,87 +354,88 @@ void KalmanNav::predictSlow(float dt)
|
||||
float cosLSq = cosL * cosL;
|
||||
float tanL = tanf(lat);
|
||||
|
||||
// prepare for matrix
|
||||
float R = R0 + float(alt);
|
||||
|
||||
// F Matrix
|
||||
// Titterton pg. 291
|
||||
//
|
||||
// difference from Jacobian
|
||||
// multiplity by dt for all elements
|
||||
// add 1.0 to diagonal elements
|
||||
|
||||
F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
|
||||
F(0, 2) = (vN / R) * dt;
|
||||
F(0, 4) = (1.0f / R) * dt;
|
||||
F(0, 6) = (-omega * sinL) * dt;
|
||||
F(0, 8) = (-vE / RSq) * dt;
|
||||
F(0, 1) = -(omega * sinL + vE * tanL / R);
|
||||
F(0, 2) = vN / R;
|
||||
F(0, 4) = 1.0f / R;
|
||||
F(0, 6) = -omega * sinL;
|
||||
F(0, 8) = -vE / RSq;
|
||||
|
||||
F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
|
||||
F(1, 2) = (omega * cosL + vE / R) * dt;
|
||||
F(1, 3) = (-1.0f / R) * dt;
|
||||
F(1, 8) = (vN / RSq) * dt;
|
||||
F(1, 0) = omega * sinL + vE * tanL / R;
|
||||
F(1, 2) = omega * cosL + vE / R;
|
||||
F(1, 3) = -1.0f / R;
|
||||
F(1, 8) = vN / RSq;
|
||||
|
||||
F(2, 0) = (-vN / R) * dt;
|
||||
F(2, 1) = (-omega * cosL - vE / R) * dt;
|
||||
F(2, 4) = (-tanL / R) * dt;
|
||||
F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
|
||||
F(2, 8) = (vE * tanL / RSq) * dt;
|
||||
F(2, 0) = -vN / R;
|
||||
F(2, 1) = -omega * cosL - vE / R;
|
||||
F(2, 4) = -tanL / R;
|
||||
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
|
||||
F(2, 8) = vE * tanL / RSq;
|
||||
|
||||
F(3, 1) = (-fD) * dt;
|
||||
F(3, 2) = (fE) * dt;
|
||||
F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
|
||||
F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
|
||||
F(3, 5) = (vN / R) * dt;
|
||||
F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
|
||||
F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
|
||||
F(3, 1) = -fD;
|
||||
F(3, 2) = fE;
|
||||
F(3, 3) = vD / R;
|
||||
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
|
||||
F(3, 5) = vN / R;
|
||||
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
|
||||
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
|
||||
|
||||
F(4, 0) = (fD) * dt;
|
||||
F(4, 2) = (-fN) * dt;
|
||||
F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
|
||||
F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
|
||||
F(4, 5) = (2 * omega * cosL + vE / R) * dt;
|
||||
F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
|
||||
vN * vE / (R * cosLSq)) * dt;
|
||||
F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
|
||||
F(4, 0) = fD;
|
||||
F(4, 2) = -fN;
|
||||
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
|
||||
F(4, 4) = (vN * tanL + vD) / R;
|
||||
F(4, 5) = 2 * omega * cosL + vE / R;
|
||||
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
|
||||
vN * vE / (R * cosLSq);
|
||||
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
|
||||
|
||||
F(5, 0) = (-fE) * dt;
|
||||
F(5, 1) = (fN) * dt;
|
||||
F(5, 3) = (-2 * vN / R) * dt;
|
||||
F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
|
||||
F(5, 6) = (2 * omega * vE * sinL) * dt;
|
||||
F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
|
||||
F(5, 0) = -fE;
|
||||
F(5, 1) = fN;
|
||||
F(5, 3) = -2 * vN / R;
|
||||
F(5, 4) = -2 * (omega * cosL + vE / R);
|
||||
F(5, 6) = 2 * omega * vE * sinL;
|
||||
F(5, 8) = (vN * vN + vE * vE) / RSq;
|
||||
|
||||
F(6, 3) = (1 / R) * dt;
|
||||
F(6, 8) = (-vN / RSq) * dt;
|
||||
F(6, 3) = 1 / R;
|
||||
F(6, 8) = -vN / RSq;
|
||||
|
||||
F(7, 4) = (1 / (R * cosL)) * dt;
|
||||
F(7, 6) = (vE * tanL / (R * cosL)) * dt;
|
||||
F(7, 8) = (-vE / (cosL * RSq)) * dt;
|
||||
F(7, 4) = 1 / (R * cosL);
|
||||
F(7, 6) = vE * tanL / (R * cosL);
|
||||
F(7, 8) = -vE / (cosL * RSq);
|
||||
|
||||
F(8, 5) = (-1) * dt;
|
||||
F(8, 5) = -1;
|
||||
|
||||
// G Matrix
|
||||
// Titterton pg. 291
|
||||
G(0, 0) = -C_nb(0, 0) * dt;
|
||||
G(0, 1) = -C_nb(0, 1) * dt;
|
||||
G(0, 2) = -C_nb(0, 2) * dt;
|
||||
G(1, 0) = -C_nb(1, 0) * dt;
|
||||
G(1, 1) = -C_nb(1, 1) * dt;
|
||||
G(1, 2) = -C_nb(1, 2) * dt;
|
||||
G(2, 0) = -C_nb(2, 0) * dt;
|
||||
G(2, 1) = -C_nb(2, 1) * dt;
|
||||
G(2, 2) = -C_nb(2, 2) * dt;
|
||||
G(0, 0) = -C_nb(0, 0);
|
||||
G(0, 1) = -C_nb(0, 1);
|
||||
G(0, 2) = -C_nb(0, 2);
|
||||
G(1, 0) = -C_nb(1, 0);
|
||||
G(1, 1) = -C_nb(1, 1);
|
||||
G(1, 2) = -C_nb(1, 2);
|
||||
G(2, 0) = -C_nb(2, 0);
|
||||
G(2, 1) = -C_nb(2, 1);
|
||||
G(2, 2) = -C_nb(2, 2);
|
||||
|
||||
G(3, 3) = C_nb(0, 0) * dt;
|
||||
G(3, 4) = C_nb(0, 1) * dt;
|
||||
G(3, 5) = C_nb(0, 2) * dt;
|
||||
G(4, 3) = C_nb(1, 0) * dt;
|
||||
G(4, 4) = C_nb(1, 1) * dt;
|
||||
G(4, 5) = C_nb(1, 2) * dt;
|
||||
G(5, 3) = C_nb(2, 0) * dt;
|
||||
G(5, 4) = C_nb(2, 1) * dt;
|
||||
G(5, 5) = C_nb(2, 2) * dt;
|
||||
G(3, 3) = C_nb(0, 0);
|
||||
G(3, 4) = C_nb(0, 1);
|
||||
G(3, 5) = C_nb(0, 2);
|
||||
G(4, 3) = C_nb(1, 0);
|
||||
G(4, 4) = C_nb(1, 1);
|
||||
G(4, 5) = C_nb(1, 2);
|
||||
G(5, 3) = C_nb(2, 0);
|
||||
G(5, 4) = C_nb(2, 1);
|
||||
G(5, 5) = C_nb(2, 2);
|
||||
|
||||
// predict equations for kalman filter
|
||||
P = F * P * F.transpose() + G * V * G.transpose();
|
||||
// continuous predictioon equations
|
||||
// for discrte time EKF
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
|
||||
}
|
||||
|
||||
void KalmanNav::correctAtt()
|
||||
@@ -428,12 +452,11 @@ void KalmanNav::correctAtt()
|
||||
|
||||
// mag measurement
|
||||
Vector3 zMag(_sensors.magnetometer_ga);
|
||||
zMag = zMag.unit();
|
||||
|
||||
// mag predicted measurement
|
||||
// choosing some typical magnetic field properties,
|
||||
// TODO dip/dec depend on lat/ lon/ time
|
||||
static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
|
||||
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 bN = cosf(dip) * cosf(dec);
|
||||
float bE = cosf(dip) * sinf(dec);
|
||||
@@ -443,10 +466,29 @@ void KalmanNav::correctAtt()
|
||||
|
||||
// accel measurement
|
||||
Vector3 zAccel(_sensors.accelerometer_m_s2);
|
||||
float accelMag = zAccel.norm();
|
||||
zAccel = zAccel.unit();
|
||||
|
||||
// ignore accel correction when accel mag not close to g
|
||||
Matrix RAttAdjust = RAtt;
|
||||
|
||||
bool ignoreAccel = fabsf(accelMag - g) > 1.1f;
|
||||
|
||||
if (ignoreAccel) {
|
||||
RAttAdjust(3, 3) = 1.0e10;
|
||||
RAttAdjust(4, 4) = 1.0e10;
|
||||
RAttAdjust(5, 5) = 1.0e10;
|
||||
|
||||
} else {
|
||||
//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, -1)).unit();
|
||||
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit();
|
||||
|
||||
// combined measurement
|
||||
Vector zAtt(6);
|
||||
@@ -498,8 +540,9 @@ void KalmanNav::correctAtt()
|
||||
HAtt(5, 1) = cosPhi * sinTheta;
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Vector y = zAtt - zAttHat; // residual
|
||||
Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance
|
||||
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
|
||||
Matrix K = P * HAtt.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
|
||||
@@ -510,24 +553,34 @@ 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;
|
||||
}
|
||||
}
|
||||
|
||||
// correct state
|
||||
phi += xCorrect(PHI);
|
||||
theta += xCorrect(THETA);
|
||||
if (!ignoreAccel) {
|
||||
phi += xCorrect(PHI);
|
||||
theta += xCorrect(THETA);
|
||||
}
|
||||
|
||||
psi += xCorrect(PSI);
|
||||
|
||||
// attitude also affects nav velocities
|
||||
vN += xCorrect(VN);
|
||||
vE += xCorrect(VE);
|
||||
vD += xCorrect(VD);
|
||||
|
||||
// update state covariance
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
P = P - K * HAtt * P;
|
||||
|
||||
// fault detection
|
||||
float beta = y.dot(S.inverse() * y);
|
||||
printf("attitude: beta = %8.4f\n", (double)beta);
|
||||
|
||||
if (beta > 10.0f) {
|
||||
//printf("fault in attitude: beta = %8.4f\n", (double)beta);
|
||||
printf("fault in attitude: beta = %8.4f\n", (double)beta);
|
||||
//printf("y:\n"); y.print();
|
||||
}
|
||||
|
||||
@@ -536,20 +589,32 @@ void KalmanNav::correctAtt()
|
||||
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||
}
|
||||
|
||||
void KalmanNav::correctGps()
|
||||
void KalmanNav::correctPos()
|
||||
{
|
||||
using namespace math;
|
||||
Vector y(6);
|
||||
y(0) = _gps.vel_n - vN;
|
||||
y(1) = _gps.vel_e - vE;
|
||||
y(2) = _gps.vel_d - vD;
|
||||
y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
|
||||
y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
|
||||
y(5) = double(_gps.alt) / 1.0e3 - alt;
|
||||
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(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
|
||||
Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
|
||||
Matrix K = P * HGps.transpose() * S.inverse();
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
|
||||
Matrix K = P * HPos.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
|
||||
// check correction is sane
|
||||
@@ -566,6 +631,8 @@ void KalmanNav::correctGps()
|
||||
setLatDegE7(_gps.lat);
|
||||
setLonDegE7(_gps.lon);
|
||||
setAltE3(_gps.alt);
|
||||
// reset P matrix to identity
|
||||
P = Matrix::identity(9) * 1.0f;
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -579,14 +646,14 @@ void KalmanNav::correctGps()
|
||||
alt += double(xCorrect(ALT));
|
||||
|
||||
// update state covariance
|
||||
P = P - K * HGps * P;
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
P = P - K * HPos * P;
|
||||
|
||||
// fault detetcion
|
||||
float beta = y.dot(S.inverse() * y);
|
||||
printf("gps: beta = %8.4f\n", (double)beta);
|
||||
|
||||
if (beta > 100.0f) {
|
||||
//printf("fault in gps: beta = %8.4f\n", (double)beta);
|
||||
if (beta > 10.0f) {
|
||||
printf("fault in gps: beta = %8.4f\n", (double)beta);
|
||||
//printf("y:\n"); y.print();
|
||||
}
|
||||
}
|
||||
@@ -597,6 +664,7 @@ 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
|
||||
@@ -608,20 +676,31 @@ void KalmanNav::updateParams()
|
||||
V(5, 5) = _vAccel.get(); // accel z
|
||||
|
||||
// magnetometer noise
|
||||
RAtt(0, 0) = _rMag.get(); // normalized direction
|
||||
RAtt(1, 1) = _rMag.get();
|
||||
RAtt(2, 2) = _rMag.get();
|
||||
float noiseMagSq = _rMag.get() * _rMag.get();
|
||||
RAtt(0, 0) = noiseMagSq; // normalized direction
|
||||
RAtt(1, 1) = noiseMagSq;
|
||||
RAtt(2, 2) = noiseMagSq;
|
||||
|
||||
// accelerometer noise
|
||||
RAtt(3, 3) = _rAccel.get(); // normalized direction
|
||||
RAtt(4, 4) = _rAccel.get();
|
||||
RAtt(5, 5) = _rAccel.get();
|
||||
float noiseAccelSq = _rAccel.get() * _rAccel.get();
|
||||
RAtt(3, 3) = noiseAccelSq; // normalized direction
|
||||
RAtt(4, 4) = noiseAccelSq;
|
||||
RAtt(5, 5) = noiseAccelSq;
|
||||
|
||||
// gps noise
|
||||
RGps(0, 0) = _rGpsV.get(); // vn, m/s
|
||||
RGps(1, 1) = _rGpsV.get(); // ve
|
||||
RGps(2, 2) = _rGpsV.get(); // vd
|
||||
RGps(3, 3) = _rGpsGeo.get(); // L, rad
|
||||
RGps(4, 4) = _rGpsGeo.get(); // l, rad
|
||||
RGps(5, 5) = _rGpsAlt.get(); // h, m
|
||||
float cosLSing = cosf(lat);
|
||||
float R = R0 + float(alt);
|
||||
|
||||
// prevent singularity
|
||||
if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
|
||||
|
||||
float noiseVel = _rGpsVel.get();
|
||||
float noiseLat = _rGpsPos.get() / R;
|
||||
float noiseLon = _rGpsPos.get() / (R * cosLSing);
|
||||
float noiseAlt = _rGpsAlt.get();
|
||||
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(4, 4) = noiseAlt * noiseAlt; // h
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user