imu_model.h
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #ifndef HECTOR_POSE_ESTIMATION_IMU_MODEL_H
30 #define HECTOR_POSE_ESTIMATION_IMU_MODEL_H
31 
34 
36 
37 namespace hector_pose_estimation {
38 
39 class GyroModel : public TimeContinuousSystemModel_<GyroModel,3>
40 {
41 public:
43 
44  GyroModel();
45  virtual ~GyroModel();
46 
47  bool init(PoseEstimation& estimator, System &system, State& state);
48  void getPrior(State &state);
49 
50  ColumnVector3 getError() const { return bias_->getVector(); }
51 
52  ColumnVector3 getRate(const ImuInput::RateType& imu_rate, const State& state) const;
53  void getRateJacobian(SystemMatrixBlock& C, const State& state, bool init = true);
54  void getRateNoise(CovarianceBlock Q, const State& state, bool init = true);
55 
57  void getSystemNoise(NoiseVariance& Q, const State& state, bool init = true);
58 
59 private:
60  typename SubState::Ptr bias_;
61  double rate_stddev_;
62  double rate_drift_;
63 };
64 
65 class AccelerometerModel : public TimeContinuousSystemModel_<AccelerometerModel,3>
66 {
67 public:
69 
71  virtual ~AccelerometerModel();
72 
73  bool init(PoseEstimation& estimator, System &system, State& state);
74  void getPrior(State &state);
75 
76  ColumnVector3 getError() const { return bias_->getVector(); }
77 
78  ColumnVector3 getAcceleration(const ImuInput::AccelerationType& imu_acceleration, const State& state) const;
79  void getAccelerationJacobian(SystemMatrixBlock& C, const State& state, bool init = true);
80  void getAccelerationNoise(CovarianceBlock Q, const State& state, bool init = true);
81 
83  void getSystemNoise(NoiseVariance& Q, const State& state, bool init = true);
84 
85 private:
86  typename SubState::Ptr bias_;
89 };
90 
92 extern template class System_<GyroModel>;
93 
95 extern template class System_<AccelerometerModel>;
96 
97 }
98 
99 #endif // HECTOR_POSE_ESTIMATION_IMU_MODEL_H
ColumnVector3 getError() const
Definition: imu_model.h:50
void getSystemNoise(NoiseVariance &Q, const State &state, bool init=true)
Definition: imu_model.cpp:62
void getRateNoise(CovarianceBlock Q, const State &state, bool init=true)
Definition: imu_model.cpp:80
System_< GyroModel > Gyro
Definition: imu_model.h:91
bool init(PoseEstimation &estimator, System &system, State &state)
Definition: imu_model.cpp:51
Vector::ConstFixedSegmentReturnType< 3 >::Type AccelerationType
Definition: imu_input.h:50
System_< AccelerometerModel > Accelerometer
Definition: imu_model.h:94
ColumnVector3 getRate(const ImuInput::RateType &imu_rate, const State &state) const
Definition: imu_model.cpp:69
void getRateJacobian(SystemMatrixBlock &C, const State &state, bool init=true)
Definition: imu_model.cpp:74
ColumnVector_< 3 >::type ColumnVector3
Definition: matrix.h:59
Vector::ConstFixedSegmentReturnType< 3 >::Type RateType
Definition: imu_input.h:51


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Thu Feb 18 2021 03:29:30