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

Fault detection working, but GPS velocity measurement causing fault.

Browse Source 
Possible error in HIL script or progpagation/ F matrix of EKF.
This commit is contained in:
jgoppert
2013-01-14 01:32:34 -05:00
parent 3db216380b
commit f2d2aafb8d
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+7 -7
View File
@@ -613,7 +613,7 @@ void KalmanNav::correctPos()
else cosLSing = -0.01; else cosLSing = -0.01;
} }
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R; float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing; float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
@@ -662,11 +662,11 @@ void KalmanNav::correctPos()
if (beta > 1.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("R:\n"); RPos.print();
printf("S:\n"); S.print(); //printf("S:\n"); S.print();
printf("S*S^T:\n"); (S*S.transpose()).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:\n"); ((S*S.transpose()).inverse()).print();
printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).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", 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.vel_n), double(_gps.vel_e), double(_gps.vel_d),
double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG, double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG,
@@ -717,7 +717,7 @@ void KalmanNav::updateParams()
} }
float noiseVel = _rGpsVel.get(); float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R; float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing; float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseAlt = _rGpsAlt.get(); float noiseAlt = _rGpsAlt.get();
RPos(0, 0) = noiseVel * noiseVel; // vn RPos(0, 0) = noiseVel * noiseVel; // vn