fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 21:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adding comments to ekf.

Browse Source
This commit is contained in:
James Goppert
2013-01-15 11:00:52 -05:00
parent 5b0aa490d6
commit d02a24ec82
2 changed files with 102 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+21 -18
View File
@@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// miss counts
_missFast(0),
_missSlow(0),
// state
// 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),
@@ -94,13 +95,15 @@ 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, "MAG_DIP"),
_magDec(this, "MAG_DEC")
{
using namespace math;
// initial state covariance matrix
P0 = Matrix::identity(9) * 1.0f;
P = P0;
P = P0;
// wait for gps lock
while (1) {
@@ -132,12 +135,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
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));
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);
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));
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);
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
@@ -342,10 +345,10 @@ void KalmanNav::predictFast(float dt)
vN * LDot + g;
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));
printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
double(LDot), double(lDot), double(rotRate));
printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
double(vNDot), double(vEDot), double(vDDot));
printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
double(LDot), double(lDot), double(rotRate));
// rectangular integration
//printf("dt: %8.4f\n", double(dt));
@@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt)
// prepare for matrix
float R = R0 + float(alt);
float RSq = R*R;
float RSq = R * R;
// F Matrix
// Titterton pg. 291
@@ -472,8 +475,8 @@ void KalmanNav::correctAtt()
// 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
static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
static const 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);
@@ -593,7 +596,7 @@ void KalmanNav::correctAtt()
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse()*y);
float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
@@ -671,7 +674,7 @@ void KalmanNav::correctPos()
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse()*y);
float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) {
printf("fault in gps: beta = %8.4f\n", (double)beta);