diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-21 11:02:27 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-21 11:02:27 +0200 |
commit | 595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9 (patch) | |
tree | cfa804d03755eec6fe12f772e27f742cf779f181 /src | |
parent | aa3aafb1e5d060593529af72bcf22d6351374df9 (diff) | |
download | px4-firmware-595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9.tar.gz px4-firmware-595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9.tar.bz2 px4-firmware-595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9.zip |
ekf_att_pos_estimator: Fixed mag initialization, now starts with initial measurement instead of defaults for faster convergence
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 19 |
1 files changed, 11 insertions, 8 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 18e17d4d9..95afbc181 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2419,15 +2419,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - magstate.q3 = 1.0f; - magstate.magN = 0.4f; - magstate.magE = 0.0f; - magstate.magD = 0.3f; - magstate.magXbias = 0.0f; - magstate.magYbias = 0.0f; - magstate.magZbias = 0.0f; + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = magData.x; + magstate.magE = magData.y; + magstate.magD = magData.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM.identity(); + magstate.DCM = DCM; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions |