[paparazzi-commits] [6158] Added a lib for estimating an orientation from a set of observations

[Martin Dieblich]

Revision: 6158
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6158
Author:   mdieblich
Date:     2010-10-15 17:54:55 +0000 (Fri, 15 Oct 2010)
Log Message:
-----------
Added a lib for estimating an orientation from a set of observations

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c
    paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.h

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c               
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c       
2010-10-15 17:54:55 UTC (rev 6158)
@@ -0,0 +1,147 @@
+#include "estimate_attitude.h"
+
+struct DoubleMat44 square_skaled(struct DoubleMat44 A){
+  double _1_max = 1/INFTY_NORM16(A);
+  struct DoubleMat44 A2;
+  int row,col;
+  for(row=0; row<4; row++){
+    for(col=0; col<4; col++){
+      M4(A2, row, col) = M4(A, row,0)*M4(A,0,col) + M4(A, row,1)*M4(A,1,col) + 
M4(A, row,2)*M4(A,2,col) + M4(A, row,3)*M4(A,3,col);
+      M4(A2, row, col) *= _1_max;   // pays attention that the values don't 
grow too far
+    }
+  }
+  return A2;
+}
+
+/* the following solver uses the Power Iteration
+ * 
+ * It is rather simple:
+ * 1. You choose a starting vektor x_0 (shouldn't be zero)
+ * 2. apply it on the Matrix
+ *               x_(k+1) = A * x_k
+ * 3. Repeat this very often.
+ * 
+ * The vector converges to the dominat eigenvector, which belongs to the 
eigenvalue with the biggest absolute value.
+ * 
+ * But there is a problem:
+ * With every step, the norm of vector grows. Therefore it's necessary to 
scale it with every step.
+ * 
+ * Important warnings:
+ * I.   This function does not converge if the Matrix is singular
+ * II.  Pay attention to the loop-condition! It does not end if it's close to 
the eigenvector!
+ *      It ends if the steps are getting too close to each other.
+ * 
+ */ 
+DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int 
maximum_iterations, double precision){
+  unsigned int  k;
+  DoubleVect4 x_k,
+              x_kp1;
+  double delta = 1,
+         scale;
+  
+  FLOAT_QUAT_ZERO(x_k);
+  
+  for(k=0; (k<maximum_iterations) && (delta>precision); k++){
+    
+    // Next step
+    DOUBLE_MAT_VMULT4(x_kp1, A, x_k);
+    
+    // Scale the vector
+    scale = 1/INFTY_NORM4(x_kp1);
+    QUAT_SMUL(x_kp1, x_kp1, scale);
+    
+    // Calculate the difference between to steps for the loop condition. Store 
temporarily in x_k
+    QUAT_DIFF(x_k, x_k, x_kp1);
+    delta = INFTY_NORM4(x_k);
+    
+    // Update the next step
+    x_k = x_kp1;
+    
+  }
+  return x_k;
+}
+
+/*    This function estimates a quaternion from a set of observations
+ * 
+ * B is the "attitude profile matrix". Use the other functions to fill it with 
the attitude observations.
+ * 
+ * The function solves Wahba's problem using Paul Davenport's solution.
+ * Unfortunatly Davenport unpublished his solution, but you can still find 
descriptions of it in the web
+ * ( e.g: home.comcast.net/~mdshuster2/PUB_2006c_J_GenWahba_AAS.pdf )
+ * 
+ */
+struct DoubleQuat estimated_attitude(struct DoubleMat33 B, unsigned int 
maximum_iterations, double precision){
+  double      traceB,
+              z1, z2, z3;
+  struct DoubleMat44 K;
+  struct DoubleQuat  q_guessed;
+  
+  // pre-computation of some values
+  traceB  = RMAT_TRACE(B);
+  z1      = M3(B,1,2) - M3(B, 2,1);
+  z2      = M3(B,2,0) - M3(B, 0,2);
+  z3      = M3(B,0,1) - M3(B, 1,0);
+  
+  /** The "K"-Matrix. See the references for it **/
+  /* Fill the upper triangle matrix */
+  M4(K,0,0) = traceB; M4(K,0,1) = z1;                 M4(K,0,2) = z2;          
         M4(K,0,3) = z3;
+                      M4(K,1,1) = 2*M3(B,0,0)-traceB; M4(K,1,2) = 
M3(B,0,1)+M3(B,1,0);  M4(K,1,3) = M3(B,0,2)+M3(B,2,0); 
+                                                      M4(K,2,2) = 
2*M3(B,1,1)-traceB;   M4(K,2,3) = M3(B,1,2)+M3(B,2,1);
+                                                                               
         M4(K,3,3) = 2*M3(B,2,2)-traceB;
+  /* Copy to  lower triangle matrix */
+  M4(K,1,0) = M4(K,0,1);
+  M4(K,2,0) = M4(K,0,2);  M4(K,2,1) = M4(K,1,2);
+  M4(K,3,0) = M4(K,0,3);  M4(K,3,1) = M4(K,1,3);  M4(K,3,2) = M4(K,2,3);
+  
+  /* compute the estimated quaternion */
+  q_guessed = dominant_Eigenvector(square_skaled(K), maximum_iterations, 
precision);    // K² is tricky. I'm mean, I know
+  
+  /* Final scaling, because the eigenvector hat not the length = 1 */
+  QUAT_SMUL(q_guessed, q_guessed, 1/NORM_VECT4(q_guessed));
+  return q_guessed;
+}
+
+/*  Adds an orientation observation to the "attitude profile matrix" B
+ *  
+ *  This is done with
+ *        B += a * W * V'
+ *  where
+ *      a     is the weight of the current measurment (how good it is, how 
often...)
+ *      W     is the observed vector
+ *      V'    is the (transposed) reference direction, that belongs to W.
+ * 
+ *  See Davenport's solution for Wabha's problem for more information
+ */
+void add_orientation_measurement(struct DoubleMat33* B, struct 
Orientation_Measurement a){
+  M3(*B,0,0) += a.weight_of_the_measurement * a.measured_direction.x * 
a.reference_direction.x;
+  M3(*B,0,1) += a.weight_of_the_measurement * a.measured_direction.x * 
a.reference_direction.y;
+  M3(*B,0,2) += a.weight_of_the_measurement * a.measured_direction.x * 
a.reference_direction.z;
+  M3(*B,1,0) += a.weight_of_the_measurement * a.measured_direction.y * 
a.reference_direction.x;
+  M3(*B,1,1) += a.weight_of_the_measurement * a.measured_direction.y * 
a.reference_direction.y;
+  M3(*B,1,2) += a.weight_of_the_measurement * a.measured_direction.y * 
a.reference_direction.z;
+  M3(*B,2,0) += a.weight_of_the_measurement * a.measured_direction.z * 
a.reference_direction.x;
+  M3(*B,2,1) += a.weight_of_the_measurement * a.measured_direction.z * 
a.reference_direction.y;
+  M3(*B,2,2) += a.weight_of_the_measurement * a.measured_direction.z * 
a.reference_direction.z;
+}
+
+/*  Generates a "faked" orientation measurement out of two true observations
+ *  
+ *  This is necessary because othwerwise the attitude profile matrix has only 
two degrees of freedom.
+ *  
+ *  The third reference direction is the cross product of the other two, same 
for the measurement.
+ */
+struct Orientation_Measurement fake_orientation_measurement(struct 
Orientation_Measurement a, struct Orientation_Measurement b){
+  struct Orientation_Measurement fake;
+  DOUBLE_VECT3_CROSS_PRODUCT(fake.reference_direction, a.reference_direction, 
b.reference_direction);
+  DOUBLE_VECT3_CROSS_PRODUCT(fake.measured_direction,  a.measured_direction,  
b.measured_direction );
+  fake.weight_of_the_measurement = a.weight_of_the_measurement * 
b.weight_of_the_measurement;
+  return fake;
+}
+
+/*  Add two true orientation measurements to the "attitude profile matrix" and 
a faked measurement */
+void add_set_of_three_measurements(struct DoubleMat33* B, struct 
Orientation_Measurement a, struct Orientation_Measurement b){
+  struct Orientation_Measurement fake = fake_orientation_measurement(a, b);
+  add_orientation_measurement(B, a);
+  add_orientation_measurement(B, b);
+  add_orientation_measurement(B, fake);
+}

