[paparazzi-commits] [5940] ins_qkf: added init for the orientation

[Martin Dieblich]

Revision: 5940
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5940
Author:   mdieblich
Date:     2010-09-24 08:23:11 +0000 (Fri, 24 Sep 2010)
Log Message:
-----------
ins_qkf:        added init for the orientation
predict:        changed some names

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

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp       2010-09-23 
23:06:26 UTC (rev 5939)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp       2010-09-24 
08:23:11 UTC (rev 5940)
@@ -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>
 
@@ -155,13 +155,23 @@
         * @param gyro_stability_noise The diagonal matrix of gyro instability 
noise
         * @param accel_white_noise The diagonal matrix of accelerometer white 
noise
         */
+                       //Old one without orientation_init()
        basic_ins_qkf(const Vector3d& pos_estimate,
                        double pos_error, double bias_error, double v_error,
                        const Vector3d& gyro_white_noise,
                        const Vector3d& gyro_stability_noise,
                        const Vector3d& accel_white_noise,
+                       Quaterniond initial_orientation = 
Quaterniond::Identity(),
                        const Vector3d& vel_estimate = Vector3d::Zero());
-
+       
+       /*              //Old one without orientation_init()
+       basic_ins_qkf(const Vector3d& pos_estimate,
+                       double pos_error, double bias_error, double v_error,
+                       const Vector3d& gyro_white_noise,
+                       const Vector3d& gyro_stability_noise,
+                       const Vector3d& accel_white_noise,
+                       const Vector3d& vel_estimate = Vector3d::Zero());
+       */
        /**
         * Report an INS observation, to propagate the filter forward by one 
time
         * step. The coordinate system is maintained in ECEF coordinates.
@@ -237,7 +247,6 @@
         */
        void obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error);
 
-
        /**
         * Measure the total angular error between the filter's attitude 
estimate
         * and some other orientation.

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp       
2010-09-23 23:06:26 UTC (rev 5939)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp       
2010-09-24 08:23:11 UTC (rev 5940)
@@ -19,11 +19,18 @@
  *  along with libeknav.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/**
+ * This is a modified version by Martin Dieblich
+ * The general changes are a more consequent naming
+ * 
+ */
+
+
 #include "ins_qkf.hpp"
 #include "assertions.hpp"
+#include "timer.hpp"
 
 #ifdef TIME_OPS
-#include "timer.hpp"
 # include <iostream>
 #endif
 
@@ -51,13 +58,13 @@
        // zero out the error vector in the z direction.
        // accel_cov is the relationship between error vectors in the tangent 
space
        // of the vehicle orientation and the translational reference frame.
-       Vector3d accel_body = 
_this.avg_state.orientation.conjugate()*accel_meas;               // accel_body 
is in ECEF!
-       std::cout << "   ACCEL_BODY(" << accel_body.transpose() << ")\n";
-       Vector3d accel_gravity = _this.avg_state.position.normalized()*9.81;
-       std::cout << "   ACCEL_GRAVITY(" << accel_gravity.transpose() << ")\n";
-       //Vector3d accel_resid = accel_body - accel_gravity;
-       Vector3d accel_resid = accel_body + accel_gravity;                      
                                        // This is better!
-       std::cout << "   ACCEL_RESID(" << accel_resid.transpose() << ")\n";
+       
+       Matrix3d rot = 
_this.avg_state.orientation.conjugate().toRotationMatrix();
+       
+       Vector3d accel_ecef = rot*accel_meas;           // a_e = (q_e2b)^* x 
a_b = q_b2e x a_b
+       Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81); 
        
+       Vector3d accel_resid = accel_ecef + accel_gravity;                      
                                        // a = (xdd-g)+g = xdd;
+       
 #if 0
        // This form works well with zero static acceleration.
        Matrix<double, 3, 3> accel_cov =
@@ -65,12 +72,12 @@
                * axis_scale(_this.avg_state.position.normalized(), 0) * 9.81;
 #elif 1
        Matrix<double, 3, 3> accel_cov =
-               Eigen::AngleAxisd(-M_PI*0.5, accel_body.normalized())
-               * axis_scale(accel_body.normalized(), 0) * accel_meas.norm();
+               Eigen::AngleAxisd(-M_PI*0.5, accel_ecef.normalized())
+               * axis_scale(accel_ecef.normalized(), 0) * accel_meas.norm();
 #else
        // The following form ends up being identical to the simpler one
        // above
-       Matrix<double, 3, 3> accel_cov = 
+       Matrix<double, 3, 3> accel_cov =
                Eigen::AngleAxisd(-M_PI*0.5, 
_this.avg_state.position.normalized())
                * axis_scale(_this.avg_state.position.normalized(), 0) * 9.81
                + Eigen::AngleAxisd(-M_PI*0.5, accel_resid.normalized())
@@ -134,9 +141,10 @@
 
        Quaterniond orientation = exp<double>((gyro_meas - 
_this.avg_state.gyro_bias) * dt)
                        * _this.avg_state.orientation;
-       Vector3d accel = accel_body - _this.avg_state.position.normalized() * 
9.81;
-       Vector3d position = _this.avg_state.position + _this.avg_state.velocity 
* dt + 0.5*accel*dt*dt;
-       Vector3d velocity = _this.avg_state.velocity + accel*dt;
+       //Vector3d accel = accel_ecef - _this.avg_state.position.normalized() * 
9.81;
+       //std::cout << "   ACCEL______(" << accel.transpose() << ")\n";
+       Vector3d position = _this.avg_state.position + _this.avg_state.velocity 
* dt + 0.5*accel_resid*dt*dt;
+       Vector3d velocity = _this.avg_state.velocity + accel_resid*dt;
 
        _this.avg_state.position = position;
        _this.avg_state.velocity = velocity;