GCOP  1.0
imu.h
Go to the documentation of this file.
00001 #ifndef GCOP_IMU_H
00002 #define GCOP_IMU_H
00003 
00004 #include "system.h"
00005 #include "imumanifold.h"
00006 #include "so3.h"
00007 #include <limits>
00008 #include <iostream>
00009 #include <utility>
00010 
00011 namespace gcop {
00012   
00013   using namespace std;
00014   using namespace Eigen;
00015   
00016   typedef Matrix<double, 9, 1> Vector9d;
00017   typedef Matrix<double, 6, 1> Vector6d;
00018   typedef Matrix<double, 9, 3> Matrix93d;
00019   typedef Matrix<double, 9, 6> Matrix96d;
00020   typedef Matrix<double, 9, 9> Matrix9d;
00021   typedef Matrix<double, 9, Dynamic> Matrix9Xd;
00022 
00023   typedef Matrix<double, 6, 3> Matrix63d;
00024   typedef Matrix<double, 6, 6> Matrix6d;
00025   typedef Matrix<double, 6, 9> Matrix69d;
00026   typedef Matrix<double, 6, Dynamic> Matrix6Xd;
00027   
00033   class Imu : public System<ImuState, 9, 3> {        
00034   public:
00035   
00036   Imu();
00037   
00038   virtual ~Imu();
00039   
00040   double Step(ImuState &xb, double t, const ImuState &xa, 
00041               const Vector3d &u, double h, const VectorXd *p = 0,
00042               Matrix9d *A = 0, Matrix93d *B = 0, Matrix9Xd *C = 0);
00043   
00044   bool Noise(Matrix9d &Q, double t, const ImuState &x, const Vector3d &u, 
00045              double dt, const VectorXd *p = 0);
00046   
00047   double sv; 
00048   double su; 
00049   double sa; 
00050 
00051   };  
00052 }
00053 
00054 #endif