Added: paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.h               
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.h       
2010-10-15 17:54:55 UTC (rev 6158)
@@ -0,0 +1,68 @@
+#ifndef ESTIMATE_ATTITUDE_H
+#define ESTIMATE_ATTITUDE_H
+
+/* for sqrt */
+#include <math.h>
+
+#include "paparazzi_eigen_conversion.h"
+#include "../../math/pprz_algebra.h"
+#include "../../math/pprz_algebra_double.h"
+
+/** beacuse I'm lazy    **/
+#define M3(Mat, row, col) MAT33_ELMT(Mat, row,col)
+
+/** everything for the 4D-algebra **/
+struct DoubleMat44{
+  double m[4*4];
+};
+typedef struct DoubleQuat DoubleVect4;
+
+// accessing single values
+#define M4(Mat, row, col) Mat.m[row*4+col]
+
+#define DOUBLE_MAT44_ASSIGN(Mat, a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, 
p){ \
+  M4(Mat, 0,0) = a; M4(Mat, 0,1) = b; M4(Mat, 0,2) = c; M4(Mat, 0,3) = d;      
   \
+  M4(Mat, 1,0) = e; M4(Mat, 1,1) = f; M4(Mat, 1,2) = g; M4(Mat, 1,3) = h;      
   \
+  M4(Mat, 2,0) = i; M4(Mat, 2,1) = j; M4(Mat, 2,2) = k; M4(Mat, 2,3) = l;      
   \
+  M4(Mat, 3,0) = m; M4(Mat, 3,1) = n; M4(Mat, 3,2) = o; M4(Mat, 3,3) = p;      
   \
+}
+
+#define DOUBLE_MAT_VMULT4(out, mat, in){                                       
           \
