[paparazzi-commits] [5941] orientation is now initilaizable

[Martin Dieblich]

Revision: 5941
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5941
Author:   mdieblich
Date:     2010-09-24 08:44:42 +0000 (Fri, 24 Sep 2010)
Log Message:
-----------
orientation is now initilaizable

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp 2010-09-24 
08:23:11 UTC (rev 5940)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp 2010-09-24 
08:44:42 UTC (rev 5941)
@@ -30,6 +30,7 @@
                const Vector3d& gyro_white_noise,
                const Vector3d& gyro_stability_noise,
                const Vector3d& accel_white_noise,
+               Quaterniond initial_orientation,                // this is new
                const Vector3d& vel_estimate)
        : gyro_stability_noise(gyro_stability_noise)
        , gyro_white_noise(gyro_white_noise)
@@ -37,7 +38,8 @@
 {
        avg_state.position = estimate;
        avg_state.gyro_bias = Vector3d::Zero();
-       avg_state.orientation = Quaterniond::Identity();
+       //avg_state.orientation = Quaterniond::Identity();
+       avg_state.orientation = initial_orientation;
        avg_state.velocity = vel_estimate;
 
        cov << Matrix3d::Identity()*bias_error*bias_error, Matrix<double, 3, 
9>::Zero(),

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp       2010-09-24 
08:23:11 UTC (rev 5940)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp       2010-09-24 
08:44:42 UTC (rev 5941)
@@ -20,7 +20,7 @@
  *  along with libeknav.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "sigma_points.hpp"
+//#include "sigma_points.hpp"
 #include "quaternions.hpp"
 #include <Eigen/StdVector>
 

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp       
2010-09-24 08:23:11 UTC (rev 5940)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_2.cpp       
2010-09-24 08:44:42 UTC (rev 5941)
@@ -20,7 +20,7 @@
   /* initial state */
   Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3);
   Vector3d speed_0_ecef(0., 0., 0.);
-  //  Vector3d orientation(0., 0., 0.);
+    Quaterniond orientation(1., 0., 0., 0.);
   Vector3d bias_0(0., 0., 0.);
 
   /* initial covariance */
@@ -45,7 +45,7 @@
   Vector3d magnetometer = Vector3d::UnitZ();
 
   basic_ins_qkf ins(pos_0_ecef, pos_cov_0, bias_cov_0, speed_cov_0,
-                   gyro_white_noise, gyro_stability_noise, accel_white_noise);
+                   gyro_white_noise, gyro_stability_noise, 
accel_white_noise,orientation);
 
   const double dt = 1./512.; /* predict at 512Hz */
   for (int i=1; i<5000; i++) {