diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-04 08:38:06 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-04 08:38:06 +0200 |
commit | f6406a0b19f833923f6f600be73116164029795f (patch) | |
tree | 0e77000ea6456dae373294dd9eaa0cf5f66b9606 /src/modules/ekf_att_pos_estimator/estimator_23states.cpp | |
parent | d5793d6cbee59779c1f5375fa3276bd0fddbe25e (diff) | |
parent | 2389a11af1249f657d85d36a5e71db83940a7959 (diff) | |
download | px4-firmware-f6406a0b19f833923f6f600be73116164029795f.tar.gz px4-firmware-f6406a0b19f833923f6f600be73116164029795f.tar.bz2 px4-firmware-f6406a0b19f833923f6f600be73116164029795f.zip |
Merge remote-tracking branch 'upstream/master' into hkmicropcb
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_23states.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 |
1 files changed, 6 insertions, 6 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 9622f7e40..deaaf55fe 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); + quat2Tbn(Tbn, initQuat); + Tnb = Tbn.transpose(); Vector3f initMagNED; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - 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; + initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z; + initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z; + initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z; magstate.q0 = initQuat[0]; magstate.q1 = initQuat[1]; @@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM = DCM; + magstate.DCM = Tbn; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions |