Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Increased KF process noise.

Browse Source
This commit is contained in:
James Goppert
2013-01-14 13:12:01 -05:00
parent f122eb7576
commit a13cf90e0a
2 changed files with 8 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+6 -15
View File
@@ -583,7 +583,7 @@ void KalmanNav::correctAtt()
// fault detection
float beta = y.dot((S*S.transpose()).inverse() * y);
if (beta > 1.0f) {
if (beta > 10.0f) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
printf("y:\n"); y.print();
}
@@ -613,8 +613,10 @@ void KalmanNav::correctPos()
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseAlt = _rGpsAlt.get();
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
@@ -659,21 +661,10 @@ void KalmanNav::correctPos()
// fault detetcion
float beta = y.dot((S*S.transpose()).inverse() * y);
if (beta > 1.0f) {
if (beta > 10.0f) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
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);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
y(0)/noiseVel, y(1)/noiseVel, y(2)/noiseLatDegE7, y(3)/noiseLonDegE7, y(4)/noiseAlt);
}
}
apps/examples/kalman_demo/params.c
+2 -2
View File
@@ -1,8 +1,8 @@
#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);