22 #ifndef OV_TYPE_TYPE_IMU_H 23 #define OV_TYPE_TYPE_IMU_H 44 _v = std::shared_ptr<Vec>(
new Vec(3));
45 _bg = std::shared_ptr<Vec>(
new Vec(3));
46 _ba = std::shared_ptr<Vec>(
new Vec(3));
49 Eigen::VectorXd imu0 = Eigen::VectorXd::Zero(16, 1);
66 _pose->set_local_id(new_id);
67 _v->set_local_id(
_pose->id() + ((new_id != -1) ?
_pose->size() : 0));
68 _bg->set_local_id(
_v->id() + ((new_id != -1) ?
_v->size() : 0));
69 _ba->set_local_id(
_bg->id() + ((new_id != -1) ?
_bg->size() : 0));
78 void update(
const Eigen::VectorXd &dx)
override {
80 assert(dx.rows() ==
_size);
82 Eigen::Matrix<double, 16, 1> newX =
_value;
84 Eigen::Matrix<double, 4, 1> dq;
85 dq << .5 * dx.block(0, 0, 3, 1), 1.0;
89 newX.block(4, 0, 3, 1) += dx.block(3, 0, 3, 1);
91 newX.block(7, 0, 3, 1) += dx.block(6, 0, 3, 1);
92 newX.block(10, 0, 3, 1) += dx.block(9, 0, 3, 1);
93 newX.block(13, 0, 3, 1) += dx.block(12, 0, 3, 1);
110 std::shared_ptr<Type>
clone()
override {
111 auto Clone = std::shared_ptr<Type>(
new IMU());
112 Clone->set_value(
value());
113 Clone->set_fej(
fej());
118 if (check ==
_pose) {
120 }
else if (check ==
_pose->check_if_subvariable(check)) {
121 return _pose->check_if_subvariable(check);
122 }
else if (check ==
_v) {
124 }
else if (check ==
_bg) {
126 }
else if (check ==
_ba) {
133 Eigen::Matrix<double, 3, 3>
Rot()
const {
return _pose->Rot(); }
136 Eigen::Matrix<double, 3, 3>
Rot_fej()
const {
return _pose->Rot_fej(); }
139 Eigen::Matrix<double, 4, 1>
quat()
const {
return _pose->quat(); }
145 Eigen::Matrix<double, 3, 1>
pos()
const {
return _pose->pos(); }
148 Eigen::Matrix<double, 3, 1>
pos_fej()
const {
return _pose->pos_fej(); }
151 Eigen::Matrix<double, 3, 1>
vel()
const {
return _v->value(); }
154 Eigen::Matrix<double, 3, 1>
vel_fej()
const {
return _v->fej(); }
157 Eigen::Matrix<double, 3, 1>
bias_g()
const {
return _bg->value(); }
163 Eigen::Matrix<double, 3, 1>
bias_a()
const {
return _ba->value(); }
172 std::shared_ptr<JPLQuat>
q() {
return _pose->q(); }
175 std::shared_ptr<Vec>
p() {
return _pose->p(); }
178 std::shared_ptr<Vec>
v() {
return _v; }
181 std::shared_ptr<Vec>
bg() {
return _bg; }
184 std::shared_ptr<Vec>
ba() {
return _ba; }
191 std::shared_ptr<Vec>
_v;
205 assert(new_value.rows() == 16);
206 assert(new_value.cols() == 1);
208 _pose->set_value(new_value.block(0, 0, 7, 1));
209 _v->set_value(new_value.block(7, 0, 3, 1));
210 _bg->set_value(new_value.block(10, 0, 3, 1));
211 _ba->set_value(new_value.block(13, 0, 3, 1));
222 assert(new_value.rows() == 16);
223 assert(new_value.cols() == 1);
225 _pose->set_fej(new_value.block(0, 0, 7, 1));
226 _v->set_fej(new_value.block(7, 0, 3, 1));
227 _bg->set_fej(new_value.block(10, 0, 3, 1));
228 _ba->set_fej(new_value.block(13, 0, 3, 1));
236 #endif // OV_TYPE_TYPE_IMU_H Eigen::Matrix< double, 4, 1 > quat_multiply(const Eigen::Matrix< double, 4, 1 > &q, const Eigen::Matrix< double, 4, 1 > &p)
Multiply two JPL quaternions.
int _size
Dimension of error state.
void update(const Eigen::VectorXd &dx) override
Performs update operation using JPLQuat update for orientation, then vector updates for position...
Eigen::Matrix< double, 3, 3 > Rot_fej() const
FEJ Rotation access.
void set_value_internal(const Eigen::MatrixXd &new_value)
Sets the value of the estimate.
Eigen::Matrix< double, 3, 1 > vel_fej() const
void set_local_id(int new_id) override
Sets id used to track location of variable in the filter covariance.
Eigen::MatrixXd _fej
First-estimate.
std::shared_ptr< Type > clone() override
Create a clone of this variable.
Eigen::Matrix< double, 3, 1 > pos_fej() const
FEJ position access.
Eigen::Matrix< double, 3, 1 > bias_a() const
Accel bias access.
std::shared_ptr< Vec > bg()
Gyroscope bias access.
std::shared_ptr< Vec > _v
Velocity subvariable.
std::shared_ptr< JPLQuat > q()
Quaternion type access.
std::shared_ptr< PoseJPL > pose()
Pose type access.
Eigen::Matrix< double, 3, 1 > bias_g_fej() const
FEJ gyro bias access.
std::shared_ptr< Vec > _bg
Gyroscope bias subvariable.
Eigen::Matrix< double, 4, 1 > quatnorm(Eigen::Matrix< double, 4, 1 > q_t)
Normalizes a quaternion to make sure it is unit norm.
Eigen::Matrix< double, 3, 1 > bias_g() const
Gyro bias access.
Dynamic type system types.
Eigen::Matrix< double, 3, 1 > bias_a_fej() const
Derived Type class that implements an IMU state.
virtual const Eigen::MatrixXd & value() const
Access variable's estimate.
std::shared_ptr< Vec > ba()
Acceleration bias access.
std::shared_ptr< Vec > v()
Velocity type access.
int _id
Location of error state in covariance.
void set_fej(const Eigen::MatrixXd &new_value) override
Sets the value of the first estimate.
Eigen::Matrix< double, 3, 1 > vel() const
Velocity access.
void set_value(const Eigen::MatrixXd &new_value) override
Sets the value of the estimate.
virtual const Eigen::MatrixXd & fej() const
Access variable's first-estimate.
Derived Type class that implements a 6 d.o.f pose.
Eigen::Matrix< double, 3, 1 > pos() const
Position access.
std::shared_ptr< Type > check_if_subvariable(const std::shared_ptr< Type > check) override
Determine if pass variable is a sub-variable.
Eigen::Matrix< double, 4, 1 > quat() const
Rotation access quaternion.
Eigen::Matrix< double, 4, 1 > quat_fej() const
FEJ Rotation access quaternion.
Eigen::Matrix< double, 3, 3 > Rot() const
Rotation access.
Base class for estimated variables.
Eigen::MatrixXd _value
Current best estimate.
std::shared_ptr< Vec > p()
Position type access.
std::shared_ptr< PoseJPL > _pose
Pose subvariable.
void set_fej_internal(const Eigen::MatrixXd &new_value)
Sets the value of the first estimate.
std::shared_ptr< Vec > _ba
Acceleration bias subvariable.
Derived Type class that implements vector variables.