Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef IMU_FILTER_IMU_MEASUREMENT_H
00039 #define IMU_FILTER_IMU_MEASUREMENT_H
00040
00041 #include <Eigen/Eigen>
00042 #include <Eigen/Geometry>
00043
00044 namespace imu_filter
00045 {
00046
00050 class ImuMeasurement
00051 {
00052 public:
00053 ImuMeasurement() :
00054 angularRate( Eigen::Vector3d::Zero() ),
00055 linearAcceleration( Eigen::Vector3d::Zero() ),
00056 angularRatePerturbation( Eigen::Vector3d::Zero() ),
00057 linearAccelerationPerturbation( Eigen::Vector3d::Zero() ),
00058 gyroBiasPerturbation( Eigen::Vector3d::Zero() ),
00059 accBiasPerturbation( Eigen::Vector3d::Zero() ),
00060 angularRateCovariance( Eigen::Matrix3d::Zero() ),
00061 linearAccelerationCovariance( Eigen::Matrix3d::Zero() ),
00062 gyroBiasCovariance( Eigen::Matrix3d::Zero() ),
00063 accBiasCovariance( Eigen::Matrix3d::Zero() )
00064 {
00065 }
00066
00067 ImuMeasurement( const ImuMeasurement & other ) :
00068 angularRate( other.angularRate ),
00069 linearAcceleration( other.linearAcceleration ),
00070 angularRatePerturbation( other.angularRatePerturbation ),
00071 linearAccelerationPerturbation( other.linearAccelerationPerturbation ),
00072 gyroBiasPerturbation( other.gyroBiasPerturbation ),
00073 accBiasPerturbation( other.accBiasPerturbation ),
00074 angularRateCovariance( other.angularRateCovariance ),
00075 linearAccelerationCovariance( other.linearAccelerationCovariance ),
00076 gyroBiasCovariance( other.gyroBiasCovariance ),
00077 accBiasCovariance( other.accBiasCovariance )
00078 {
00079 }
00080
00084 void setAngularRateCovariance( double v )
00085 {
00086 angularRateCovariance( 0, 0 ) = v;
00087 angularRateCovariance( 1, 1 ) = v;
00088 angularRateCovariance( 2, 2 ) = v;
00089 }
00090
00094 void setLinearAccelerationCovariance( double v )
00095 {
00096 linearAccelerationCovariance( 0, 0 ) = v;
00097 linearAccelerationCovariance( 1, 1 ) = v;
00098 linearAccelerationCovariance( 2, 2 ) = v;
00099 }
00100
00104 void setGyroBiasCovariance( double v_x, double v_y, double v_z )
00105 {
00106 gyroBiasCovariance( 0, 0 ) = v_x;
00107 gyroBiasCovariance( 1, 1 ) = v_y;
00108 gyroBiasCovariance( 2, 2 ) = v_z;
00109 }
00110
00114 void setAccBiasCovariance( double v_x, double v_y, double v_z )
00115 {
00116 accBiasCovariance( 0, 0 ) = v_x;
00117 accBiasCovariance( 1, 1 ) = v_y;
00118 accBiasCovariance( 2, 2 ) = v_z;
00119 }
00120
00121 Eigen::Vector3d angularRate;
00123 Eigen::Vector3d linearAcceleration;
00125 Eigen::Vector3d angularRatePerturbation;
00126 Eigen::Vector3d linearAccelerationPerturbation;
00127 Eigen::Vector3d gyroBiasPerturbation;
00128 Eigen::Vector3d accBiasPerturbation;
00130 Eigen::Matrix3d angularRateCovariance;
00131 Eigen::Matrix3d linearAccelerationCovariance;
00132 Eigen::Matrix3d gyroBiasCovariance;
00133 Eigen::Matrix3d accBiasCovariance;
00134 };
00135
00136 }
00137
00138 #endif