diff options
author | jgoppert <james.goppert@gmail.com> | 2013-01-14 01:32:34 -0500 |
---|---|---|
committer | jgoppert <james.goppert@gmail.com> | 2013-01-14 01:32:34 -0500 |
commit | f2d2aafb8d340bdad7e75e9df27b8fd002da1344 (patch) | |
tree | c406903979e2ba86d01b0b24e6cad05f7998758c | |
parent | 3db216380bed13fd25c21e49a1fbd66680968937 (diff) | |
download | px4-firmware-f2d2aafb8d340bdad7e75e9df27b8fd002da1344.tar.gz px4-firmware-f2d2aafb8d340bdad7e75e9df27b8fd002da1344.tar.bz2 px4-firmware-f2d2aafb8d340bdad7e75e9df27b8fd002da1344.zip |
Fault detection working, but GPS velocity measurement causing fault.
Possible error in HIL script or progpagation/ F matrix of EKF.
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 14 |
1 files changed, 7 insertions, 7 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 788cb5a71..c3093fe7f 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -613,7 +613,7 @@ void KalmanNav::correctPos() 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; RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; @@ -662,11 +662,11 @@ void KalmanNav::correctPos() if (beta > 1.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("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, @@ -717,7 +717,7 @@ void KalmanNav::updateParams() } 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 noiseAlt = _rGpsAlt.get(); RPos(0, 0) = noiseVel * noiseVel; // vn |