kr_attitude_eskf is an implementation of the Error-State Kalman Filter described in:
- “Attitude Error Representations for Kalman Filtering” - F. Landis Markley
- Removed the reference calib option, which produced somewhat poor results anyways.
- Mag calib code now uses either float or double.
- Removed kr_math, eigen_conversions dependencies.
- AttitudeESKF accepts matrices for covariance parameters, instead of assuming diagonal noise.
- Noise parameters are not provided by
rosparamanymore, and are expected to arrive in the incoming messages.
- Process model treats gyro covariance with the correct units (rad/s)^2 instead of (rad)^2.
- Magnetic bias/scale/reference vectors are loaded as lists instead of key-value arrays.
- Magnetometer calibration now uses least squares for initial guess, and LM iteration is robustified with Cauchy weighting.
- Calibration is triggered by a callable ROS service, instead of from the launch file options.
- Added support for
- Refactored ROS code into a Node class.
- Pose is always published.
- First public release.
This implementation utilizes accelerometer, gyroscope, and (optionally) the magnetometer in order to provide an attitude estimate. Gyroscope biases are estimated online with a moving average filter.
A 4th order Runge Kutta process model is employed, along with a classical EKF update - save for the error-state modifications. Internally, the filter uses a quaternion parameterization of SO(3), thereby avoiding the uglier qualities of euler angles.
The core ESKF code depends on Eigen. The ROS node + magnetometer calibrator depends on kr_math also.
The ROS implementation exposes several parameters:
||If true, magnetometer readings are included in the state estimation update.||
||Bias of the magnetometer.||
||Scale of the magnetometer.||
||World frame reference vector of the magnetometer.||
||'Fudge factor' to multiply by incoming gyro covariances.||1|
||Threshold of motion below which we may estimate gyro bias.||0.01 rad/s|
When using the node, you should remap
~field to the appropriate topics. See
attitude_eskf.launch for an example.
The following topics are always published:
||Filter orientation, covariance, IMU acceleration, and angular velocity (with bias subtracted).|
||Current estimate of gyroscope bias.|
||Status of filter. See
||Stamped pose, with translation component zeroed.|
enable_magnetometer is set, the following topics will also be published:
||Magnetic field, with bias and scale estimates applied.|
This node includes a built-in magnetometer calibration mode. Calibration is started by calling the
begin_calibration service. You will observe a log message on
rosout indicating that calibration has started. Steps:
- Rotate the device about the world z axis 360 degrees.
- Rotate the device 90 degrees about the roll/pitch axis, and then again 360 degrees about the world z axis.
- You should see the resulting parameters printed on
rosout. They will also be saved to
- After calibration,
enable_magnetometerwill be set to true and the magnetic field will be included in the estimate.
Important Operating Notes
- It is assumed that all IMU topics are strictly synchronized.
- It is assumed that all IMU sensors are physically aligned.
- The default value of
mag_calib/referenceis not valid. You must either calibrate the device, or set a known value using a magnetic model (such as IGRF11).
Please report issues to the official package maintainer.