diff options
| author | Andrea Nistico <andrea.nistic@gmail.com> | 2015-04-24 16:45:17 +0200 |
|---|---|---|
| committer | Andrea Nistico <andrea.nistic@gmail.com> | 2015-04-24 16:45:17 +0200 |
| commit | 1f5775615ffb04f22456795ea39cb1402ae51d5f (patch) | |
| tree | a8f9fc0e3def37772224effcfe37c3d70d4b0e64 | |
| parent | c5ce8f39ae6a25655fc7f85a0639458bb170b312 (diff) | |
| download | px4-firmware-1f5775615ffb04f22456795ea39cb1402ae51d5f.tar.gz px4-firmware-1f5775615ffb04f22456795ea39cb1402ae51d5f.tar.bz2 px4-firmware-1f5775615ffb04f22456795ea39cb1402ae51d5f.zip | |
Rvis transposed, in this way we have consisency
| -rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b2a3572a7..68780d5dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) math::Vector<3> v(1.0f, 0.0f, 0.4f); - math::Vector<3> vn = Rvis * v; - + math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency z_k[6] = vn(0); z_k[7] = vn(1); z_k[8] = vn(2); |