class
PropagatorPerforms the state covariance and mean propagation using imu measurements.
Contents
We will first select what measurements we need to propagate with. We then compute the state transition matrix at each step and update the state and covariance. For derivations look at IMU Propagation Derivations page which has detailed equations.
Public static functions
-
static auto select_imu_readings(const std::vector<ov_core::
ImuData>& imu_data, double time0, double time1, bool warn = true) -> std::vector<ov_core:: ImuData> - Helper function that given current imu data, will select imu readings between the two times.
-
static auto interpolate_data(const ov_core::
ImuData& imu_1, const ov_core:: ImuData& imu_2, double timestamp) -> ov_core:: ImuData - Nice helper function that will linearly interpolate between two imu messages.
Constructors, destructors, conversion operators
- Propagator(NoiseManager noises, double gravity_mag)
- Default constructor.
Public functions
-
void feed_imu(const ov_core::
ImuData& message, double oldest_time = -1) - Stores incoming inertial readings.
- void clean_old_imu_measurements(double oldest_time)
- This will remove any IMU measurements that are older then the given measurement time.
- void propagate_and_clone(std::shared_ptr<State> state, double timestamp)
- Propagate state up to given timestamp and then clone.
- auto fast_state_propagate(std::shared_ptr<State> state, double timestamp, Eigen::Matrix<double, 13, 1>& state_plus, Eigen::Matrix<double, 12, 12>& covariance) -> bool
- Gets what the state and its covariance will be at a given timestamp.
Protected functions
-
void predict_and_compute(std::shared_ptr<State> state,
const ov_core::
ImuData& data_minus, const ov_core:: ImuData& data_plus, Eigen::Matrix<double, 15, 15>& F, Eigen::Matrix<double, 15, 15>& Qd) - Propagates the state forward using the imu data and computes the noise covariance and state-transition matrix of this interval.
- void predict_mean_discrete(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat1, const Eigen::Vector3d& a_hat1, const Eigen::Vector3d& w_hat2, const Eigen::Vector3d& a_hat2, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p)
- Discrete imu mean propagation.
- void predict_mean_rk4(std::shared_ptr<State> state, double dt, const Eigen::Vector3d& w_hat1, const Eigen::Vector3d& a_hat1, const Eigen::Vector3d& w_hat2, const Eigen::Vector3d& a_hat2, Eigen::Vector4d& new_q, Eigen::Vector3d& new_v, Eigen::Vector3d& new_p)
- RK4 imu mean propagation.
Protected variables
- double last_prop_time_offset
- Estimate for time offset at last propagation time.
- NoiseManager _noises
- Container for the noise values.
-
std::vector<ov_core::
ImuData> imu_data - Our history of IMU messages (time, angular, linear)
- Eigen::Vector3d _gravity
- Gravity vector.
Function documentation
static std::vector<ov_core:: ImuData> ov_msckf:: Propagator:: select_imu_readings(const std::vector<ov_core:: ImuData>& imu_data,
double time0,
double time1,
bool warn = true)
Helper function that given current imu data, will select imu readings between the two times.
Parameters | |
---|---|
imu_data | IMU data we will select measurements from |
time0 | Start timestamp |
time1 | End timestamp |
warn | If we should warn if we don't have enough IMU to propagate with (e.g. fast prop will get warnings otherwise) |
Returns | Vector of measurements (if we could compute them) |
This will create measurements that we will integrate with, and an extra measurement at the end. We use the interpolate_
static ov_core:: ImuData ov_msckf:: Propagator:: interpolate_data(const ov_core:: ImuData& imu_1,
const ov_core:: ImuData& imu_2,
double timestamp)
Nice helper function that will linearly interpolate between two imu messages.
Parameters | |
---|---|
imu_1 | imu at begining of interpolation interval |
imu_2 | imu at end of interpolation interval |
timestamp | Timestamp being interpolated to |
This should be used instead of just "cutting" imu messages that bound the camera times Give better time offset if we use this function, could try other orders/splines if the imu is slow.
ov_msckf:: Propagator:: Propagator(NoiseManager noises,
double gravity_mag)
Default constructor.
Parameters | |
---|---|
noises | imu noise characteristics (continuous time) |
gravity_mag | Global gravity magnitude of the system (normally 9.81) |
void ov_msckf:: Propagator:: feed_imu(const ov_core:: ImuData& message,
double oldest_time = -1)
Stores incoming inertial readings.
Parameters | |
---|---|
message | Contains our timestamp and inertial information |
oldest_time | Time that we can discard measurements before (in IMU clock) |
void ov_msckf:: Propagator:: clean_old_imu_measurements(double oldest_time)
This will remove any IMU measurements that are older then the given measurement time.
Parameters | |
---|---|
oldest_time | Time that we can discard measurements before (in IMU clock) |
void ov_msckf:: Propagator:: propagate_and_clone(std::shared_ptr<State> state,
double timestamp)
Propagate state up to given timestamp and then clone.
Parameters | |
---|---|
state | Pointer to state |
timestamp | Time to propagate to and clone at (CAM clock frame) |
This will first collect all imu readings that occured between the current state time and the new time we want the state to be at. If we don't have any imu readings we will try to extrapolate into the future. After propagating the mean and covariance using our dynamics, We clone the current imu pose as a new clone in our state.
bool ov_msckf:: Propagator:: fast_state_propagate(std::shared_ptr<State> state,
double timestamp,
Eigen::Matrix<double, 13, 1>& state_plus,
Eigen::Matrix<double, 12, 12>& covariance)
Gets what the state and its covariance will be at a given timestamp.
Parameters | |
---|---|
state | Pointer to state |
timestamp | Time to propagate to (IMU clock frame) |
state_plus | The propagated state (q_GtoI, p_IinG, v_IinI, w_IinI) |
covariance | The propagated covariance (q_GtoI, p_IinG, v_IinI, w_IinI) |
Returns | True if we were able to propagate the state to the current timestep |
This can be used to find what the state will be in the "future" without propagating it. This will propagate a clone of the current IMU state and its covariance matrix. This is typically used to provide high frequency pose estimates between updates.
void ov_msckf:: Propagator:: predict_and_compute(std::shared_ptr<State> state,
const ov_core:: ImuData& data_minus,
const ov_core:: ImuData& data_plus,
Eigen::Matrix<double, 15, 15>& F,
Eigen::Matrix<double, 15, 15>& Qd) protected
Propagates the state forward using the imu data and computes the noise covariance and state-transition matrix of this interval.
Parameters | |
---|---|
state | Pointer to state |
data_minus | imu readings at beginning of interval |
data_plus | imu readings at end of interval |
F | State-transition matrix over the interval |
Qd | Discrete-time noise covariance over the interval |
This function can be replaced with analytical/numerical integration or when using a different state representation. This contains our state transition matrix along with how our noise evolves in time. If you have other state variables besides the IMU that evolve you would add them here. See the Discrete-time Error-state Propagation page for details on how this was derived.
void ov_msckf:: Propagator:: predict_mean_discrete(std::shared_ptr<State> state,
double dt,
const Eigen::Vector3d& w_hat1,
const Eigen::Vector3d& a_hat1,
const Eigen::Vector3d& w_hat2,
const Eigen::Vector3d& a_hat2,
Eigen::Vector4d& new_q,
Eigen::Vector3d& new_v,
Eigen::Vector3d& new_p) protected
Discrete imu mean propagation.
Parameters | |
---|---|
state | Pointer to state |
dt | Time we should integrate over |
w_hat1 | Angular velocity with bias removed |
a_hat1 | Linear acceleration with bias removed |
w_hat2 | Next angular velocity with bias removed |
a_hat2 | Next linear acceleration with bias removed |
new_q | The resulting new orientation after integration |
new_v | The resulting new velocity after integration |
new_p | The resulting new position after integration |
See IMU Propagation Derivations for details on these equations.
void ov_msckf:: Propagator:: predict_mean_rk4(std::shared_ptr<State> state,
double dt,
const Eigen::Vector3d& w_hat1,
const Eigen::Vector3d& a_hat1,
const Eigen::Vector3d& w_hat2,
const Eigen::Vector3d& a_hat2,
Eigen::Vector4d& new_q,
Eigen::Vector3d& new_v,
Eigen::Vector3d& new_p) protected
RK4 imu mean propagation.
Parameters | |
---|---|
state | Pointer to state |
dt | Time we should integrate over |
w_hat1 | Angular velocity with bias removed |
a_hat1 | Linear acceleration with bias removed |
w_hat2 | Next angular velocity with bias removed |
a_hat2 | Next linear acceleration with bias removed |
new_q | The resulting new orientation after integration |
new_v | The resulting new velocity after integration |
new_p | The resulting new position after integration |
See this wikipedia page on Runge-Kutta Methods. We are doing a RK4 method, this wolframe page has the forth order equation defined below. We define function where y is a function of time t, see IMU Kinematics for the definition of the continous-time functions.