GCOP
1.0
|
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