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

Tracking down gps ekf bug, not enough precision for GPS in rad.

Browse Source
This commit is contained in:
James Goppert
2013-01-13 21:12:24 -05:00
parent 63e6ea1b95
commit 5745cfae38
1 changed files with 32 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+32 -10
View File
@@ -316,7 +316,11 @@ void KalmanNav::predictFast(float dt)
float cosL = cosf(lat); float cosL = cosf(lat);
float cosLSing = 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 // position update
// neglects angular deflections in local gravity // neglects angular deflections in local gravity
@@ -577,11 +581,11 @@ void KalmanNav::correctAtt()
P = P - K * HAtt * P; P = P - K * HAtt * P;
// fault detection // fault detection
float beta = y.dot(S.inverse() * y); float beta = y.dot((S*S.transpose()).inverse() * y);
if (beta > 1000.0f) { if (beta > 1.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(); printf("y:\n"); y.print();
} }
// update quaternions from euler // update quaternions from euler
@@ -592,7 +596,7 @@ void KalmanNav::correctAtt()
void KalmanNav::correctPos() void KalmanNav::correctPos()
{ {
using namespace math; using namespace math;
Vector y(6); Vector y(5);
y(0) = _gps.vel_n - vN; y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE; y(1) = _gps.vel_e - vE;
y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat; y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
@@ -604,7 +608,10 @@ void KalmanNav::correctPos()
float R = R0 + float(alt); float R = R0 + float(alt);
// prevent singularity // 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 noiseLat = _rGpsPos.get() / R; float noiseLat = _rGpsPos.get() / R;
float noiseLon = _rGpsPos.get() / (R * cosLSing); float noiseLon = _rGpsPos.get() / (R * cosLSing);
@@ -650,11 +657,23 @@ void KalmanNav::correctPos()
P = P - K * HPos * P; P = P - K * HPos * P;
// fault detetcion // fault detetcion
float beta = y.dot(S.inverse() * y); float beta = y.dot((S*S.transpose()).inverse() * y);
if (beta > 1000.0f) { if (beta > 1.0f) {
printf("fault in gps: beta = %8.4f\n", (double)beta); printf("fault in gps: beta = %8.4f\n", (double)beta);
//printf("y:\n"); y.print(); printf("y:\n"); y.print();
printf("R:\n"); RPos.print();
printf("S:\n"); S.print();
printf("S*S^T:\n"); (S*S.transpose()).print();
printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print();
printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print();
printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n",
double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d),
double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG,
double(_gps.lon)/ 1.0e7 / M_RAD_TO_DEG,
double(_gps.alt)/ 1.0e3);
printf("x : vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n",
double(vN), double(vE), double(vD), lat, lon, alt);
} }
} }
@@ -692,7 +711,10 @@ void KalmanNav::updateParams()
float R = R0 + float(alt); float R = R0 + float(alt);
// prevent singularity // 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 noiseVel = _rGpsVel.get();
float noiseLat = _rGpsPos.get() / R; float noiseLat = _rGpsPos.get() / R;