+  out.qi = M4(mat, 0,0)*in.qi + M4(mat, 0,1)*in.qx + M4(mat, 0,2)*in.qy + 
M4(mat, 0,3)*in.qz;  \
+  out.qx = M4(mat, 1,0)*in.qi + M4(mat, 1,1)*in.qx + M4(mat, 1,2)*in.qy + 
M4(mat, 1,3)*in.qz;  \
+  out.qy = M4(mat, 2,0)*in.qi + M4(mat, 2,1)*in.qx + M4(mat, 2,2)*in.qy + 
M4(mat, 2,3)*in.qz;  \
+  out.qz = M4(mat, 3,0)*in.qi + M4(mat, 3,1)*in.qx + M4(mat, 3,2)*in.qy + 
M4(mat, 3,3)*in.qz;  \
+}
+
+struct DoubleMat44 square_skaled(struct DoubleMat44);
+
+/* everything for normes */
+#define MAX4(a,b,c,d) (MAX(MAX(a,b),MAX(c,d)))
+#define MAX4ABS(a,b,c,d) (MAX(MAXABS(a,b),MAXABS(c,d)))
+#define MAX16ABS(a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p) MAX4(MAX4ABS(a,b,c,d), 
MAX4ABS(e,f,g,h), MAX4ABS(i,j,k,l), MAX4ABS(m,n,o,p))
+
+#define INFTY_NORM4(v) (MAX4ABS(v.qi,v.qx,v.qy,v.qz))
+#define INFTY_NORM16(M) MAX16ABS( M.m[ 0], M.m[ 1], M.m[ 2], M.m[ 3], \
+                                  M.m[ 4], M.m[ 5], M.m[ 6], M.m[ 7], \
+                                  M.m[ 8], M.m[ 9], M.m[10], M.m[11], \
+                                  M.m[12], M.m[13], M.m[14], M.m[15])
+
+#define NORM_VECT4(v) sqrt(SQUARE(v.qi)+SQUARE(v.qx)+SQUARE(v.qy)+SQUARE(v.qz))
+
+/** Wahba-solver **/
+DoubleVect4 dominant_Eigenvector(struct DoubleMat44, unsigned int, double);
+struct DoubleQuat  estimated_attitude(  struct DoubleMat33, unsigned int, 
double);
+
+
+/** Attitude Measurement Handling **/
+struct Orientation_Measurement{
+  struct DoubleVect3 reference_direction,
+              measured_direction;
+  double weight_of_the_measurement;
+};
+
+void add_orientation_measurement(struct DoubleMat33*, struct 
Orientation_Measurement);
+struct Orientation_Measurement fake_orientation_measurement(struct 
Orientation_Measurement, struct Orientation_Measurement);
+void add_set_of_three_measurements(struct DoubleMat33*, struct 
Orientation_Measurement, struct Orientation_Measurement);
+
+#endif /* ESTIMATE_ATTITUDE_H */

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-13 19:53:30 UTC (rev 6157)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-15 17:54:55 UTC (rev 6158)
@@ -6,20 +6,22 @@
 #include "ins_qkf.hpp"
 #include "paparazzi_eigen_conversion.h"
 #include <stdint.h>
+
+#include <stdio.h>
 #include <stdlib.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
 
-#include <event.h>
+//#include <event.h>
 
 
 #include "math/pprz_algebra_float.h"
 #include "math/pprz_algebra_double.h"
 #include "math/pprz_geodetic.h"
-#include "math/pprz_geodetic_int.c"
-#include "math/pprz_geodetic_float.c"
-#include "math/pprz_geodetic_double.c"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
+#include "math/pprz_geodetic_double.h"
 #include <math.h>
 
 #include <unistd.h>

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-13 19:53:30 UTC (rev 6157)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-15 17:54:55 UTC (rev 6158)
@@ -36,8 +36,11 @@
  * (could also be in math/algebra.h)
  */
 
+#define PI_180 1.74532925199433e-02
 #define ARCSEC_ACRMIN_ANGLE_IN_RADIANS(degree, arcmin, arcsec) 
((degree+arcmin/60+arcsec/3600)*M_PI/180)
 #define ABS(a) ((a<0)?-a:a)
+#define MAX(a,b) ((a)>(b)?(a):(b))
+#define MAXABS(a,b) MAX(ABS(a),ABS(b))
 #define MAT33_TRANSP(_to,_from) {                                  \
     MAT33_ELMT((_to),0,0) = MAT33_ELMT((_from),0,0);   \
     MAT33_ELMT((_to),0,1) = MAT33_ELMT((_from),1,0);   \