Repository: EliaTarasov/ESKF
Branch: master
Commit: 439cc566f73f
Files: 11
Total size: 183.8 KB
Directory structure:
gitextract_sg1e4u5d/
├── CMakeLists.txt
├── README.md
├── include/
│ └── eskf/
│ ├── ESKF.hpp
│ ├── Node.hpp
│ ├── RingBuffer.hpp
│ └── common.h
├── launch/
│ └── eskf.launch
├── package.xml
└── src/
├── ESKF.cpp
├── Node.cpp
└── eskf.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(eskf)
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
sensor_msgs
message_filters
message_generation
eigen_conversions
mavros_msgs
)
find_package(cmake_modules)
find_package(Eigen3 REQUIRED)
catkin_package(
CATKIN_DEPENDS message_runtime
)
# include headers
include_directories(
${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
${mavros_INCLUDE_DIRS}
include
include/eskf
)
# build shared library
add_library(ESKF
src/ESKF.cpp
)
set(RELEVANT_LIBRARIES
ESKF
${catkin_LIBRARIES}
${mavros_LIBRARIES}
)
# rigorous error checking
add_definitions("-Wall -Werror -O3 -std=c++11")
# build main filter
add_executable(${PROJECT_NAME}
src/eskf.cpp
src/Node.cpp
)
target_link_libraries(${PROJECT_NAME} ${RELEVANT_LIBRARIES})
# ensures messages are generated before hand
add_dependencies(${PROJECT_NAME}
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
================================================
FILE: README.md
================================================
# ESKF
ROS Error-State Kalman Filter based on PX4/ecl. Performs GPS/Magnetometer/Vision Pose/Optical Flow/RangeFinder fusion with IMU
# Description
Multisensor fusion ROS node with delayed time horizon based on EKF2.
# Building ESKF
Prerequisites:
* Eigen - https://bitbucket.org/eigen/eigen
* Mavros - https://github.com/mavlink/mavros
Steps:
1. Clone repository in your `catkin` workspace -`git clone https://github.com/EliaTarasov/ESKF.git`
2. Run `catkin_make`
3. Edit launch file `launch/eskf.launch` according to topics you want to subscribe.
4. Run ROS node which provides subscribed topics.
5. In case you want to use magnetometer for attitude correction make sure IMU has at least 3-4 times higher rate than magnetometer.
If not run `rosrun mavros mavcmd long 511 31 4000 0 0 0 0 0`.
6. Run `roslaunch eskf eskf.launch` to start eskf node.
7. Run `echo /eskf/pose` to display results.
================================================
FILE: include/eskf/ESKF.hpp
================================================
#ifndef ESKF_H_
#define ESKF_H_
#include <common.h>
#include <RingBuffer.hpp>
namespace eskf {
class ESKF {
public:
static constexpr int k_num_states_ = 22;
ESKF();
void setFusionMask(int fusion_mask);
void run(const vec3& w, const vec3& a, uint64_t time_us, scalar_t dt);
void updateVision(const quat& q, const vec3& p, uint64_t time_us, scalar_t dt);
void updateGps(const vec3& v, const vec3& p, uint64_t time_us, scalar_t dt);
void updateOpticalFlow(const vec2& int_xy, const vec2& int_xy_gyro, uint32_t integration_time_us, scalar_t distance, uint8_t quality, uint64_t time_us, scalar_t dt);
void updateRangeFinder(scalar_t range, uint64_t time_us, scalar_t dt);
void updateMagnetometer(const vec3& m, uint64_t time_us, scalar_t dt);
void updateLandedState(uint8_t landed_state);
quat getQuat();
vec3 getPosition();
vec3 getVelocity();
private:
void constrainStates();
bool initializeFilter();
void initialiseQuatCovariances(const vec3& rot_vec_var);
void zeroRows(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last);
void zeroCols(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last);
void makeSymmetrical(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last);
void setDiag(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last, scalar_t variance);
void fuse(scalar_t *K, scalar_t innovation);
void fixCovarianceErrors();
void initialiseCovariance();
void predictCovariance();
void fuseVelPosHeight();
void resetHeight();
void resetPosition();
void resetVelocity();
void fuseHeading();
void fuseOptFlow();
void fuseHagl();
bool initHagl();
void runTerrainEstimator();
void controlFusionModes();
void controlMagFusion();
void controlOpticalFlowFusion();
void controlGpsFusion();
void controlVelPosFusion();
void controlExternalVisionFusion();
void controlHeightSensorTimeouts();
bool calcOptFlowBodyRateComp();
bool collect_imu(imuSample& imu);
///< state vector
state state_;
///< FIFO buffers
RingBuffer<imuSample> imu_buffer_;
RingBuffer<extVisionSample> ext_vision_buffer_;
RingBuffer<gpsSample> gps_buffer_;
RingBuffer<rangeSample> range_buffer_;
RingBuffer<optFlowSample> opt_flow_buffer_;
RingBuffer<magSample> mag_buffer_;
///< FIFO buffers lengths
const int obs_buffer_length_ {9};
const int imu_buffer_length_ {15};
///< delayed samples
imuSample imu_sample_delayed_ {}; // captures the imu sample on the delayed time horizon
extVisionSample ev_sample_delayed_ {}; // captures the external vision sample on the delayed time horizon
gpsSample gps_sample_delayed_ {}; // captures the gps sample on the delayed time horizon
rangeSample range_sample_delayed_{}; // captures the range sample on the delayed time horizon
optFlowSample opt_flow_sample_delayed_ {}; // captures the optical flow sample on the delayed time horizon
magSample mag_sample_delayed_ {}; //captures magnetometer sample on the delayed time horizon
///< new samples
imuSample imu_sample_new_ {}; ///< imu sample capturing the newest imu data
///< downsampled
imuSample imu_down_sampled_ {}; ///< down sampled imu data (sensor rate -> filter update rate)
quat q_down_sampled_; ///< down sampled rotation data (sensor rate -> filter update rate)
///< masks
int gps_check_mask = MASK_GPS_NSATS && MASK_GPS_GDOP && MASK_GPS_HACC && MASK_GPS_VACC && MASK_GPS_SACC && MASK_GPS_HDRIFT && MASK_GPS_VDRIFT && MASK_GPS_HSPD && MASK_GPS_VSPD; ///< GPS checks by default
int fusion_mask_ = MASK_EV_POS && MASK_EV_YAW && MASK_EV_HGT; /// < ekf fusion mask (see launch file), vision pose set by default
///< timestamps
uint64_t time_last_opt_flow_ {0};
uint64_t time_last_ext_vision_ {0};
uint64_t time_last_gps_ {0};
uint64_t time_last_imu_ {0};
uint64_t time_last_mag_ {0};
uint64_t time_last_hgt_fuse_ {0};
uint64_t time_last_range_ {0};
uint64_t time_last_fake_gps_ {0};
uint64_t time_last_hagl_fuse_{0}; ///< last system time that the hagl measurement failed it's checks (uSec)
uint64_t time_acc_bias_check_{0};
///< sensors delays
scalar_t gps_delay_ms_ {110.0f}; ///< gps measurement delay relative to the IMU (mSec)
scalar_t ev_delay_ms_ {100.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
scalar_t flow_delay_ms_ {5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
scalar_t range_delay_ms_{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
scalar_t mag_delay_ms_{0.0f};
///< frames rotations
mat3 R_to_earth_; ///< Rotation (DCM) from FRD to NED
/**
* Quaternion for rotation between ENU and NED frames
*
* NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
*/
const quat q_NED2ENU = quat(0, 0.70711, 0.70711, 0);
/**
* Quaternion for rotation between body FLU and body FRD frames
*
* FLU to FRD: +PI rotation about X(forward)
* FRD to FLU: -PI rotation about X(forward)
*/
const quat q_FLU2FRD = quat(0, 1, 0, 0);
///< filter times
const unsigned FILTER_UPDATE_PERIOD_MS = 12; ///< ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
scalar_t dt_ekf_avg_ {0.001f * FILTER_UPDATE_PERIOD_MS}; ///< average update rate of the ekf
scalar_t imu_collection_time_adj_ {0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
unsigned min_obs_interval_us_ {0}; // minimum time interval between observations that will guarantee data is not lost (usec)
///< filter initialisation
bool NED_origin_initialised_; ///< true when the NED origin has been initialised
bool terrain_initialised_; ///< true when the terrain estimator has been intialised
bool filter_initialised_; ///< true when the filter has been initialised
///< Covariance
scalar_t P_[k_num_states_][k_num_states_]; ///< System covariance matrix
// process noise
const scalar_t gyro_bias_p_noise_ {1.0e-3}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
const scalar_t accel_bias_p_noise_ {3.0e-3}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
// input noise
const scalar_t gyro_noise_ {1.5e-2}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
const scalar_t accel_noise_ {3.5e-1}; ///< IMU acceleration noise use for covariance prediction (m/sec**2)
///< Measurement (observation) noise
const scalar_t range_noise_{0.1f}; ///< observation noise for range finder measurements (m)
const scalar_t flow_noise_ {0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
const scalar_t gps_vel_noise_ {5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
const scalar_t gps_pos_noise_ {0.5f}; ///< minimum allowed observation noise for gps position fusion (m)
const scalar_t pos_noaid_noise_ {10.0f}; ///< observation noise for non-aiding position fusion (m)
const scalar_t vel_noise_ {0.5f}; ///< minimum allowed observation noise for velocity fusion (m/sec)
const scalar_t pos_noise_ {0.5f}; ///< minimum allowed observation noise for position fusion (m)
const scalar_t terrain_p_noise_{5.0f}; ///< process noise for terrain offset (m/sec)
scalar_t posObsNoiseNE_ {0.0f}; ///< 1-STD observtion noise used for the fusion of NE position data (m)
///< Measurement (observation) variance
scalar_t terrain_var_{1e4f}; ///< variance of terrain position estimate (m**2)
vec2 velObsVarNE_; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2
///< initialization errors
const scalar_t switch_on_gyro_bias_ {0.01f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
const scalar_t switch_on_accel_bias_ {0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
const scalar_t initial_tilt_err_ {0.01f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
///< innovation gates
const scalar_t range_innov_gate_{5.0f}; ///< range finder fusion innovation consistency gate size (STD)
const scalar_t range_aid_innov_gate_{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
const scalar_t flow_innov_gate_{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
scalar_t heading_innov_gate_ {2.6f}; ///< heading fusion innovation consistency gate size (STD)
const scalar_t posNE_innov_gate_ {5.0f}; ///< GPS horizontal position innovation consistency gate size (STD)
const scalar_t vel_innov_gate_ {5.0f}; ///< GPS velocity innovation consistency gate size (STD)
scalar_t hvelInnovGate_ {1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check
scalar_t posInnovGateNE_ {1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check
///< innovations
scalar_t heading_innov_ {0.0f}; ///< innovation of the last heading measurement (rad)
scalar_t hagl_innov_{0.0f}; ///< innovation of the last height above terrain measurement (m)
scalar_t flow_innov_[2] {}; ///< flow measurement innovation (rad/sec)
scalar_t vel_pos_innov_[6] {}; ///< velocity and position innovations: (m/s and m)
///< innovation variances
scalar_t hagl_innov_var_{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
scalar_t vel_pos_innov_var_[6] {}; ///< velocity and position innovation variances: (m**2)
scalar_t flow_innov_var_[2] {}; ///< flow innovation variance ((rad/sec)**2)
///< test ratios
scalar_t terr_test_ratio_{0.0f}; // height above terrain measurement innovation consistency check ratio
scalar_t vel_pos_test_ratio_[6] {}; // velocity and position innovation consistency check ratios
///< range specific params
const scalar_t rng_gnd_clearance_{0.1f}; ///< minimum valid value for range when on ground (m)
const scalar_t rng_sens_pitch_{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
scalar_t range_noise_scaler_{0.0f}; ///< scaling from range measurement to noise (m/m)
scalar_t vehicle_variance_scaler_{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
const scalar_t max_hagl_for_range_aid_{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
const scalar_t max_vel_for_range_aid_{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
int32_t range_aid_{0}; ///< allow switching primary height source to range finder if certian conditions are met
const scalar_t range_cos_max_tilt_{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder data
scalar_t terrain_gradient_{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
scalar_t terrain_vpos_{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
scalar_t sin_tilt_rng_{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis
scalar_t cos_tilt_rng_{1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis
scalar_t R_rng_to_earth_2_2_{0.0f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame
bool hagl_valid_{false}; ///< true when the height above ground estimate is valid
///< optical flow specific params
const scalar_t flow_max_rate_ {2.5}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
int32_t flow_quality_min_ {1};
const scalar_t flow_noise_qual_min_ {0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
vec2 flow_rad_xy_comp_ {};
vec3 flow_gyro_bias_; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
vec3 imu_del_ang_of_; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
scalar_t delta_time_of_{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
///< mag specific params
scalar_t mag_declination_{0.0f};
scalar_t mag_heading_noise_{3.0e-1f};
scalar_t mage_p_noise_{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
scalar_t magb_p_noise_{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
scalar_t mag_noise_{5.0e-2f}; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
bool imu_updated_;
vec3 last_known_posNED_;
static constexpr scalar_t kOneG = 9.80665; /// Earth gravity (m/s^2)
static constexpr scalar_t acc_bias_lim = 0.4; ///< maximum accel bias magnitude (m/sec**2)
static constexpr scalar_t hgt_reset_lim = 0.0f; ///<
static constexpr scalar_t CONSTANTS_ONE_G = 9.80665f; // m/s^2
bool mag_use_inhibit_{false}; ///< true when magnetomer use is being inhibited
bool mag_use_inhibit_prev_{false}; ///< true when magnetomer use was being inhibited the previous frame
scalar_t last_static_yaw_{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
///< flags on receive updates
bool gps_data_ready_ = false;
bool vision_data_ready_ = false;
bool range_data_ready_ = false;
bool flow_data_ready_ = false;
bool mag_data_ready_ = false;
///< flags on fusion modes
bool fuse_pos_ = false;
bool fuse_height_ = false;
bool mag_hdg_ = false;
bool mag_3D_ = false;
bool opt_flow_ = false;
bool ev_pos_ = false;
bool ev_yaw_ = false;
bool ev_hgt_ = false;
bool gps_pos_ = false;
bool gps_vel_ = false;
bool gps_hgt_ = false;
bool fuse_hor_vel_ = false;
bool fuse_vert_vel_ = false;
bool rng_hgt_ = false;
///< flags on vehicle's state
bool in_air_ = false;
bool vehicle_at_rest_ = !in_air_; // true when the vehicle is at rest
bool vehicle_at_rest_prev_ {false}; ///< true when the vehicle was at rest the previous time the status was checked
};
}
#endif /* defined(ESKF_H_) */
================================================
FILE: include/eskf/Node.hpp
================================================
#ifndef ESKF_NODE_HPP_
#define ESKF_NODE_HPP_
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <mavros_msgs/ExtendedState.h>
#include <message_filters/subscriber.h>
#include <eskf/ESKF.hpp>
namespace eskf {
class Node {
public:
static constexpr int default_publish_rate_ = 100;
static constexpr int default_fusion_mask_ = MASK_EV;
Node(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
private:
ros::NodeHandle nh_;
// publishers
ros::Publisher pubPose_;
// subsribers
ros::Subscriber subImu_;
ros::Subscriber subVisionPose_;
ros::Subscriber subGpsPose_;
ros::Subscriber subOpticalFlowPose_;
ros::Subscriber subMagPose_;
ros::Subscriber subExtendedState_;
ros::Subscriber subRangeFinderPose_;
// implementation
eskf::ESKF eskf_;
ros::Time prevStampImu_;
ros::Time prevStampVisionPose_;
ros::Time prevStampGpsPose_;
ros::Time prevStampOpticalFlowPose_;
ros::Time prevStampMagPose_;
ros::Time prevStampRangeFinderPose_;
ros::Timer pubTimer_;
bool init_;
// callbacks
void inputCallback(const sensor_msgs::ImuConstPtr&);
void visionCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr&);
void gpsCallback(const nav_msgs::OdometryConstPtr&);
void opticalFlowCallback(const mavros_msgs::OpticalFlowRadConstPtr&);
void magCallback(const sensor_msgs::MagneticFieldConstPtr&);
void extendedStateCallback(const mavros_msgs::ExtendedStateConstPtr&);
void rangeFinderCallback(const sensor_msgs::RangeConstPtr&);
void publishState(const ros::TimerEvent&);
};
} // namespace eskf
#endif // ESKF_NODE_HPP_
================================================
FILE: include/eskf/RingBuffer.hpp
================================================
#ifndef RINGBUFFER_H_
#define RINGBUFFER_H_
#include <cstdlib>
namespace eskf {
template <typename data_type>
class RingBuffer {
public:
RingBuffer() {
_buffer = NULL;
_head = _tail = _size = 0;
_first_write = true;
}
~RingBuffer() { delete[] _buffer; }
bool allocate(int size) {
if (size <= 0) {
return false;
}
if (_buffer != NULL) {
delete[] _buffer;
}
_buffer = new data_type[size];
if (_buffer == NULL) {
return false;
}
_size = size;
// set the time elements to zero so that bad data is not retrieved from the buffers
for (unsigned index = 0; index < _size; index++) {
_buffer[index].time_us = 0;
}
_first_write = true;
return true;
}
void unallocate() {
if (_buffer != NULL) {
delete[] _buffer;
}
}
inline void push(data_type sample) {
int head_new = _head;
if (_first_write) {
head_new = _head;
} else {
head_new = (_head + 1) % _size;
}
_buffer[head_new] = sample;
_head = head_new;
// move tail if we overwrite it
if (_head == _tail && !_first_write) {
_tail = (_tail + 1) % _size;
} else {
_first_write = false;
}
}
inline data_type get_oldest() {
return _buffer[_tail];
}
unsigned get_oldest_index() {
return _tail;
}
inline data_type get_newest() {
return _buffer[_head];
}
inline bool pop_first_older_than(uint64_t timestamp, data_type *sample) {
// start looking from newest observation data
for (unsigned i = 0; i < _size; i++) {
int index = (_head - i);
index = index < 0 ? _size + index : index;
if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < 100000) {
// TODO Re-evaluate the static cast and usage patterns
memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample));
// Now we can set the tail to the item which comes after the one we removed
// since we don't want to have any older data in the buffer
if (index == static_cast<int>(_head)) {
_tail = _head;
_first_write = true;
} else {
_tail = (index + 1) % _size;
}
_buffer[index].time_us = 0;
return true;
}
if (index == static_cast<int>(_tail)) {
// we have reached the tail and haven't got a match
return false;
}
}
return false;
}
data_type &operator[](unsigned index) {
return _buffer[index];
}
// return data at the specified index
inline data_type get_from_index(unsigned index) {
if (index >= _size) {
index = _size-1;
}
return _buffer[index];
}
// push data to the specified index
inline void push_to_index(unsigned index, data_type sample) {
if (index >= _size) {
index = _size-1;
}
_buffer[index] = sample;
}
// return the length of the buffer
unsigned get_length() {
return _size;
}
private:
data_type *_buffer;
unsigned _head, _tail, _size;
bool _first_write;
};
}
#endif /* defined(RINGBUFFER_H_) */
================================================
FILE: include/eskf/common.h
================================================
#ifndef COMMON_H_
#define COMMON_H_
#include <Eigen/Core>
#include <Eigen/Dense>
#define EV_MAX_INTERVAL 2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
#define GPS_MAX_INTERVAL 1e6 ///< Maximum allowable time interval between external gps system measurements (uSec)
#define OPTICAL_FLOW_INTERVAL 2e5 ///< Maximum allowable time interval between optical flow system measurements (uSec)
#define MAG_INTERVAL 2e5 ///< Maximum allowable time interval between mag system measurements (uSec)
#define RANGE_MAX_INTERVAL 1e6 ///< Maximum allowable time interval between rangefinder system measurements (uSec)
#define MASK_EV_POS 1<<0
#define MASK_EV_YAW 1<<1
#define MASK_EV_HGT 1<<2
#define MASK_GPS_POS 1<<3
#define MASK_GPS_VEL 1<<4
#define MASK_GPS_HGT 1<<5
#define MASK_MAG_INHIBIT 1<<6
#define MASK_OPTICAL_FLOW 1<<7
#define MASK_RANGEFINDER 1<<8
#define MASK_MAG_HEADING 1<<9
#define MASK_EV (MASK_EV_POS | MASK_EV_YAW | MASK_EV_HGT)
#define MASK_GPS (MASK_GPS_POS | MASK_GPS_VEL | MASK_GPS_HGT)
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_GDOP (1<<1)
#define MASK_GPS_HACC (1<<2)
#define MASK_GPS_VACC (1<<3)
#define MASK_GPS_SACC (1<<4)
#define MASK_GPS_HDRIFT (1<<5)
#define MASK_GPS_VDRIFT (1<<6)
#define MASK_GPS_HSPD (1<<7)
#define MASK_GPS_VSPD (1<<8)
namespace eskf {
typedef float scalar_t;
typedef Eigen::Matrix<scalar_t, 3, 1> vec3; /// Vector in R3
typedef Eigen::Matrix<scalar_t, 2, 1> vec2; /// Vector in R2
typedef Eigen::Matrix<scalar_t, 3, 3> mat3; /// Matrix in R3
typedef Eigen::Quaternion<scalar_t> quat; /// Member of S4
/* State vector:
* Attitude quaternion
* NED velocity
* NED position
* Delta Angle bias - rad
* Delta Velocity bias - m/s
*/
struct state {
quat quat_nominal; ///< quaternion defining the rotaton from NED to XYZ frame
vec3 vel; ///< NED velocity in earth frame in m/s
vec3 pos; ///< NED position in earth frame in m
vec3 gyro_bias; ///< delta angle bias estimate in rad
vec3 accel_bias; ///< delta velocity bias estimate in m/s
vec3 mag_I; ///< NED earth magnetic field in gauss
vec3 mag_B; ///< magnetometer bias estimate in body frame in gauss
};
struct imuSample {
vec3 delta_ang; ///< delta angle in body frame (integrated gyro measurements) (rad)
vec3 delta_vel; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
scalar_t delta_ang_dt; ///< delta angle integration period (sec)
scalar_t delta_vel_dt; ///< delta velocity integration period (sec)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
struct extVisionSample {
vec3 posNED; ///< measured NED body position relative to the local origin (m)
quat quatNED; ///< measured quaternion orientation defining rotation from NED to body frame
scalar_t posErr; ///< 1-Sigma spherical position accuracy (m)
scalar_t angErr; ///< 1-Sigma angular error (rad)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
struct gpsSample {
vec2 pos; ///< NE earth frame gps horizontal position measurement (m)
vec3 vel; ///< NED earth frame gps velocity measurement (m/sec)
scalar_t hgt; ///< gps height measurement (m)
scalar_t hacc; ///< 1-std horizontal position error (m)
scalar_t vacc; ///< 1-std vertical position error (m)
scalar_t sacc; ///< 1-std speed error (m/sec)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
struct optFlowSample {
uint8_t quality; ///< quality indicator between 0 and 255
vec2 flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
vec2 flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
vec2 gyroXY; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
scalar_t dt; ///< amount of integration time (sec)
uint64_t time_us; ///< timestamp of the integration period mid-point (uSec)
};
struct rangeSample {
scalar_t rng; ///< range (distance to ground) measurement (m)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
struct magSample {
vec3 mag; ///< NED magnetometer body frame measurements (Gauss)
uint64_t time_us; ///< timestamp of the measurement (uSec)
};
struct gps_message {
uint64_t time_usec;
int32_t lat; ///< Latitude in 1E-7 degrees
int32_t lon; ///< Longitude in 1E-7 degrees
int32_t alt; ///< Altitude in 1E-3 meters (millimeters) above MSL
float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float yaw_offset; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
float eph; ///< GPS horizontal position accuracy in m
float epv; ///< GPS vertical position accuracy in m
float sacc; ///< GPS speed accuracy in m/s
float vel_m_s; ///< GPS ground speed (m/sec)
float vel_ned[3]; ///< GPS ground speed NED
bool vel_ned_valid; ///< GPS ground speed is valid
uint8_t nsats; ///< number of satellites used
float gdop; ///< geometric dilution of precision
};
// publish the status of various GPS quality checks
union gps_check_fail_status_u {
struct {
uint16_t fix : 1; ///< 0 - true if the fix type is insufficient (no 3D solution)
uint16_t nsats : 1; ///< 1 - true if number of satellites used is insufficient
uint16_t gdop : 1; ///< 2 - true if geometric dilution of precision is insufficient
uint16_t hacc : 1; ///< 3 - true if reported horizontal accuracy is insufficient
uint16_t vacc : 1; ///< 4 - true if reported vertical accuracy is insufficient
uint16_t sacc : 1; ///< 5 - true if reported speed accuracy is insufficient
uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
} flags;
uint16_t value;
};
///< common math functions
template <typename T> inline T sq(T var) {
return var * var;
}
template <typename T> inline T max(T val1, T val2) {
return (val1 > val2) ? val1 : val2;
}
template<typename Scalar>
static inline constexpr const Scalar &constrain(const Scalar &val, const Scalar &min_val, const Scalar &max_val) {
return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
}
template<typename Type>
Type wrap_pi(Type x) {
while (x >= Type(M_PI)) {
x -= Type(2.0 * M_PI);
}
while (x < Type(-M_PI)) {
x += Type(2.0 * M_PI);
}
return x;
}
///< tf functions
inline quat from_axis_angle(const vec3 &axis, scalar_t theta) {
quat q;
if (theta < scalar_t(1e-10)) {
q.w() = scalar_t(1.0);
q.x() = q.y() = q.z() = 0;
}
scalar_t magnitude = sin(theta / 2.0f);
q.w() = cos(theta / 2.0f);
q.x() = axis(0) * magnitude;
q.y() = axis(1) * magnitude;
q.z() = axis(2) * magnitude;
return q;
}
inline quat from_axis_angle(vec3 vec) {
quat q;
scalar_t theta = vec.norm();
if (theta < scalar_t(1e-10)) {
q.w() = scalar_t(1.0);
q.x() = q.y() = q.z() = 0;
return q;
}
vec3 tmp = vec / theta;
return from_axis_angle(tmp, theta);
}
inline vec3 to_axis_angle(const quat& q) {
scalar_t axis_magnitude = scalar_t(sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z()));
vec3 vec;
vec(0) = q.x();
vec(1) = q.y();
vec(2) = q.z();
if (axis_magnitude >= scalar_t(1e-10)) {
vec = vec / axis_magnitude;
vec = vec * wrap_pi(scalar_t(2.0) * atan2(axis_magnitude, q.w()));
}
return vec;
}
inline mat3 quat2dcm(const quat& q) {
mat3 dcm;
scalar_t a = q.w();
scalar_t b = q.x();
scalar_t c = q.y();
scalar_t d = q.z();
scalar_t aSq = a * a;
scalar_t bSq = b * b;
scalar_t cSq = c * c;
scalar_t dSq = d * d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0, 1) = 2 * (b * c - a * d);
dcm(0, 2) = 2 * (a * c + b * d);
dcm(1, 0) = 2 * (b * c + a * d);
dcm(1, 1) = aSq - bSq + cSq - dSq;
dcm(1, 2) = 2 * (c * d - a * b);
dcm(2, 0) = 2 * (b * d - a * c);
dcm(2, 1) = 2 * (a * b + c * d);
dcm(2, 2) = aSq - bSq - cSq + dSq;
return dcm;
}
inline vec3 dcm2vec(const mat3& dcm) {
scalar_t phi_val = atan2(dcm(2, 1), dcm(2, 2));
scalar_t theta_val = asin(-dcm(2, 0));
scalar_t psi_val = atan2(dcm(1, 0), dcm(0, 0));
scalar_t pi = M_PI;
if (fabs(theta_val - pi / 2) < 1.0e-3) {
phi_val = 0.0;
psi_val = atan2(dcm(1, 2), dcm(0, 2));
} else if (fabs(theta_val + pi / 2) < 1.0e-3) {
phi_val = 0.0;
psi_val = atan2(-dcm(1, 2), -dcm(0, 2));
}
return vec3(phi_val, theta_val, psi_val);
}
// calculate the inverse rotation matrix from a quaternion rotation
inline mat3 quat_to_invrotmat(const quat &q) {
scalar_t q00 = q.w() * q.w();
scalar_t q11 = q.x() * q.x();
scalar_t q22 = q.y() * q.y();
scalar_t q33 = q.z() * q.z();
scalar_t q01 = q.w() * q.x();
scalar_t q02 = q.w() * q.y();
scalar_t q03 = q.w() * q.z();
scalar_t q12 = q.x() * q.y();
scalar_t q13 = q.x() * q.z();
scalar_t q23 = q.y() * q.z();
mat3 dcm;
dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = q00 - q11 - q22 + q33;
dcm(0, 1) = 2.0f * (q12 - q03);
dcm(0, 2) = 2.0f * (q13 + q02);
dcm(1, 0) = 2.0f * (q12 + q03);
dcm(1, 2) = 2.0f * (q23 - q01);
dcm(2, 0) = 2.0f * (q13 - q02);
dcm(2, 1) = 2.0f * (q23 + q01);
return dcm;
}
/**
* Constructor from euler angles
*
* This sets the instance to a quaternion representing coordinate transformation from
* frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
* by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
*
* @param euler euler angle instance
*/
inline quat from_euler(const vec3& euler) {
quat q;
scalar_t cosPhi_2 = cos(euler(0) / 2.0);
scalar_t cosTheta_2 = cos(euler(1) / 2.0);
scalar_t cosPsi_2 = cos(euler(2) / 2.0);
scalar_t sinPhi_2 = sin(euler(0) / 2.0);
scalar_t sinTheta_2 = sin(euler(1) / 2.0);
scalar_t sinPsi_2 = sin(euler(2) / 2.0);
q.w() = cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2;
q.x() = sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2;
q.y() = cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2;
q.z() = cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2;
return q;
}
}
#endif
================================================
FILE: launch/eskf.launch
================================================
<launch>
<!-- Name of the node -->
<arg name="eskf_node" default="eskf"/>
<!-- IMU topic to use -->
<arg name="imu_topic" default="/mavros/imu/data"/>
<!-- MAG topic to use -->
<arg name="mag_topic" default="/mavros/imu/mag"/>
<!-- VISION topic to use -->
<arg name="vision_topic" default="/svo/pose"/>
<!-- GPS topic to use -->
<arg name="gps_topic" default="/mavros/global_position/local"/>
<!-- OPTICAL FLOW topic to use -->
<arg name="optical_flow_topic" default="/mavros/px4flow/raw/optical_flow_rad"/>
<!-- EXTENDED STATE topic to use -->
<arg name="extended_state_topic" default="/mavros/extended_state"/>
<!-- RANGEFINDER topic to use -->
<arg name="rangefinder_topic" default="/mavros/distance_sensor/hrlv_ez4_pub"/>
<node pkg="eskf" name="$(arg eskf_node)" type="eskf" output="screen">
<remap from="~imu" to="$(arg imu_topic)"/>
<remap from="~vision" to="$(arg vision_topic)"/>
<remap from="~gps" to="$(arg gps_topic)"/>
<remap from="~optical_flow" to="$(arg optical_flow_topic)"/>
<remap from="~mag" to="$(arg mag_topic)"/>
<remap from="~extended_state" to="$(arg extended_state_topic)"/>
<remap from="~rangefinder" to="$(arg rangefinder_topic)"/>
<param name="fusion_mask" value="7" type="int"/>
<!--
1 - vision position.
2 - vision yaw.
4 - vision height.
8 - gps position.
16 - gps velocity
32 - gps height.
64 - mag inhibit.
128 - optical flow.
256 - rangefinder height.
512 - mag heading.
-->
<param name="publish_rate" value="10" type="int"/>
</node>
</launch>
================================================
FILE: package.xml
================================================
<?xml version="1.0"?>
<package>
<name>eskf</name>
<version>0.0.1</version>
<description>Error state kalman filter for position and attitude determination</description>
<maintainer email="elias.tarasov@gmail.com">etarasov</maintainer>
<author>Elia Tarasov</author>
<license>Apache 2</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>message_filters</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>eigen_conversions</build_depend>
<build_depend>mavros_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>message_filters</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>mavros_msgs</run_depend>
<export>
</export>
</package>
================================================
FILE: src/ESKF.cpp
================================================
#ifndef NDEBUG
#define NDEBUG
#endif
#include <ESKF.hpp>
#include <cmath>
using namespace Eigen;
namespace eskf {
ESKF::ESKF() {
// zeros state_
state_.quat_nominal = quat(1, 0, 0, 0);
state_.vel = vec3(0, 0, 0);
state_.pos = vec3(0, 0, 0);
state_.gyro_bias = vec3(0, 0, 0);
state_.accel_bias = vec3(0, 0, 0);
state_.mag_I.setZero();
state_.mag_B.setZero();
// zeros P_
for (unsigned i = 0; i < k_num_states_; i++) {
for (unsigned j = 0; j < k_num_states_; j++) {
P_[i][j] = 0.0f;
}
}
imu_down_sampled_.delta_ang.setZero();
imu_down_sampled_.delta_vel.setZero();
imu_down_sampled_.delta_ang_dt = 0.0f;
imu_down_sampled_.delta_vel_dt = 0.0f;
q_down_sampled_.w() = 1.0f;
q_down_sampled_.x() = 0.0f;
q_down_sampled_.y() = 0.0f;
q_down_sampled_.z() = 0.0f;
imu_buffer_.allocate(imu_buffer_length_);
for (int index = 0; index < imu_buffer_length_; index++) {
imuSample imu_sample_init = {};
imu_buffer_.push(imu_sample_init);
}
ext_vision_buffer_.allocate(obs_buffer_length_);
for (int index = 0; index < obs_buffer_length_; index++) {
extVisionSample ext_vision_sample_init = {};
ext_vision_buffer_.push(ext_vision_sample_init);
}
gps_buffer_.allocate(obs_buffer_length_);
for (int index = 0; index < obs_buffer_length_; index++) {
gpsSample gps_sample_init = {};
gps_buffer_.push(gps_sample_init);
}
opt_flow_buffer_.allocate(obs_buffer_length_);
for (int index = 0; index < obs_buffer_length_; index++) {
optFlowSample opt_flow_sample_init = {};
opt_flow_buffer_.push(opt_flow_sample_init);
}
range_buffer_.allocate(obs_buffer_length_);
for (int index = 0; index < obs_buffer_length_; index++) {
rangeSample range_sample_init = {};
range_buffer_.push(range_sample_init);
}
mag_buffer_.allocate(obs_buffer_length_);
for (int index = 0; index < obs_buffer_length_; index++) {
magSample mag_sample_init = {};
mag_buffer_.push(mag_sample_init);
}
dt_ekf_avg_ = 0.001f * (scalar_t)(FILTER_UPDATE_PERIOD_MS);
///< filter initialisation
NED_origin_initialised_ = false;
filter_initialised_ = false;
terrain_initialised_ = false;
imu_updated_ = false;
memset(vel_pos_innov_, 0, 6*sizeof(scalar_t));
last_known_posNED_ = vec3(0, 0, 0);
}
void ESKF::initialiseCovariance() {
// define the initial angle uncertainty as variances for a rotation vector
for (unsigned i = 0; i < k_num_states_; i++) {
for (unsigned j = 0; j < k_num_states_; j++) {
P_[i][j] = 0.0f;
}
}
// calculate average prediction time step in sec
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
vec3 rot_vec_var;
rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(initial_tilt_err_);
// update the quaternion state covariances
initialiseQuatCovariances(rot_vec_var);
// velocity
P_[4][4] = sq(fmaxf(vel_noise_, 0.01f));
P_[5][5] = P_[4][4];
P_[6][6] = sq(1.5f) * P_[4][4];
// position
P_[7][7] = sq(fmaxf(pos_noise_, 0.01f));
P_[8][8] = P_[7][7];
P_[9][9] = sq(fmaxf(range_noise_, 0.01f));
// gyro bias
P_[10][10] = sq(switch_on_gyro_bias_ * dt);
P_[11][11] = P_[10][10];
P_[12][12] = P_[10][10];
P_[13][13] = sq(switch_on_accel_bias_ * dt);
P_[14][14] = P_[13][13];
P_[15][15] = P_[13][13];
// variances for optional states
// earth frame and body frame magnetic field
// set to observation variance
for (uint8_t index = 16; index <= 21; index ++) {
P_[index][index] = sq(mag_noise_);
}
}
bool ESKF::initializeFilter() {
scalar_t pitch = 0.0;
scalar_t roll = 0.0;
scalar_t yaw = 0.0;
imuSample imu_init = imu_buffer_.get_newest();
static vec3 delVel_sum(0, 0, 0); ///< summed delta velocity (m/sec)
delVel_sum += imu_init.delta_vel;
if (delVel_sum.norm() > 0.001) {
delVel_sum.normalize();
pitch = asin(delVel_sum(0));
roll = atan2(-delVel_sum(1), -delVel_sum(2));
} else {
return false;
}
// calculate initial tilt alignment
state_.quat_nominal = AngleAxis<scalar_t>(yaw, vec3::UnitZ()) * AngleAxis<scalar_t>(pitch, vec3::UnitY()) * AngleAxis<scalar_t>(roll, vec3::UnitX());
// update transformation matrix from body to world frame
R_to_earth_ = quat_to_invrotmat(state_.quat_nominal);
initialiseCovariance();
return true;
}
bool ESKF::collect_imu(imuSample &imu) {
// accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long
// copy imu data to local variables
imu_sample_new_.delta_ang = imu.delta_ang;
imu_sample_new_.delta_vel = imu.delta_vel;
imu_sample_new_.delta_ang_dt = imu.delta_ang_dt;
imu_sample_new_.delta_vel_dt = imu.delta_vel_dt;
imu_sample_new_.time_us = imu.time_us;
// accumulate the time deltas
imu_down_sampled_.delta_ang_dt += imu.delta_ang_dt;
imu_down_sampled_.delta_vel_dt += imu.delta_vel_dt;
// use a quaternion to accumulate delta angle data
// this quaternion represents the rotation from the start to end of the accumulation period
quat delta_q(1, 0, 0, 0);
quat res = from_axis_angle(imu.delta_ang);
delta_q = delta_q * res;
q_down_sampled_ = q_down_sampled_ * delta_q;
q_down_sampled_.normalize();
// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
mat3 delta_R = quat2dcm(delta_q.inverse());
imu_down_sampled_.delta_vel = delta_R * imu_down_sampled_.delta_vel;
// accumulate the most recent delta velocity data at the updated rotation frame
// assume effective sample time is halfway between the previous and current rotation frame
imu_down_sampled_.delta_vel += (imu_sample_new_.delta_vel + delta_R * imu_sample_new_.delta_vel) * 0.5f;
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
scalar_t target_dt = (scalar_t)(FILTER_UPDATE_PERIOD_MS) / 1000;
if (imu_down_sampled_.delta_ang_dt >= target_dt - imu_collection_time_adj_) {
// accumulate the amount of time to advance the IMU collection time so that we meet the
// average EKF update rate requirement
imu_collection_time_adj_ += 0.01f * (imu_down_sampled_.delta_ang_dt - target_dt);
imu_collection_time_adj_ = constrain(imu_collection_time_adj_, -0.5f * target_dt, 0.5f * target_dt);
imu.delta_ang = to_axis_angle(q_down_sampled_);
imu.delta_vel = imu_down_sampled_.delta_vel;
imu.delta_ang_dt = imu_down_sampled_.delta_ang_dt;
imu.delta_vel_dt = imu_down_sampled_.delta_vel_dt;
imu_down_sampled_.delta_ang.setZero();
imu_down_sampled_.delta_vel.setZero();
imu_down_sampled_.delta_ang_dt = 0.0f;
imu_down_sampled_.delta_vel_dt = 0.0f;
q_down_sampled_.w() = 1.0f;
q_down_sampled_.x() = q_down_sampled_.y() = q_down_sampled_.z() = 0.0f;
return true;
}
min_obs_interval_us_ = (imu_sample_new_.time_us - imu_sample_delayed_.time_us) / (obs_buffer_length_ - 1);
return false;
}
void ESKF::run(const vec3 &w, const vec3 &a, uint64_t time_us, scalar_t dt) {
// convert FLU to FRD body frame IMU data
vec3 gyro_b = q_FLU2FRD.toRotationMatrix() * w;
vec3 accel_b = q_FLU2FRD.toRotationMatrix() * a;
vec3 delta_ang = vec3(gyro_b.x(), gyro_b.y(), gyro_b.z()) * dt; // current delta angle (rad)
vec3 delta_vel = vec3(accel_b.x(), accel_b.y(), accel_b.z()) * dt; //current delta velocity (m/s)
// copy data
imuSample imu_sample_new = {};
imu_sample_new.delta_ang = delta_ang;
imu_sample_new.delta_vel = delta_vel;
imu_sample_new.delta_ang_dt = dt;
imu_sample_new.delta_vel_dt = dt;
imu_sample_new.time_us = time_us;
time_last_imu_ = time_us;
if(collect_imu(imu_sample_new)) {
imu_buffer_.push(imu_sample_new);
imu_updated_ = true;
// get the oldest data from the buffer
imu_sample_delayed_ = imu_buffer_.get_oldest();
} else {
imu_updated_ = false;
return;
}
if (!filter_initialised_) {
filter_initialised_ = initializeFilter();
if (!filter_initialised_) {
return;
}
}
if(!imu_updated_) return;
// apply imu bias corrections
vec3 corrected_delta_ang = imu_sample_delayed_.delta_ang - state_.gyro_bias;
vec3 corrected_delta_vel = imu_sample_delayed_.delta_vel - state_.accel_bias;
// convert the delta angle to a delta quaternion
quat dq;
dq = from_axis_angle(corrected_delta_ang);
// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
state_.quat_nominal = state_.quat_nominal * dq;
// quaternions must be normalised whenever they are modified
state_.quat_nominal.normalize();
// save the previous value of velocity so we can use trapezoidal integration
vec3 vel_last = state_.vel;
// update transformation matrix from body to world frame
R_to_earth_ = quat_to_invrotmat(state_.quat_nominal);
// Calculate an earth frame delta velocity
vec3 corrected_delta_vel_ef = R_to_earth_ * corrected_delta_vel;
// calculate the increment in velocity using the current orientation
state_.vel += corrected_delta_vel_ef;
// compensate for acceleration due to gravity
state_.vel(2) += kOneG * imu_sample_delayed_.delta_vel_dt;
// predict position states via trapezoidal integration of velocity
state_.pos += (vel_last + state_.vel) * imu_sample_delayed_.delta_vel_dt * 0.5f;
constrainStates();
// calculate an average filter update time
scalar_t input = 0.5f * (imu_sample_delayed_.delta_vel_dt + imu_sample_delayed_.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value
input = constrain(input, 0.0005f * (scalar_t)(FILTER_UPDATE_PERIOD_MS), 0.002f * (scalar_t)(FILTER_UPDATE_PERIOD_MS));
dt_ekf_avg_ = 0.99f * dt_ekf_avg_ + 0.01f * input;
predictCovariance();
controlFusionModes();
}
void ESKF::predictCovariance() {
// error-state jacobian
// assign intermediate state variables
scalar_t q0 = state_.quat_nominal.w();
scalar_t q1 = state_.quat_nominal.x();
scalar_t q2 = state_.quat_nominal.y();
scalar_t q3 = state_.quat_nominal.z();
scalar_t dax = imu_sample_delayed_.delta_ang(0);
scalar_t day = imu_sample_delayed_.delta_ang(1);
scalar_t daz = imu_sample_delayed_.delta_ang(2);
scalar_t dvx = imu_sample_delayed_.delta_vel(0);
scalar_t dvy = imu_sample_delayed_.delta_vel(1);
scalar_t dvz = imu_sample_delayed_.delta_vel(2);
scalar_t dax_b = state_.gyro_bias(0);
scalar_t day_b = state_.gyro_bias(1);
scalar_t daz_b = state_.gyro_bias(2);
scalar_t dvx_b = state_.accel_bias(0);
scalar_t dvy_b = state_.accel_bias(1);
scalar_t dvz_b = state_.accel_bias(2);
// compute noise variance for stationary processes
scalar_t process_noise[k_num_states_] = {};
scalar_t dt = constrain(imu_sample_delayed_.delta_ang_dt, 0.0005f * (scalar_t)(FILTER_UPDATE_PERIOD_MS), 0.002f * (scalar_t)(FILTER_UPDATE_PERIOD_MS));
// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
scalar_t d_ang_bias_sig = dt * dt * constrain(gyro_bias_p_noise_, 0.0f, 1.0f);
// convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
scalar_t d_vel_bias_sig = dt * dt * constrain(accel_bias_p_noise_, 0.0f, 1.0f);
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
scalar_t mag_I_sig;
if (mag_3D_ && (P_[16][16] + P_[17][17] + P_[18][18]) < 0.1f) {
mag_I_sig = dt * constrain(mage_p_noise_, 0.0f, 1.0f);
} else {
mag_I_sig = 0.0f;
}
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
scalar_t mag_B_sig;
if (mag_3D_ && (P_[19][19] + P_[20][20] + P_[21][21]) < 0.1f) {
mag_B_sig = dt * constrain(magb_p_noise_, 0.0f, 1.0f);
} else {
mag_B_sig = 0.0f;
}
// Construct the process noise variance diagonal for those states with a stationary process model
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
for (unsigned i = 0; i <= 9; i++) {
process_noise[i] = 0.0;
}
// delta angle bias states
process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig);
// delta_velocity bias states
process_noise[15] = process_noise[14] = process_noise[13] = sq(d_vel_bias_sig);
// earth frame magnetic field states
process_noise[18] = process_noise[17] = process_noise[16] = sq(mag_I_sig);
// body frame magnetic field states
process_noise[21] = process_noise[20] = process_noise[19] = sq(mag_B_sig);
// assign IMU noise variances
// inputs to the system are 3 delta angles and 3 delta velocities
scalar_t daxVar, dayVar, dazVar;
scalar_t dvxVar, dvyVar, dvzVar;
daxVar = dayVar = dazVar = sq(dt * gyro_noise_); // gyro prediction variance TODO get variance from sensor
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise_); //accel prediction variance TODO get variance from sensor
// intermediate calculations
scalar_t SF[21];
SF[0] = dvz - dvz_b;
SF[1] = dvy - dvy_b;
SF[2] = dvx - dvx_b;
SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0];
SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2];
SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1];
SF[6] = day/2 - day_b/2;
SF[7] = daz/2 - daz_b/2;
SF[8] = dax/2 - dax_b/2;
SF[9] = dax_b/2 - dax/2;
SF[10] = daz_b/2 - daz/2;
SF[11] = day_b/2 - day/2;
SF[12] = 2*q1*SF[1];
SF[13] = 2*q0*SF[0];
SF[14] = q1/2;
SF[15] = q2/2;
SF[16] = q3/2;
SF[17] = sq(q3);
SF[18] = sq(q2);
SF[19] = sq(q1);
SF[20] = sq(q0);
scalar_t SG[8];
SG[0] = q0/2;
SG[1] = sq(q3);
SG[2] = sq(q2);
SG[3] = sq(q1);
SG[4] = sq(q0);
SG[5] = 2*q2*q3;
SG[6] = 2*q1*q3;
SG[7] = 2*q1*q2;
scalar_t SQ[11];
SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4;
SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4;
SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4;
SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4;
SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2;
SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4;
SQ[9] = sq(SG[0]);
SQ[10] = sq(q1);
scalar_t SPP[11];
SPP[0] = SF[12] + SF[13] - 2*q2*SF[2];
SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
SPP[4] = 2*q0*q2 - 2*q1*q3;
SPP[5] = 2*q0*q1 - 2*q2*q3;
SPP[6] = 2*q0*q3 - 2*q1*q2;
SPP[7] = 2*q0*q1 + 2*q2*q3;
SPP[8] = 2*q0*q3 + 2*q1*q2;
SPP[9] = 2*q0*q2 + 2*q1*q3;
SPP[10] = SF[16];
// covariance update
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
scalar_t nextP[k_num_states_][k_num_states_];
nextP[0][0] = P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P_[0][1] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10]) + SF[11]*(P_[0][2] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10]) + SF[10]*(P_[0][3] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10]) + SF[14]*(P_[0][10] + P_[1][10]*SF[9] + P_[2][10]*SF[11] + P_[3][10]*SF[10] + P_[10][10]*SF[14] + P_[11][10]*SF[15] + P_[12][10]*SPP[10]) + SF[15]*(P_[0][11] + P_[1][11]*SF[9] + P_[2][11]*SF[11] + P_[3][11]*SF[10] + P_[10][11]*SF[14] + P_[11][11]*SF[15] + P_[12][11]*SPP[10]) + SPP[10]*(P_[0][12] + P_[1][12]*SF[9] + P_[2][12]*SF[11] + P_[3][12]*SF[10] + P_[10][12]*SF[14] + P_[11][12]*SF[15] + P_[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4;
nextP[0][1] = P_[0][1] + SQ[8] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10] + SF[8]*(P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10]) + SF[7]*(P_[0][2] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10]) + SF[11]*(P_[0][3] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10]) - SF[15]*(P_[0][12] + P_[1][12]*SF[9] + P_[2][12]*SF[11] + P_[3][12]*SF[10] + P_[10][12]*SF[14] + P_[11][12]*SF[15] + P_[12][12]*SPP[10]) + SPP[10]*(P_[0][11] + P_[1][11]*SF[9] + P_[2][11]*SF[11] + P_[3][11]*SF[10] + P_[10][11]*SF[14] + P_[11][11]*SF[15] + P_[12][11]*SPP[10]) - (q0*(P_[0][10] + P_[1][10]*SF[9] + P_[2][10]*SF[11] + P_[3][10]*SF[10] + P_[10][10]*SF[14] + P_[11][10]*SF[15] + P_[12][10]*SPP[10]))/2;
nextP[1][1] = P_[1][1] + P_[0][1]*SF[8] + P_[2][1]*SF[7] + P_[3][1]*SF[11] - P_[12][1]*SF[15] + P_[11][1]*SPP[10] + daxVar*SQ[9] - (P_[10][1]*q0)/2 + SF[8]*(P_[1][0] + P_[0][0]*SF[8] + P_[2][0]*SF[7] + P_[3][0]*SF[11] - P_[12][0]*SF[15] + P_[11][0]*SPP[10] - (P_[10][0]*q0)/2) + SF[7]*(P_[1][2] + P_[0][2]*SF[8] + P_[2][2]*SF[7] + P_[3][2]*SF[11] - P_[12][2]*SF[15] + P_[11][2]*SPP[10] - (P_[10][2]*q0)/2) + SF[11]*(P_[1][3] + P_[0][3]*SF[8] + P_[2][3]*SF[7] + P_[3][3]*SF[11] - P_[12][3]*SF[15] + P_[11][3]*SPP[10] - (P_[10][3]*q0)/2) - SF[15]*(P_[1][12] + P_[0][12]*SF[8] + P_[2][12]*SF[7] + P_[3][12]*SF[11] - P_[12][12]*SF[15] + P_[11][12]*SPP[10] - (P_[10][12]*q0)/2) + SPP[10]*(P_[1][11] + P_[0][11]*SF[8] + P_[2][11]*SF[7] + P_[3][11]*SF[11] - P_[12][11]*SF[15] + P_[11][11]*SPP[10] - (P_[10][11]*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P_[1][10] + P_[0][10]*SF[8] + P_[2][10]*SF[7] + P_[3][10]*SF[11] - P_[12][10]*SF[15] + P_[11][10]*SPP[10] - (P_[10][10]*q0)/2))/2;
nextP[0][2] = P_[0][2] + SQ[7] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10] + SF[6]*(P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10]) + SF[10]*(P_[0][1] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10]) + SF[8]*(P_[0][3] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10]) + SF[14]*(P_[0][12] + P_[1][12]*SF[9] + P_[2][12]*SF[11] + P_[3][12]*SF[10] + P_[10][12]*SF[14] + P_[11][12]*SF[15] + P_[12][12]*SPP[10]) - SPP[10]*(P_[0][10] + P_[1][10]*SF[9] + P_[2][10]*SF[11] + P_[3][10]*SF[10] + P_[10][10]*SF[14] + P_[11][10]*SF[15] + P_[12][10]*SPP[10]) - (q0*(P_[0][11] + P_[1][11]*SF[9] + P_[2][11]*SF[11] + P_[3][11]*SF[10] + P_[10][11]*SF[14] + P_[11][11]*SF[15] + P_[12][11]*SPP[10]))/2;
nextP[1][2] = P_[1][2] + SQ[5] + P_[0][2]*SF[8] + P_[2][2]*SF[7] + P_[3][2]*SF[11] - P_[12][2]*SF[15] + P_[11][2]*SPP[10] - (P_[10][2]*q0)/2 + SF[6]*(P_[1][0] + P_[0][0]*SF[8] + P_[2][0]*SF[7] + P_[3][0]*SF[11] - P_[12][0]*SF[15] + P_[11][0]*SPP[10] - (P_[10][0]*q0)/2) + SF[10]*(P_[1][1] + P_[0][1]*SF[8] + P_[2][1]*SF[7] + P_[3][1]*SF[11] - P_[12][1]*SF[15] + P_[11][1]*SPP[10] - (P_[10][1]*q0)/2) + SF[8]*(P_[1][3] + P_[0][3]*SF[8] + P_[2][3]*SF[7] + P_[3][3]*SF[11] - P_[12][3]*SF[15] + P_[11][3]*SPP[10] - (P_[10][3]*q0)/2) + SF[14]*(P_[1][12] + P_[0][12]*SF[8] + P_[2][12]*SF[7] + P_[3][12]*SF[11] - P_[12][12]*SF[15] + P_[11][12]*SPP[10] - (P_[10][12]*q0)/2) - SPP[10]*(P_[1][10] + P_[0][10]*SF[8] + P_[2][10]*SF[7] + P_[3][10]*SF[11] - P_[12][10]*SF[15] + P_[11][10]*SPP[10] - (P_[10][10]*q0)/2) - (q0*(P_[1][11] + P_[0][11]*SF[8] + P_[2][11]*SF[7] + P_[3][11]*SF[11] - P_[12][11]*SF[15] + P_[11][11]*SPP[10] - (P_[10][11]*q0)/2))/2;
nextP[2][2] = P_[2][2] + P_[0][2]*SF[6] + P_[1][2]*SF[10] + P_[3][2]*SF[8] + P_[12][2]*SF[14] - P_[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P_[11][2]*q0)/2 + SF[6]*(P_[2][0] + P_[0][0]*SF[6] + P_[1][0]*SF[10] + P_[3][0]*SF[8] + P_[12][0]*SF[14] - P_[10][0]*SPP[10] - (P_[11][0]*q0)/2) + SF[10]*(P_[2][1] + P_[0][1]*SF[6] + P_[1][1]*SF[10] + P_[3][1]*SF[8] + P_[12][1]*SF[14] - P_[10][1]*SPP[10] - (P_[11][1]*q0)/2) + SF[8]*(P_[2][3] + P_[0][3]*SF[6] + P_[1][3]*SF[10] + P_[3][3]*SF[8] + P_[12][3]*SF[14] - P_[10][3]*SPP[10] - (P_[11][3]*q0)/2) + SF[14]*(P_[2][12] + P_[0][12]*SF[6] + P_[1][12]*SF[10] + P_[3][12]*SF[8] + P_[12][12]*SF[14] - P_[10][12]*SPP[10] - (P_[11][12]*q0)/2) - SPP[10]*(P_[2][10] + P_[0][10]*SF[6] + P_[1][10]*SF[10] + P_[3][10]*SF[8] + P_[12][10]*SF[14] - P_[10][10]*SPP[10] - (P_[11][10]*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P_[2][11] + P_[0][11]*SF[6] + P_[1][11]*SF[10] + P_[3][11]*SF[8] + P_[12][11]*SF[14] - P_[10][11]*SPP[10] - (P_[11][11]*q0)/2))/2;
nextP[0][3] = P_[0][3] + SQ[6] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10] + SF[7]*(P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10]) + SF[6]*(P_[0][1] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10]) + SF[9]*(P_[0][2] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10]) + SF[15]*(P_[0][10] + P_[1][10]*SF[9] + P_[2][10]*SF[11] + P_[3][10]*SF[10] + P_[10][10]*SF[14] + P_[11][10]*SF[15] + P_[12][10]*SPP[10]) - SF[14]*(P_[0][11] + P_[1][11]*SF[9] + P_[2][11]*SF[11] + P_[3][11]*SF[10] + P_[10][11]*SF[14] + P_[11][11]*SF[15] + P_[12][11]*SPP[10]) - (q0*(P_[0][12] + P_[1][12]*SF[9] + P_[2][12]*SF[11] + P_[3][12]*SF[10] + P_[10][12]*SF[14] + P_[11][12]*SF[15] + P_[12][12]*SPP[10]))/2;
nextP[1][3] = P_[1][3] + SQ[4] + P_[0][3]*SF[8] + P_[2][3]*SF[7] + P_[3][3]*SF[11] - P_[12][3]*SF[15] + P_[11][3]*SPP[10] - (P_[10][3]*q0)/2 + SF[7]*(P_[1][0] + P_[0][0]*SF[8] + P_[2][0]*SF[7] + P_[3][0]*SF[11] - P_[12][0]*SF[15] + P_[11][0]*SPP[10] - (P_[10][0]*q0)/2) + SF[6]*(P_[1][1] + P_[0][1]*SF[8] + P_[2][1]*SF[7] + P_[3][1]*SF[11] - P_[12][1]*SF[15] + P_[11][1]*SPP[10] - (P_[10][1]*q0)/2) + SF[9]*(P_[1][2] + P_[0][2]*SF[8] + P_[2][2]*SF[7] + P_[3][2]*SF[11] - P_[12][2]*SF[15] + P_[11][2]*SPP[10] - (P_[10][2]*q0)/2) + SF[15]*(P_[1][10] + P_[0][10]*SF[8] + P_[2][10]*SF[7] + P_[3][10]*SF[11] - P_[12][10]*SF[15] + P_[11][10]*SPP[10] - (P_[10][10]*q0)/2) - SF[14]*(P_[1][11] + P_[0][11]*SF[8] + P_[2][11]*SF[7] + P_[3][11]*SF[11] - P_[12][11]*SF[15] + P_[11][11]*SPP[10] - (P_[10][11]*q0)/2) - (q0*(P_[1][12] + P_[0][12]*SF[8] + P_[2][12]*SF[7] + P_[3][12]*SF[11] - P_[12][12]*SF[15] + P_[11][12]*SPP[10] - (P_[10][12]*q0)/2))/2;
nextP[2][3] = P_[2][3] + SQ[3] + P_[0][3]*SF[6] + P_[1][3]*SF[10] + P_[3][3]*SF[8] + P_[12][3]*SF[14] - P_[10][3]*SPP[10] - (P_[11][3]*q0)/2 + SF[7]*(P_[2][0] + P_[0][0]*SF[6] + P_[1][0]*SF[10] + P_[3][0]*SF[8] + P_[12][0]*SF[14] - P_[10][0]*SPP[10] - (P_[11][0]*q0)/2) + SF[6]*(P_[2][1] + P_[0][1]*SF[6] + P_[1][1]*SF[10] + P_[3][1]*SF[8] + P_[12][1]*SF[14] - P_[10][1]*SPP[10] - (P_[11][1]*q0)/2) + SF[9]*(P_[2][2] + P_[0][2]*SF[6] + P_[1][2]*SF[10] + P_[3][2]*SF[8] + P_[12][2]*SF[14] - P_[10][2]*SPP[10] - (P_[11][2]*q0)/2) + SF[15]*(P_[2][10] + P_[0][10]*SF[6] + P_[1][10]*SF[10] + P_[3][10]*SF[8] + P_[12][10]*SF[14] - P_[10][10]*SPP[10] - (P_[11][10]*q0)/2) - SF[14]*(P_[2][11] + P_[0][11]*SF[6] + P_[1][11]*SF[10] + P_[3][11]*SF[8] + P_[12][11]*SF[14] - P_[10][11]*SPP[10] - (P_[11][11]*q0)/2) - (q0*(P_[2][12] + P_[0][12]*SF[6] + P_[1][12]*SF[10] + P_[3][12]*SF[8] + P_[12][12]*SF[14] - P_[10][12]*SPP[10] - (P_[11][12]*q0)/2))/2;
nextP[3][3] = P_[3][3] + P_[0][3]*SF[7] + P_[1][3]*SF[6] + P_[2][3]*SF[9] + P_[10][3]*SF[15] - P_[11][3]*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P_[12][3]*q0)/2 + SF[7]*(P_[3][0] + P_[0][0]*SF[7] + P_[1][0]*SF[6] + P_[2][0]*SF[9] + P_[10][0]*SF[15] - P_[11][0]*SF[14] - (P_[12][0]*q0)/2) + SF[6]*(P_[3][1] + P_[0][1]*SF[7] + P_[1][1]*SF[6] + P_[2][1]*SF[9] + P_[10][1]*SF[15] - P_[11][1]*SF[14] - (P_[12][1]*q0)/2) + SF[9]*(P_[3][2] + P_[0][2]*SF[7] + P_[1][2]*SF[6] + P_[2][2]*SF[9] + P_[10][2]*SF[15] - P_[11][2]*SF[14] - (P_[12][2]*q0)/2) + SF[15]*(P_[3][10] + P_[0][10]*SF[7] + P_[1][10]*SF[6] + P_[2][10]*SF[9] + P_[10][10]*SF[15] - P_[11][10]*SF[14] - (P_[12][10]*q0)/2) - SF[14]*(P_[3][11] + P_[0][11]*SF[7] + P_[1][11]*SF[6] + P_[2][11]*SF[9] + P_[10][11]*SF[15] - P_[11][11]*SF[14] - (P_[12][11]*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P_[3][12] + P_[0][12]*SF[7] + P_[1][12]*SF[6] + P_[2][12]*SF[9] + P_[10][12]*SF[15] - P_[11][12]*SF[14] - (P_[12][12]*q0)/2))/2;
nextP[0][4] = P_[0][4] + P_[1][4]*SF[9] + P_[2][4]*SF[11] + P_[3][4]*SF[10] + P_[10][4]*SF[14] + P_[11][4]*SF[15] + P_[12][4]*SPP[10] + SF[5]*(P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10]) + SF[3]*(P_[0][1] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10]) - SF[4]*(P_[0][3] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10]) + SPP[0]*(P_[0][2] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10]) + SPP[3]*(P_[0][13] + P_[1][13]*SF[9] + P_[2][13]*SF[11] + P_[3][13]*SF[10] + P_[10][13]*SF[14] + P_[11][13]*SF[15] + P_[12][13]*SPP[10]) + SPP[6]*(P_[0][14] + P_[1][14]*SF[9] + P_[2][14]*SF[11] + P_[3][14]*SF[10] + P_[10][14]*SF[14] + P_[11][14]*SF[15] + P_[12][14]*SPP[10]) - SPP[9]*(P_[0][15] + P_[1][15]*SF[9] + P_[2][15]*SF[11] + P_[3][15]*SF[10] + P_[10][15]*SF[14] + P_[11][15]*SF[15] + P_[12][15]*SPP[10]);
nextP[1][4] = P_[1][4] + P_[0][4]*SF[8] + P_[2][4]*SF[7] + P_[3][4]*SF[11] - P_[12][4]*SF[15] + P_[11][4]*SPP[10] - (P_[10][4]*q0)/2 + SF[5]*(P_[1][0] + P_[0][0]*SF[8] + P_[2][0]*SF[7] + P_[3][0]*SF[11] - P_[12][0]*SF[15] + P_[11][0]*SPP[10] - (P_[10][0]*q0)/2) + SF[3]*(P_[1][1] + P_[0][1]*SF[8] + P_[2][1]*SF[7] + P_[3][1]*SF[11] - P_[12][1]*SF[15] + P_[11][1]*SPP[10] - (P_[10][1]*q0)/2) - SF[4]*(P_[1][3] + P_[0][3]*SF[8] + P_[2][3]*SF[7] + P_[3][3]*SF[11] - P_[12][3]*SF[15] + P_[11][3]*SPP[10] - (P_[10][3]*q0)/2) + SPP[0]*(P_[1][2] + P_[0][2]*SF[8] + P_[2][2]*SF[7] + P_[3][2]*SF[11] - P_[12][2]*SF[15] + P_[11][2]*SPP[10] - (P_[10][2]*q0)/2) + SPP[3]*(P_[1][13] + P_[0][13]*SF[8] + P_[2][13]*SF[7] + P_[3][13]*SF[11] - P_[12][13]*SF[15] + P_[11][13]*SPP[10] - (P_[10][13]*q0)/2) + SPP[6]*(P_[1][14] + P_[0][14]*SF[8] + P_[2][14]*SF[7] + P_[3][14]*SF[11] - P_[12][14]*SF[15] + P_[11][14]*SPP[10] - (P_[10][14]*q0)/2) - SPP[9]*(P_[1][15] + P_[0][15]*SF[8] + P_[2][15]*SF[7] + P_[3][15]*SF[11] - P_[12][15]*SF[15] + P_[11][15]*SPP[10] - (P_[10][15]*q0)/2);
nextP[2][4] = P_[2][4] + P_[0][4]*SF[6] + P_[1][4]*SF[10] + P_[3][4]*SF[8] + P_[12][4]*SF[14] - P_[10][4]*SPP[10] - (P_[11][4]*q0)/2 + SF[5]*(P_[2][0] + P_[0][0]*SF[6] + P_[1][0]*SF[10] + P_[3][0]*SF[8] + P_[12][0]*SF[14] - P_[10][0]*SPP[10] - (P_[11][0]*q0)/2) + SF[3]*(P_[2][1] + P_[0][1]*SF[6] + P_[1][1]*SF[10] + P_[3][1]*SF[8] + P_[12][1]*SF[14] - P_[10][1]*SPP[10] - (P_[11][1]*q0)/2) - SF[4]*(P_[2][3] + P_[0][3]*SF[6] + P_[1][3]*SF[10] + P_[3][3]*SF[8] + P_[12][3]*SF[14] - P_[10][3]*SPP[10] - (P_[11][3]*q0)/2) + SPP[0]*(P_[2][2] + P_[0][2]*SF[6] + P_[1][2]*SF[10] + P_[3][2]*SF[8] + P_[12][2]*SF[14] - P_[10][2]*SPP[10] - (P_[11][2]*q0)/2) + SPP[3]*(P_[2][13] + P_[0][13]*SF[6] + P_[1][13]*SF[10] + P_[3][13]*SF[8] + P_[12][13]*SF[14] - P_[10][13]*SPP[10] - (P_[11][13]*q0)/2) + SPP[6]*(P_[2][14] + P_[0][14]*SF[6] + P_[1][14]*SF[10] + P_[3][14]*SF[8] + P_[12][14]*SF[14] - P_[10][14]*SPP[10] - (P_[11][14]*q0)/2) - SPP[9]*(P_[2][15] + P_[0][15]*SF[6] + P_[1][15]*SF[10] + P_[3][15]*SF[8] + P_[12][15]*SF[14] - P_[10][15]*SPP[10] - (P_[11][15]*q0)/2);
nextP[3][4] = P_[3][4] + P_[0][4]*SF[7] + P_[1][4]*SF[6] + P_[2][4]*SF[9] + P_[10][4]*SF[15] - P_[11][4]*SF[14] - (P_[12][4]*q0)/2 + SF[5]*(P_[3][0] + P_[0][0]*SF[7] + P_[1][0]*SF[6] + P_[2][0]*SF[9] + P_[10][0]*SF[15] - P_[11][0]*SF[14] - (P_[12][0]*q0)/2) + SF[3]*(P_[3][1] + P_[0][1]*SF[7] + P_[1][1]*SF[6] + P_[2][1]*SF[9] + P_[10][1]*SF[15] - P_[11][1]*SF[14] - (P_[12][1]*q0)/2) - SF[4]*(P_[3][3] + P_[0][3]*SF[7] + P_[1][3]*SF[6] + P_[2][3]*SF[9] + P_[10][3]*SF[15] - P_[11][3]*SF[14] - (P_[12][3]*q0)/2) + SPP[0]*(P_[3][2] + P_[0][2]*SF[7] + P_[1][2]*SF[6] + P_[2][2]*SF[9] + P_[10][2]*SF[15] - P_[11][2]*SF[14] - (P_[12][2]*q0)/2) + SPP[3]*(P_[3][13] + P_[0][13]*SF[7] + P_[1][13]*SF[6] + P_[2][13]*SF[9] + P_[10][13]*SF[15] - P_[11][13]*SF[14] - (P_[12][13]*q0)/2) + SPP[6]*(P_[3][14] + P_[0][14]*SF[7] + P_[1][14]*SF[6] + P_[2][14]*SF[9] + P_[10][14]*SF[15] - P_[11][14]*SF[14] - (P_[12][14]*q0)/2) - SPP[9]*(P_[3][15] + P_[0][15]*SF[7] + P_[1][15]*SF[6] + P_[2][15]*SF[9] + P_[10][15]*SF[15] - P_[11][15]*SF[14] - (P_[12][15]*q0)/2);
nextP[4][4] = P_[4][4] + P_[0][4]*SF[5] + P_[1][4]*SF[3] - P_[3][4]*SF[4] + P_[2][4]*SPP[0] + P_[13][4]*SPP[3] + P_[14][4]*SPP[6] - P_[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P_[4][0] + P_[0][0]*SF[5] + P_[1][0]*SF[3] - P_[3][0]*SF[4] + P_[2][0]*SPP[0] + P_[13][0]*SPP[3] + P_[14][0]*SPP[6] - P_[15][0]*SPP[9]) + SF[3]*(P_[4][1] + P_[0][1]*SF[5] + P_[1][1]*SF[3] - P_[3][1]*SF[4] + P_[2][1]*SPP[0] + P_[13][1]*SPP[3] + P_[14][1]*SPP[6] - P_[15][1]*SPP[9]) - SF[4]*(P_[4][3] + P_[0][3]*SF[5] + P_[1][3]*SF[3] - P_[3][3]*SF[4] + P_[2][3]*SPP[0] + P_[13][3]*SPP[3] + P_[14][3]*SPP[6] - P_[15][3]*SPP[9]) + SPP[0]*(P_[4][2] + P_[0][2]*SF[5] + P_[1][2]*SF[3] - P_[3][2]*SF[4] + P_[2][2]*SPP[0] + P_[13][2]*SPP[3] + P_[14][2]*SPP[6] - P_[15][2]*SPP[9]) + SPP[3]*(P_[4][13] + P_[0][13]*SF[5] + P_[1][13]*SF[3] - P_[3][13]*SF[4] + P_[2][13]*SPP[0] + P_[13][13]*SPP[3] + P_[14][13]*SPP[6] - P_[15][13]*SPP[9]) + SPP[6]*(P_[4][14] + P_[0][14]*SF[5] + P_[1][14]*SF[3] - P_[3][14]*SF[4] + P_[2][14]*SPP[0] + P_[13][14]*SPP[3] + P_[14][14]*SPP[6] - P_[15][14]*SPP[9]) - SPP[9]*(P_[4][15] + P_[0][15]*SF[5] + P_[1][15]*SF[3] - P_[3][15]*SF[4] + P_[2][15]*SPP[0] + P_[13][15]*SPP[3] + P_[14][15]*SPP[6] - P_[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
nextP[0][5] = P_[0][5] + P_[1][5]*SF[9] + P_[2][5]*SF[11] + P_[3][5]*SF[10] + P_[10][5]*SF[14] + P_[11][5]*SF[15] + P_[12][5]*SPP[10] + SF[4]*(P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10]) + SF[3]*(P_[0][2] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10]) + SF[5]*(P_[0][3] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10]) - SPP[0]*(P_[0][1] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10]) - SPP[8]*(P_[0][13] + P_[1][13]*SF[9] + P_[2][13]*SF[11] + P_[3][13]*SF[10] + P_[10][13]*SF[14] + P_[11][13]*SF[15] + P_[12][13]*SPP[10]) + SPP[2]*(P_[0][14] + P_[1][14]*SF[9] + P_[2][14]*SF[11] + P_[3][14]*SF[10] + P_[10][14]*SF[14] + P_[11][14]*SF[15] + P_[12][14]*SPP[10]) + SPP[5]*(P_[0][15] + P_[1][15]*SF[9] + P_[2][15]*SF[11] + P_[3][15]*SF[10] + P_[10][15]*SF[14] + P_[11][15]*SF[15] + P_[12][15]*SPP[10]);
nextP[1][5] = P_[1][5] + P_[0][5]*SF[8] + P_[2][5]*SF[7] + P_[3][5]*SF[11] - P_[12][5]*SF[15] + P_[11][5]*SPP[10] - (P_[10][5]*q0)/2 + SF[4]*(P_[1][0] + P_[0][0]*SF[8] + P_[2][0]*SF[7] + P_[3][0]*SF[11] - P_[12][0]*SF[15] + P_[11][0]*SPP[10] - (P_[10][0]*q0)/2) + SF[3]*(P_[1][2] + P_[0][2]*SF[8] + P_[2][2]*SF[7] + P_[3][2]*SF[11] - P_[12][2]*SF[15] + P_[11][2]*SPP[10] - (P_[10][2]*q0)/2) + SF[5]*(P_[1][3] + P_[0][3]*SF[8] + P_[2][3]*SF[7] + P_[3][3]*SF[11] - P_[12][3]*SF[15] + P_[11][3]*SPP[10] - (P_[10][3]*q0)/2) - SPP[0]*(P_[1][1] + P_[0][1]*SF[8] + P_[2][1]*SF[7] + P_[3][1]*SF[11] - P_[12][1]*SF[15] + P_[11][1]*SPP[10] - (P_[10][1]*q0)/2) - SPP[8]*(P_[1][13] + P_[0][13]*SF[8] + P_[2][13]*SF[7] + P_[3][13]*SF[11] - P_[12][13]*SF[15] + P_[11][13]*SPP[10] - (P_[10][13]*q0)/2) + SPP[2]*(P_[1][14] + P_[0][14]*SF[8] + P_[2][14]*SF[7] + P_[3][14]*SF[11] - P_[12][14]*SF[15] + P_[11][14]*SPP[10] - (P_[10][14]*q0)/2) + SPP[5]*(P_[1][15] + P_[0][15]*SF[8] + P_[2][15]*SF[7] + P_[3][15]*SF[11] - P_[12][15]*SF[15] + P_[11][15]*SPP[10] - (P_[10][15]*q0)/2);
nextP[2][5] = P_[2][5] + P_[0][5]*SF[6] + P_[1][5]*SF[10] + P_[3][5]*SF[8] + P_[12][5]*SF[14] - P_[10][5]*SPP[10] - (P_[11][5]*q0)/2 + SF[4]*(P_[2][0] + P_[0][0]*SF[6] + P_[1][0]*SF[10] + P_[3][0]*SF[8] + P_[12][0]*SF[14] - P_[10][0]*SPP[10] - (P_[11][0]*q0)/2) + SF[3]*(P_[2][2] + P_[0][2]*SF[6] + P_[1][2]*SF[10] + P_[3][2]*SF[8] + P_[12][2]*SF[14] - P_[10][2]*SPP[10] - (P_[11][2]*q0)/2) + SF[5]*(P_[2][3] + P_[0][3]*SF[6] + P_[1][3]*SF[10] + P_[3][3]*SF[8] + P_[12][3]*SF[14] - P_[10][3]*SPP[10] - (P_[11][3]*q0)/2) - SPP[0]*(P_[2][1] + P_[0][1]*SF[6] + P_[1][1]*SF[10] + P_[3][1]*SF[8] + P_[12][1]*SF[14] - P_[10][1]*SPP[10] - (P_[11][1]*q0)/2) - SPP[8]*(P_[2][13] + P_[0][13]*SF[6] + P_[1][13]*SF[10] + P_[3][13]*SF[8] + P_[12][13]*SF[14] - P_[10][13]*SPP[10] - (P_[11][13]*q0)/2) + SPP[2]*(P_[2][14] + P_[0][14]*SF[6] + P_[1][14]*SF[10] + P_[3][14]*SF[8] + P_[12][14]*SF[14] - P_[10][14]*SPP[10] - (P_[11][14]*q0)/2) + SPP[5]*(P_[2][15] + P_[0][15]*SF[6] + P_[1][15]*SF[10] + P_[3][15]*SF[8] + P_[12][15]*SF[14] - P_[10][15]*SPP[10] - (P_[11][15]*q0)/2);
nextP[3][5] = P_[3][5] + P_[0][5]*SF[7] + P_[1][5]*SF[6] + P_[2][5]*SF[9] + P_[10][5]*SF[15] - P_[11][5]*SF[14] - (P_[12][5]*q0)/2 + SF[4]*(P_[3][0] + P_[0][0]*SF[7] + P_[1][0]*SF[6] + P_[2][0]*SF[9] + P_[10][0]*SF[15] - P_[11][0]*SF[14] - (P_[12][0]*q0)/2) + SF[3]*(P_[3][2] + P_[0][2]*SF[7] + P_[1][2]*SF[6] + P_[2][2]*SF[9] + P_[10][2]*SF[15] - P_[11][2]*SF[14] - (P_[12][2]*q0)/2) + SF[5]*(P_[3][3] + P_[0][3]*SF[7] + P_[1][3]*SF[6] + P_[2][3]*SF[9] + P_[10][3]*SF[15] - P_[11][3]*SF[14] - (P_[12][3]*q0)/2) - SPP[0]*(P_[3][1] + P_[0][1]*SF[7] + P_[1][1]*SF[6] + P_[2][1]*SF[9] + P_[10][1]*SF[15] - P_[11][1]*SF[14] - (P_[12][1]*q0)/2) - SPP[8]*(P_[3][13] + P_[0][13]*SF[7] + P_[1][13]*SF[6] + P_[2][13]*SF[9] + P_[10][13]*SF[15] - P_[11][13]*SF[14] - (P_[12][13]*q0)/2) + SPP[2]*(P_[3][14] + P_[0][14]*SF[7] + P_[1][14]*SF[6] + P_[2][14]*SF[9] + P_[10][14]*SF[15] - P_[11][14]*SF[14] - (P_[12][14]*q0)/2) + SPP[5]*(P_[3][15] + P_[0][15]*SF[7] + P_[1][15]*SF[6] + P_[2][15]*SF[9] + P_[10][15]*SF[15] - P_[11][15]*SF[14] - (P_[12][15]*q0)/2);
nextP[4][5] = P_[4][5] + SQ[2] + P_[0][5]*SF[5] + P_[1][5]*SF[3] - P_[3][5]*SF[4] + P_[2][5]*SPP[0] + P_[13][5]*SPP[3] + P_[14][5]*SPP[6] - P_[15][5]*SPP[9] + SF[4]*(P_[4][0] + P_[0][0]*SF[5] + P_[1][0]*SF[3] - P_[3][0]*SF[4] + P_[2][0]*SPP[0] + P_[13][0]*SPP[3] + P_[14][0]*SPP[6] - P_[15][0]*SPP[9]) + SF[3]*(P_[4][2] + P_[0][2]*SF[5] + P_[1][2]*SF[3] - P_[3][2]*SF[4] + P_[2][2]*SPP[0] + P_[13][2]*SPP[3] + P_[14][2]*SPP[6] - P_[15][2]*SPP[9]) + SF[5]*(P_[4][3] + P_[0][3]*SF[5] + P_[1][3]*SF[3] - P_[3][3]*SF[4] + P_[2][3]*SPP[0] + P_[13][3]*SPP[3] + P_[14][3]*SPP[6] - P_[15][3]*SPP[9]) - SPP[0]*(P_[4][1] + P_[0][1]*SF[5] + P_[1][1]*SF[3] - P_[3][1]*SF[4] + P_[2][1]*SPP[0] + P_[13][1]*SPP[3] + P_[14][1]*SPP[6] - P_[15][1]*SPP[9]) - SPP[8]*(P_[4][13] + P_[0][13]*SF[5] + P_[1][13]*SF[3] - P_[3][13]*SF[4] + P_[2][13]*SPP[0] + P_[13][13]*SPP[3] + P_[14][13]*SPP[6] - P_[15][13]*SPP[9]) + SPP[2]*(P_[4][14] + P_[0][14]*SF[5] + P_[1][14]*SF[3] - P_[3][14]*SF[4] + P_[2][14]*SPP[0] + P_[13][14]*SPP[3] + P_[14][14]*SPP[6] - P_[15][14]*SPP[9]) + SPP[5]*(P_[4][15] + P_[0][15]*SF[5] + P_[1][15]*SF[3] - P_[3][15]*SF[4] + P_[2][15]*SPP[0] + P_[13][15]*SPP[3] + P_[14][15]*SPP[6] - P_[15][15]*SPP[9]);
nextP[5][5] = P_[5][5] + P_[0][5]*SF[4] + P_[2][5]*SF[3] + P_[3][5]*SF[5] - P_[1][5]*SPP[0] - P_[13][5]*SPP[8] + P_[14][5]*SPP[2] + P_[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P_[5][0] + P_[0][0]*SF[4] + P_[2][0]*SF[3] + P_[3][0]*SF[5] - P_[1][0]*SPP[0] - P_[13][0]*SPP[8] + P_[14][0]*SPP[2] + P_[15][0]*SPP[5]) + SF[3]*(P_[5][2] + P_[0][2]*SF[4] + P_[2][2]*SF[3] + P_[3][2]*SF[5] - P_[1][2]*SPP[0] - P_[13][2]*SPP[8] + P_[14][2]*SPP[2] + P_[15][2]*SPP[5]) + SF[5]*(P_[5][3] + P_[0][3]*SF[4] + P_[2][3]*SF[3] + P_[3][3]*SF[5] - P_[1][3]*SPP[0] - P_[13][3]*SPP[8] + P_[14][3]*SPP[2] + P_[15][3]*SPP[5]) - SPP[0]*(P_[5][1] + P_[0][1]*SF[4] + P_[2][1]*SF[3] + P_[3][1]*SF[5] - P_[1][1]*SPP[0] - P_[13][1]*SPP[8] + P_[14][1]*SPP[2] + P_[15][1]*SPP[5]) - SPP[8]*(P_[5][13] + P_[0][13]*SF[4] + P_[2][13]*SF[3] + P_[3][13]*SF[5] - P_[1][13]*SPP[0] - P_[13][13]*SPP[8] + P_[14][13]*SPP[2] + P_[15][13]*SPP[5]) + SPP[2]*(P_[5][14] + P_[0][14]*SF[4] + P_[2][14]*SF[3] + P_[3][14]*SF[5] - P_[1][14]*SPP[0] - P_[13][14]*SPP[8] + P_[14][14]*SPP[2] + P_[15][14]*SPP[5]) + SPP[5]*(P_[5][15] + P_[0][15]*SF[4] + P_[2][15]*SF[3] + P_[3][15]*SF[5] - P_[1][15]*SPP[0] - P_[13][15]*SPP[8] + P_[14][15]*SPP[2] + P_[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
nextP[0][6] = P_[0][6] + P_[1][6]*SF[9] + P_[2][6]*SF[11] + P_[3][6]*SF[10] + P_[10][6]*SF[14] + P_[11][6]*SF[15] + P_[12][6]*SPP[10] + SF[4]*(P_[0][1] + P_[1][1]*SF[9] + P_[2][1]*SF[11] + P_[3][1]*SF[10] + P_[10][1]*SF[14] + P_[11][1]*SF[15] + P_[12][1]*SPP[10]) - SF[5]*(P_[0][2] + P_[1][2]*SF[9] + P_[2][2]*SF[11] + P_[3][2]*SF[10] + P_[10][2]*SF[14] + P_[11][2]*SF[15] + P_[12][2]*SPP[10]) + SF[3]*(P_[0][3] + P_[1][3]*SF[9] + P_[2][3]*SF[11] + P_[3][3]*SF[10] + P_[10][3]*SF[14] + P_[11][3]*SF[15] + P_[12][3]*SPP[10]) + SPP[0]*(P_[0][0] + P_[1][0]*SF[9] + P_[2][0]*SF[11] + P_[3][0]*SF[10] + P_[10][0]*SF[14] + P_[11][0]*SF[15] + P_[12][0]*SPP[10]) + SPP[4]*(P_[0][13] + P_[1][13]*SF[9] + P_[2][13]*SF[11] + P_[3][13]*SF[10] + P_[10][13]*SF[14] + P_[11][13]*SF[15] + P_[12][13]*SPP[10]) - SPP[7]*(P_[0][14] + P_[1][14]*SF[9] + P_[2][14]*SF[11] + P_[3][14]*SF[10] + P_[10][14]*SF[14] + P_[11][14]*SF[15] + P_[12][14]*SPP[10]) - SPP[1]*(P_[0][15] + P_[1][15]*SF[9] + P_[2][15]*SF[11] + P_[3][15]*SF[10] + P_[10][15]*SF[14] + P_[11][15]*SF[15] + P_[12][15]*SPP[10]);
nextP[1][6] = P_[1][6] + P_[0][6]*SF[8] + P_[2][6]*SF[7] + P_[3][6]*SF[11] - P_[12][6]*SF[15] + P_[11][6]*SPP[10] - (P_[10][6]*q0)/2 + SF[4]*(P_[1][1] + P_[0][1]*SF[8] + P_[2][1]*SF[7] + P_[3][1]*SF[11] - P_[12][1]*SF[15] + P_[11][1]*SPP[10] - (P_[10][1]*q0)/2) - SF[5]*(P_[1][2] + P_[0][2]*SF[8] + P_[2][2]*SF[7] + P_[3][2]*SF[11] - P_[12][2]*SF[15] + P_[11][2]*SPP[10] - (P_[10][2]*q0)/2) + SF[3]*(P_[1][3] + P_[0][3]*SF[8] + P_[2][3]*SF[7] + P_[3][3]*SF[11] - P_[12][3]*SF[15] + P_[11][3]*SPP[10] - (P_[10][3]*q0)/2) + SPP[0]*(P_[1][0] + P_[0][0]*SF[8] + P_[2][0]*SF[7] + P_[3][0]*SF[11] - P_[12][0]*SF[15] + P_[11][0]*SPP[10] - (P_[10][0]*q0)/2) + SPP[4]*(P_[1][13] + P_[0][13]*SF[8] + P_[2][13]*SF[7] + P_[3][13]*SF[11] - P_[12][13]*SF[15] + P_[11][13]*SPP[10] - (P_[10][13]*q0)/2) - SPP[7]*(P_[1][14] + P_[0][14]*SF[8] + P_[2][14]*SF[7] + P_[3][14]*SF[11] - P_[12][14]*SF[15] + P_[11][14]*SPP[10] - (P_[10][14]*q0)/2) - SPP[1]*(P_[1][15] + P_[0][15]*SF[8] + P_[2][15]*SF[7] + P_[3][15]*SF[11] - P_[12][15]*SF[15] + P_[11][15]*SPP[10] - (P_[10][15]*q0)/2);
nextP[2][6] = P_[2][6] + P_[0][6]*SF[6] + P_[1][6]*SF[10] + P_[3][6]*SF[8] + P_[12][6]*SF[14] - P_[10][6]*SPP[10] - (P_[11][6]*q0)/2 + SF[4]*(P_[2][1] + P_[0][1]*SF[6] + P_[1][1]*SF[10] + P_[3][1]*SF[8] + P_[12][1]*SF[14] - P_[10][1]*SPP[10] - (P_[11][1]*q0)/2) - SF[5]*(P_[2][2] + P_[0][2]*SF[6] + P_[1][2]*SF[10] + P_[3][2]*SF[8] + P_[12][2]*SF[14] - P_[10][2]*SPP[10] - (P_[11][2]*q0)/2) + SF[3]*(P_[2][3] + P_[0][3]*SF[6] + P_[1][3]*SF[10] + P_[3][3]*SF[8] + P_[12][3]*SF[14] - P_[10][3]*SPP[10] - (P_[11][3]*q0)/2) + SPP[0]*(P_[2][0] + P_[0][0]*SF[6] + P_[1][0]*SF[10] + P_[3][0]*SF[8] + P_[12][0]*SF[14] - P_[10][0]*SPP[10] - (P_[11][0]*q0)/2) + SPP[4]*(P_[2][13] + P_[0][13]*SF[6] + P_[1][13]*SF[10] + P_[3][13]*SF[8] + P_[12][13]*SF[14] - P_[10][13]*SPP[10] - (P_[11][13]*q0)/2) - SPP[7]*(P_[2][14] + P_[0][14]*SF[6] + P_[1][14]*SF[10] + P_[3][14]*SF[8] + P_[12][14]*SF[14] - P_[10][14]*SPP[10] - (P_[11][14]*q0)/2) - SPP[1]*(P_[2][15] + P_[0][15]*SF[6] + P_[1][15]*SF[10] + P_[3][15]*SF[8] + P_[12][15]*SF[14] - P_[10][15]*SPP[10] - (P_[11][15]*q0)/2);
nextP[3][6] = P_[3][6] + P_[0][6]*SF[7] + P_[1][6]*SF[6] + P_[2][6]*SF[9] + P_[10][6]*SF[15] - P_[11][6]*SF[14] - (P_[12][6]*q0)/2 + SF[4]*(P_[3][1] + P_[0][1]*SF[7] + P_[1][1]*SF[6] + P_[2][1]*SF[9] + P_[10][1]*SF[15] - P_[11][1]*SF[14] - (P_[12][1]*q0)/2) - SF[5]*(P_[3][2] + P_[0][2]*SF[7] + P_[1][2]*SF[6] + P_[2][2]*SF[9] + P_[10][2]*SF[15] - P_[11][2]*SF[14] - (P_[12][2]*q0)/2) + SF[3]*(P_[3][3] + P_[0][3]*SF[7] + P_[1][3]*SF[6] + P_[2][3]*SF[9] + P_[10][3]*SF[15] - P_[11][3]*SF[14] - (P_[12][3]*q0)/2) + SPP[0]*(P_[3][0] + P_[0][0]*SF[7] + P_[1][0]*SF[6] + P_[2][0]*SF[9] + P_[10][0]*SF[15] - P_[11][0]*SF[14] - (P_[12][0]*q0)/2) + SPP[4]*(P_[3][13] + P_[0][13]*SF[7] + P_[1][13]*SF[6] + P_[2][13]*SF[9] + P_[10][13]*SF[15] - P_[11][13]*SF[14] - (P_[12][13]*q0)/2) - SPP[7]*(P_[3][14] + P_[0][14]*SF[7] + P_[1][14]*SF[6] + P_[2][14]*SF[9] + P_[10][14]*SF[15] - P_[11][14]*SF[14] - (P_[12][14]*q0)/2) - SPP[1]*(P_[3][15] + P_[0][15]*SF[7] + P_[1][15]*SF[6] + P_[2][15]*SF[9] + P_[10][15]*SF[15] - P_[11][15]*SF[14] - (P_[12][15]*q0)/2);
nextP[4][6] = P_[4][6] + SQ[1] + P_[0][6]*SF[5] + P_[1][6]*SF[3] - P_[3][6]*SF[4] + P_[2][6]*SPP[0] + P_[13][6]*SPP[3] + P_[14][6]*SPP[6] - P_[15][6]*SPP[9] + SF[4]*(P_[4][1] + P_[0][1]*SF[5] + P_[1][1]*SF[3] - P_[3][1]*SF[4] + P_[2][1]*SPP[0] + P_[13][1]*SPP[3] + P_[14][1]*SPP[6] - P_[15][1]*SPP[9]) - SF[5]*(P_[4][2] + P_[0][2]*SF[5] + P_[1][2]*SF[3] - P_[3][2]*SF[4] + P_[2][2]*SPP[0] + P_[13][2]*SPP[3] + P_[14][2]*SPP[6] - P_[15][2]*SPP[9]) + SF[3]*(P_[4][3] + P_[0][3]*SF[5] + P_[1][3]*SF[3] - P_[3][3]*SF[4] + P_[2][3]*SPP[0] + P_[13][3]*SPP[3] + P_[14][3]*SPP[6] - P_[15][3]*SPP[9]) + SPP[0]*(P_[4][0] + P_[0][0]*SF[5] + P_[1][0]*SF[3] - P_[3][0]*SF[4] + P_[2][0]*SPP[0] + P_[13][0]*SPP[3] + P_[14][0]*SPP[6] - P_[15][0]*SPP[9]) + SPP[4]*(P_[4][13] + P_[0][13]*SF[5] + P_[1][13]*SF[3] - P_[3][13]*SF[4] + P_[2][13]*SPP[0] + P_[13][13]*SPP[3] + P_[14][13]*SPP[6] - P_[15][13]*SPP[9]) - SPP[7]*(P_[4][14] + P_[0][14]*SF[5] + P_[1][14]*SF[3] - P_[3][14]*SF[4] + P_[2][14]*SPP[0] + P_[13][14]*SPP[3] + P_[14][14]*SPP[6] - P_[15][14]*SPP[9]) - SPP[1]*(P_[4][15] + P_[0][15]*SF[5] + P_[1][15]*SF[3] - P_[3][15]*SF[4] + P_[2][15]*SPP[0] + P_[13][15]*SPP[3] + P_[14][15]*SPP[6] - P_[15][15]*SPP[9]);
nextP[5][6] = P_[5][6] + SQ[0] + P_[0][6]*SF[4] + P_[2][6]*SF[3] + P_[3][6]*SF[5] - P_[1][6]*SPP[0] - P_[13][6]*SPP[8] + P_[14][6]*SPP[2] + P_[15][6]*SPP[5] + SF[4]*(P_[5][1] + P_[0][1]*SF[4] + P_[2][1]*SF[3] + P_[3][1]*SF[5] - P_[1][1]*SPP[0] - P_[13][1]*SPP[8] + P_[14][1]*SPP[2] + P_[15][1]*SPP[5]) - SF[5]*(P_[5][2] + P_[0][2]*SF[4] + P_[2][2]*SF[3] + P_[3][2]*SF[5] - P_[1][2]*SPP[0] - P_[13][2]*SPP[8] + P_[14][2]*SPP[2] + P_[15][2]*SPP[5]) + SF[3]*(P_[5][3] + P_[0][3]*SF[4] + P_[2][3]*SF[3] + P_[3][3]*SF[5] - P_[1][3]*SPP[0] - P_[13][3]*SPP[8] + P_[14][3]*SPP[2] + P_[15][3]*SPP[5]) + SPP[0]*(P_[5][0] + P_[0][0]*SF[4] + P_[2][0]*SF[3] + P_[3][0]*SF[5] - P_[1][0]*SPP[0] - P_[13][0]*SPP[8] + P_[14][0]*SPP[2] + P_[15][0]*SPP[5]) + SPP[4]*(P_[5][13] + P_[0][13]*SF[4] + P_[2][13]*SF[3] + P_[3][13]*SF[5] - P_[1][13]*SPP[0] - P_[13][13]*SPP[8] + P_[14][13]*SPP[2] + P_[15][13]*SPP[5]) - SPP[7]*(P_[5][14] + P_[0][14]*SF[4] + P_[2][14]*SF[3] + P_[3][14]*SF[5] - P_[1][14]*SPP[0] - P_[13][14]*SPP[8] + P_[14][14]*SPP[2] + P_[15][14]*SPP[5]) - SPP[1]*(P_[5][15] + P_[0][15]*SF[4] + P_[2][15]*SF[3] + P_[3][15]*SF[5] - P_[1][15]*SPP[0] - P_[13][15]*SPP[8] + P_[14][15]*SPP[2] + P_[15][15]*SPP[5]);
nextP[6][6] = P_[6][6] + P_[1][6]*SF[4] - P_[2][6]*SF[5] + P_[3][6]*SF[3] + P_[0][6]*SPP[0] + P_[13][6]*SPP[4] - P_[14][6]*SPP[7] - P_[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P_[6][1] + P_[1][1]*SF[4] - P_[2][1]*SF[5] + P_[3][1]*SF[3] + P_[0][1]*SPP[0] + P_[13][1]*SPP[4] - P_[14][1]*SPP[7] - P_[15][1]*SPP[1]) - SF[5]*(P_[6][2] + P_[1][2]*SF[4] - P_[2][2]*SF[5] + P_[3][2]*SF[3] + P_[0][2]*SPP[0] + P_[13][2]*SPP[4] - P_[14][2]*SPP[7] - P_[15][2]*SPP[1]) + SF[3]*(P_[6][3] + P_[1][3]*SF[4] - P_[2][3]*SF[5] + P_[3][3]*SF[3] + P_[0][3]*SPP[0] + P_[13][3]*SPP[4] - P_[14][3]*SPP[7] - P_[15][3]*SPP[1]) + SPP[0]*(P_[6][0] + P_[1][0]*SF[4] - P_[2][0]*SF[5] + P_[3][0]*SF[3] + P_[0][0]*SPP[0] + P_[13][0]*SPP[4] - P_[14][0]*SPP[7] - P_[15][0]*SPP[1]) + SPP[4]*(P_[6][13] + P_[1][13]*SF[4] - P_[2][13]*SF[5] + P_[3][13]*SF[3] + P_[0][13]*SPP[0] + P_[13][13]*SPP[4] - P_[14][13]*SPP[7] - P_[15][13]*SPP[1]) - SPP[7]*(P_[6][14] + P_[1][14]*SF[4] - P_[2][14]*SF[5] + P_[3][14]*SF[3] + P_[0][14]*SPP[0] + P_[13][14]*SPP[4] - P_[14][14]*SPP[7] - P_[15][14]*SPP[1]) - SPP[1]*(P_[6][15] + P_[1][15]*SF[4] - P_[2][15]*SF[5] + P_[3][15]*SF[3] + P_[0][15]*SPP[0] + P_[13][15]*SPP[4] - P_[14][15]*SPP[7] - P_[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
nextP[0][7] = P_[0][7] + P_[1][7]*SF[9] + P_[2][7]*SF[11] + P_[3][7]*SF[10] + P_[10][7]*SF[14] + P_[11][7]*SF[15] + P_[12][7]*SPP[10] + dt*(P_[0][4] + P_[1][4]*SF[9] + P_[2][4]*SF[11] + P_[3][4]*SF[10] + P_[10][4]*SF[14] + P_[11][4]*SF[15] + P_[12][4]*SPP[10]);
nextP[1][7] = P_[1][7] + P_[0][7]*SF[8] + P_[2][7]*SF[7] + P_[3][7]*SF[11] - P_[12][7]*SF[15] + P_[11][7]*SPP[10] - (P_[10][7]*q0)/2 + dt*(P_[1][4] + P_[0][4]*SF[8] + P_[2][4]*SF[7] + P_[3][4]*SF[11] - P_[12][4]*SF[15] + P_[11][4]*SPP[10] - (P_[10][4]*q0)/2);
nextP[2][7] = P_[2][7] + P_[0][7]*SF[6] + P_[1][7]*SF[10] + P_[3][7]*SF[8] + P_[12][7]*SF[14] - P_[10][7]*SPP[10] - (P_[11][7]*q0)/2 + dt*(P_[2][4] + P_[0][4]*SF[6] + P_[1][4]*SF[10] + P_[3][4]*SF[8] + P_[12][4]*SF[14] - P_[10][4]*SPP[10] - (P_[11][4]*q0)/2);
nextP[3][7] = P_[3][7] + P_[0][7]*SF[7] + P_[1][7]*SF[6] + P_[2][7]*SF[9] + P_[10][7]*SF[15] - P_[11][7]*SF[14] - (P_[12][7]*q0)/2 + dt*(P_[3][4] + P_[0][4]*SF[7] + P_[1][4]*SF[6] + P_[2][4]*SF[9] + P_[10][4]*SF[15] - P_[11][4]*SF[14] - (P_[12][4]*q0)/2);
nextP[4][7] = P_[4][7] + P_[0][7]*SF[5] + P_[1][7]*SF[3] - P_[3][7]*SF[4] + P_[2][7]*SPP[0] + P_[13][7]*SPP[3] + P_[14][7]*SPP[6] - P_[15][7]*SPP[9] + dt*(P_[4][4] + P_[0][4]*SF[5] + P_[1][4]*SF[3] - P_[3][4]*SF[4] + P_[2][4]*SPP[0] + P_[13][4]*SPP[3] + P_[14][4]*SPP[6] - P_[15][4]*SPP[9]);
nextP[5][7] = P_[5][7] + P_[0][7]*SF[4] + P_[2][7]*SF[3] + P_[3][7]*SF[5] - P_[1][7]*SPP[0] - P_[13][7]*SPP[8] + P_[14][7]*SPP[2] + P_[15][7]*SPP[5] + dt*(P_[5][4] + P_[0][4]*SF[4] + P_[2][4]*SF[3] + P_[3][4]*SF[5] - P_[1][4]*SPP[0] - P_[13][4]*SPP[8] + P_[14][4]*SPP[2] + P_[15][4]*SPP[5]);
nextP[6][7] = P_[6][7] + P_[1][7]*SF[4] - P_[2][7]*SF[5] + P_[3][7]*SF[3] + P_[0][7]*SPP[0] + P_[13][7]*SPP[4] - P_[14][7]*SPP[7] - P_[15][7]*SPP[1] + dt*(P_[6][4] + P_[1][4]*SF[4] - P_[2][4]*SF[5] + P_[3][4]*SF[3] + P_[0][4]*SPP[0] + P_[13][4]*SPP[4] - P_[14][4]*SPP[7] - P_[15][4]*SPP[1]);
nextP[7][7] = P_[7][7] + P_[4][7]*dt + dt*(P_[7][4] + P_[4][4]*dt);
nextP[0][8] = P_[0][8] + P_[1][8]*SF[9] + P_[2][8]*SF[11] + P_[3][8]*SF[10] + P_[10][8]*SF[14] + P_[11][8]*SF[15] + P_[12][8]*SPP[10] + dt*(P_[0][5] + P_[1][5]*SF[9] + P_[2][5]*SF[11] + P_[3][5]*SF[10] + P_[10][5]*SF[14] + P_[11][5]*SF[15] + P_[12][5]*SPP[10]);
nextP[1][8] = P_[1][8] + P_[0][8]*SF[8] + P_[2][8]*SF[7] + P_[3][8]*SF[11] - P_[12][8]*SF[15] + P_[11][8]*SPP[10] - (P_[10][8]*q0)/2 + dt*(P_[1][5] + P_[0][5]*SF[8] + P_[2][5]*SF[7] + P_[3][5]*SF[11] - P_[12][5]*SF[15] + P_[11][5]*SPP[10] - (P_[10][5]*q0)/2);
nextP[2][8] = P_[2][8] + P_[0][8]*SF[6] + P_[1][8]*SF[10] + P_[3][8]*SF[8] + P_[12][8]*SF[14] - P_[10][8]*SPP[10] - (P_[11][8]*q0)/2 + dt*(P_[2][5] + P_[0][5]*SF[6] + P_[1][5]*SF[10] + P_[3][5]*SF[8] + P_[12][5]*SF[14] - P_[10][5]*SPP[10] - (P_[11][5]*q0)/2);
nextP[3][8] = P_[3][8] + P_[0][8]*SF[7] + P_[1][8]*SF[6] + P_[2][8]*SF[9] + P_[10][8]*SF[15] - P_[11][8]*SF[14] - (P_[12][8]*q0)/2 + dt*(P_[3][5] + P_[0][5]*SF[7] + P_[1][5]*SF[6] + P_[2][5]*SF[9] + P_[10][5]*SF[15] - P_[11][5]*SF[14] - (P_[12][5]*q0)/2);
nextP[4][8] = P_[4][8] + P_[0][8]*SF[5] + P_[1][8]*SF[3] - P_[3][8]*SF[4] + P_[2][8]*SPP[0] + P_[13][8]*SPP[3] + P_[14][8]*SPP[6] - P_[15][8]*SPP[9] + dt*(P_[4][5] + P_[0][5]*SF[5] + P_[1][5]*SF[3] - P_[3][5]*SF[4] + P_[2][5]*SPP[0] + P_[13][5]*SPP[3] + P_[14][5]*SPP[6] - P_[15][5]*SPP[9]);
nextP[5][8] = P_[5][8] + P_[0][8]*SF[4] + P_[2][8]*SF[3] + P_[3][8]*SF[5] - P_[1][8]*SPP[0] - P_[13][8]*SPP[8] + P_[14][8]*SPP[2] + P_[15][8]*SPP[5] + dt*(P_[5][5] + P_[0][5]*SF[4] + P_[2][5]*SF[3] + P_[3][5]*SF[5] - P_[1][5]*SPP[0] - P_[13][5]*SPP[8] + P_[14][5]*SPP[2] + P_[15][5]*SPP[5]);
nextP[6][8] = P_[6][8] + P_[1][8]*SF[4] - P_[2][8]*SF[5] + P_[3][8]*SF[3] + P_[0][8]*SPP[0] + P_[13][8]*SPP[4] - P_[14][8]*SPP[7] - P_[15][8]*SPP[1] + dt*(P_[6][5] + P_[1][5]*SF[4] - P_[2][5]*SF[5] + P_[3][5]*SF[3] + P_[0][5]*SPP[0] + P_[13][5]*SPP[4] - P_[14][5]*SPP[7] - P_[15][5]*SPP[1]);
nextP[7][8] = P_[7][8] + P_[4][8]*dt + dt*(P_[7][5] + P_[4][5]*dt);
nextP[8][8] = P_[8][8] + P_[5][8]*dt + dt*(P_[8][5] + P_[5][5]*dt);
nextP[0][9] = P_[0][9] + P_[1][9]*SF[9] + P_[2][9]*SF[11] + P_[3][9]*SF[10] + P_[10][9]*SF[14] + P_[11][9]*SF[15] + P_[12][9]*SPP[10] + dt*(P_[0][6] + P_[1][6]*SF[9] + P_[2][6]*SF[11] + P_[3][6]*SF[10] + P_[10][6]*SF[14] + P_[11][6]*SF[15] + P_[12][6]*SPP[10]);
nextP[1][9] = P_[1][9] + P_[0][9]*SF[8] + P_[2][9]*SF[7] + P_[3][9]*SF[11] - P_[12][9]*SF[15] + P_[11][9]*SPP[10] - (P_[10][9]*q0)/2 + dt*(P_[1][6] + P_[0][6]*SF[8] + P_[2][6]*SF[7] + P_[3][6]*SF[11] - P_[12][6]*SF[15] + P_[11][6]*SPP[10] - (P_[10][6]*q0)/2);
nextP[2][9] = P_[2][9] + P_[0][9]*SF[6] + P_[1][9]*SF[10] + P_[3][9]*SF[8] + P_[12][9]*SF[14] - P_[10][9]*SPP[10] - (P_[11][9]*q0)/2 + dt*(P_[2][6] + P_[0][6]*SF[6] + P_[1][6]*SF[10] + P_[3][6]*SF[8] + P_[12][6]*SF[14] - P_[10][6]*SPP[10] - (P_[11][6]*q0)/2);
nextP[3][9] = P_[3][9] + P_[0][9]*SF[7] + P_[1][9]*SF[6] + P_[2][9]*SF[9] + P_[10][9]*SF[15] - P_[11][9]*SF[14] - (P_[12][9]*q0)/2 + dt*(P_[3][6] + P_[0][6]*SF[7] + P_[1][6]*SF[6] + P_[2][6]*SF[9] + P_[10][6]*SF[15] - P_[11][6]*SF[14] - (P_[12][6]*q0)/2);
nextP[4][9] = P_[4][9] + P_[0][9]*SF[5] + P_[1][9]*SF[3] - P_[3][9]*SF[4] + P_[2][9]*SPP[0] + P_[13][9]*SPP[3] + P_[14][9]*SPP[6] - P_[15][9]*SPP[9] + dt*(P_[4][6] + P_[0][6]*SF[5] + P_[1][6]*SF[3] - P_[3][6]*SF[4] + P_[2][6]*SPP[0] + P_[13][6]*SPP[3] + P_[14][6]*SPP[6] - P_[15][6]*SPP[9]);
nextP[5][9] = P_[5][9] + P_[0][9]*SF[4] + P_[2][9]*SF[3] + P_[3][9]*SF[5] - P_[1][9]*SPP[0] - P_[13][9]*SPP[8] + P_[14][9]*SPP[2] + P_[15][9]*SPP[5] + dt*(P_[5][6] + P_[0][6]*SF[4] + P_[2][6]*SF[3] + P_[3][6]*SF[5] - P_[1][6]*SPP[0] - P_[13][6]*SPP[8] + P_[14][6]*SPP[2] + P_[15][6]*SPP[5]);
nextP[6][9] = P_[6][9] + P_[1][9]*SF[4] - P_[2][9]*SF[5] + P_[3][9]*SF[3] + P_[0][9]*SPP[0] + P_[13][9]*SPP[4] - P_[14][9]*SPP[7] - P_[15][9]*SPP[1] + dt*(P_[6][6] + P_[1][6]*SF[4] - P_[2][6]*SF[5] + P_[3][6]*SF[3] + P_[0][6]*SPP[0] + P_[13][6]*SPP[4] - P_[14][6]*SPP[7] - P_[15][6]*SPP[1]);
nextP[7][9] = P_[7][9] + P_[4][9]*dt + dt*(P_[7][6] + P_[4][6]*dt);
nextP[8][9] = P_[8][9] + P_[5][9]*dt + dt*(P_[8][6] + P_[5][6]*dt);
nextP[9][9] = P_[9][9] + P_[6][9]*dt + dt*(P_[9][6] + P_[6][6]*dt);
nextP[0][10] = P_[0][10] + P_[1][10]*SF[9] + P_[2][10]*SF[11] + P_[3][10]*SF[10] + P_[10][10]*SF[14] + P_[11][10]*SF[15] + P_[12][10]*SPP[10];
nextP[1][10] = P_[1][10] + P_[0][10]*SF[8] + P_[2][10]*SF[7] + P_[3][10]*SF[11] - P_[12][10]*SF[15] + P_[11][10]*SPP[10] - (P_[10][10]*q0)/2;
nextP[2][10] = P_[2][10] + P_[0][10]*SF[6] + P_[1][10]*SF[10] + P_[3][10]*SF[8] + P_[12][10]*SF[14] - P_[10][10]*SPP[10] - (P_[11][10]*q0)/2;
nextP[3][10] = P_[3][10] + P_[0][10]*SF[7] + P_[1][10]*SF[6] + P_[2][10]*SF[9] + P_[10][10]*SF[15] - P_[11][10]*SF[14] - (P_[12][10]*q0)/2;
nextP[4][10] = P_[4][10] + P_[0][10]*SF[5] + P_[1][10]*SF[3] - P_[3][10]*SF[4] + P_[2][10]*SPP[0] + P_[13][10]*SPP[3] + P_[14][10]*SPP[6] - P_[15][10]*SPP[9];
nextP[5][10] = P_[5][10] + P_[0][10]*SF[4] + P_[2][10]*SF[3] + P_[3][10]*SF[5] - P_[1][10]*SPP[0] - P_[13][10]*SPP[8] + P_[14][10]*SPP[2] + P_[15][10]*SPP[5];
nextP[6][10] = P_[6][10] + P_[1][10]*SF[4] - P_[2][10]*SF[5] + P_[3][10]*SF[3] + P_[0][10]*SPP[0] + P_[13][10]*SPP[4] - P_[14][10]*SPP[7] - P_[15][10]*SPP[1];
nextP[7][10] = P_[7][10] + P_[4][10]*dt;
nextP[8][10] = P_[8][10] + P_[5][10]*dt;
nextP[9][10] = P_[9][10] + P_[6][10]*dt;
nextP[10][10] = P_[10][10];
nextP[0][11] = P_[0][11] + P_[1][11]*SF[9] + P_[2][11]*SF[11] + P_[3][11]*SF[10] + P_[10][11]*SF[14] + P_[11][11]*SF[15] + P_[12][11]*SPP[10];
nextP[1][11] = P_[1][11] + P_[0][11]*SF[8] + P_[2][11]*SF[7] + P_[3][11]*SF[11] - P_[12][11]*SF[15] + P_[11][11]*SPP[10] - (P_[10][11]*q0)/2;
nextP[2][11] = P_[2][11] + P_[0][11]*SF[6] + P_[1][11]*SF[10] + P_[3][11]*SF[8] + P_[12][11]*SF[14] - P_[10][11]*SPP[10] - (P_[11][11]*q0)/2;
nextP[3][11] = P_[3][11] + P_[0][11]*SF[7] + P_[1][11]*SF[6] + P_[2][11]*SF[9] + P_[10][11]*SF[15] - P_[11][11]*SF[14] - (P_[12][11]*q0)/2;
nextP[4][11] = P_[4][11] + P_[0][11]*SF[5] + P_[1][11]*SF[3] - P_[3][11]*SF[4] + P_[2][11]*SPP[0] + P_[13][11]*SPP[3] + P_[14][11]*SPP[6] - P_[15][11]*SPP[9];
nextP[5][11] = P_[5][11] + P_[0][11]*SF[4] + P_[2][11]*SF[3] + P_[3][11]*SF[5] - P_[1][11]*SPP[0] - P_[13][11]*SPP[8] + P_[14][11]*SPP[2] + P_[15][11]*SPP[5];
nextP[6][11] = P_[6][11] + P_[1][11]*SF[4] - P_[2][11]*SF[5] + P_[3][11]*SF[3] + P_[0][11]*SPP[0] + P_[13][11]*SPP[4] - P_[14][11]*SPP[7] - P_[15][11]*SPP[1];
nextP[7][11] = P_[7][11] + P_[4][11]*dt;
nextP[8][11] = P_[8][11] + P_[5][11]*dt;
nextP[9][11] = P_[9][11] + P_[6][11]*dt;
nextP[10][11] = P_[10][11];
nextP[11][11] = P_[11][11];
nextP[0][12] = P_[0][12] + P_[1][12]*SF[9] + P_[2][12]*SF[11] + P_[3][12]*SF[10] + P_[10][12]*SF[14] + P_[11][12]*SF[15] + P_[12][12]*SPP[10];
nextP[1][12] = P_[1][12] + P_[0][12]*SF[8] + P_[2][12]*SF[7] + P_[3][12]*SF[11] - P_[12][12]*SF[15] + P_[11][12]*SPP[10] - (P_[10][12]*q0)/2;
nextP[2][12] = P_[2][12] + P_[0][12]*SF[6] + P_[1][12]*SF[10] + P_[3][12]*SF[8] + P_[12][12]*SF[14] - P_[10][12]*SPP[10] - (P_[11][12]*q0)/2;
nextP[3][12] = P_[3][12] + P_[0][12]*SF[7] + P_[1][12]*SF[6] + P_[2][12]*SF[9] + P_[10][12]*SF[15] - P_[11][12]*SF[14] - (P_[12][12]*q0)/2;
nextP[4][12] = P_[4][12] + P_[0][12]*SF[5] + P_[1][12]*SF[3] - P_[3][12]*SF[4] + P_[2][12]*SPP[0] + P_[13][12]*SPP[3] + P_[14][12]*SPP[6] - P_[15][12]*SPP[9];
nextP[5][12] = P_[5][12] + P_[0][12]*SF[4] + P_[2][12]*SF[3] + P_[3][12]*SF[5] - P_[1][12]*SPP[0] - P_[13][12]*SPP[8] + P_[14][12]*SPP[2] + P_[15][12]*SPP[5];
nextP[6][12] = P_[6][12] + P_[1][12]*SF[4] - P_[2][12]*SF[5] + P_[3][12]*SF[3] + P_[0][12]*SPP[0] + P_[13][12]*SPP[4] - P_[14][12]*SPP[7] - P_[15][12]*SPP[1];
nextP[7][12] = P_[7][12] + P_[4][12]*dt;
nextP[8][12] = P_[8][12] + P_[5][12]*dt;
nextP[9][12] = P_[9][12] + P_[6][12]*dt;
nextP[10][12] = P_[10][12];
nextP[11][12] = P_[11][12];
nextP[12][12] = P_[12][12];
// add process noise that is not from the IMU
for (unsigned i = 0; i <= 12; i++) {
nextP[i][i] += process_noise[i];
}
// calculate variances and upper diagonal covariances for IMU delta velocity bias states
nextP[0][13] = P_[0][13] + P_[1][13]*SF[9] + P_[2][13]*SF[11] + P_[3][13]*SF[10] + P_[10][13]*SF[14] + P_[11][13]*SF[15] + P_[12][13]*SPP[10];
nextP[1][13] = P_[1][13] + P_[0][13]*SF[8] + P_[2][13]*SF[7] + P_[3][13]*SF[11] - P_[12][13]*SF[15] + P_[11][13]*SPP[10] - (P_[10][13]*q0)/2;
nextP[2][13] = P_[2][13] + P_[0][13]*SF[6] + P_[1][13]*SF[10] + P_[3][13]*SF[8] + P_[12][13]*SF[14] - P_[10][13]*SPP[10] - (P_[11][13]*q0)/2;
nextP[3][13] = P_[3][13] + P_[0][13]*SF[7] + P_[1][13]*SF[6] + P_[2][13]*SF[9] + P_[10][13]*SF[15] - P_[11][13]*SF[14] - (P_[12][13]*q0)/2;
nextP[4][13] = P_[4][13] + P_[0][13]*SF[5] + P_[1][13]*SF[3] - P_[3][13]*SF[4] + P_[2][13]*SPP[0] + P_[13][13]*SPP[3] + P_[14][13]*SPP[6] - P_[15][13]*SPP[9];
nextP[5][13] = P_[5][13] + P_[0][13]*SF[4] + P_[2][13]*SF[3] + P_[3][13]*SF[5] - P_[1][13]*SPP[0] - P_[13][13]*SPP[8] + P_[14][13]*SPP[2] + P_[15][13]*SPP[5];
nextP[6][13] = P_[6][13] + P_[1][13]*SF[4] - P_[2][13]*SF[5] + P_[3][13]*SF[3] + P_[0][13]*SPP[0] + P_[13][13]*SPP[4] - P_[14][13]*SPP[7] - P_[15][13]*SPP[1];
nextP[7][13] = P_[7][13] + P_[4][13]*dt;
nextP[8][13] = P_[8][13] + P_[5][13]*dt;
nextP[9][13] = P_[9][13] + P_[6][13]*dt;
nextP[10][13] = P_[10][13];
nextP[11][13] = P_[11][13];
nextP[12][13] = P_[12][13];
nextP[13][13] = P_[13][13];
nextP[0][14] = P_[0][14] + P_[1][14]*SF[9] + P_[2][14]*SF[11] + P_[3][14]*SF[10] + P_[10][14]*SF[14] + P_[11][14]*SF[15] + P_[12][14]*SPP[10];
nextP[1][14] = P_[1][14] + P_[0][14]*SF[8] + P_[2][14]*SF[7] + P_[3][14]*SF[11] - P_[12][14]*SF[15] + P_[11][14]*SPP[10] - (P_[10][14]*q0)/2;
nextP[2][14] = P_[2][14] + P_[0][14]*SF[6] + P_[1][14]*SF[10] + P_[3][14]*SF[8] + P_[12][14]*SF[14] - P_[10][14]*SPP[10] - (P_[11][14]*q0)/2;
nextP[3][14] = P_[3][14] + P_[0][14]*SF[7] + P_[1][14]*SF[6] + P_[2][14]*SF[9] + P_[10][14]*SF[15] - P_[11][14]*SF[14] - (P_[12][14]*q0)/2;
nextP[4][14] = P_[4][14] + P_[0][14]*SF[5] + P_[1][14]*SF[3] - P_[3][14]*SF[4] + P_[2][14]*SPP[0] + P_[13][14]*SPP[3] + P_[14][14]*SPP[6] - P_[15][14]*SPP[9];
nextP[5][14] = P_[5][14] + P_[0][14]*SF[4] + P_[2][14]*SF[3] + P_[3][14]*SF[5] - P_[1][14]*SPP[0] - P_[13][14]*SPP[8] + P_[14][14]*SPP[2] + P_[15][14]*SPP[5];
nextP[6][14] = P_[6][14] + P_[1][14]*SF[4] - P_[2][14]*SF[5] + P_[3][14]*SF[3] + P_[0][14]*SPP[0] + P_[13][14]*SPP[4] - P_[14][14]*SPP[7] - P_[15][14]*SPP[1];
nextP[7][14] = P_[7][14] + P_[4][14]*dt;
nextP[8][14] = P_[8][14] + P_[5][14]*dt;
nextP[9][14] = P_[9][14] + P_[6][14]*dt;
nextP[10][14] = P_[10][14];
nextP[11][14] = P_[11][14];
nextP[12][14] = P_[12][14];
nextP[13][14] = P_[13][14];
nextP[14][14] = P_[14][14];
nextP[0][15] = P_[0][15] + P_[1][15]*SF[9] + P_[2][15]*SF[11] + P_[3][15]*SF[10] + P_[10][15]*SF[14] + P_[11][15]*SF[15] + P_[12][15]*SPP[10];
nextP[1][15] = P_[1][15] + P_[0][15]*SF[8] + P_[2][15]*SF[7] + P_[3][15]*SF[11] - P_[12][15]*SF[15] + P_[11][15]*SPP[10] - (P_[10][15]*q0)/2;
nextP[2][15] = P_[2][15] + P_[0][15]*SF[6] + P_[1][15]*SF[10] + P_[3][15]*SF[8] + P_[12][15]*SF[14] - P_[10][15]*SPP[10] - (P_[11][15]*q0)/2;
nextP[3][15] = P_[3][15] + P_[0][15]*SF[7] + P_[1][15]*SF[6] + P_[2][15]*SF[9] + P_[10][15]*SF[15] - P_[11][15]*SF[14] - (P_[12][15]*q0)/2;
nextP[4][15] = P_[4][15] + P_[0][15]*SF[5] + P_[1][15]*SF[3] - P_[3][15]*SF[4] + P_[2][15]*SPP[0] + P_[13][15]*SPP[3] + P_[14][15]*SPP[6] - P_[15][15]*SPP[9];
nextP[5][15] = P_[5][15] + P_[0][15]*SF[4] + P_[2][15]*SF[3] + P_[3][15]*SF[5] - P_[1][15]*SPP[0] - P_[13][15]*SPP[8] + P_[14][15]*SPP[2] + P_[15][15]*SPP[5];
nextP[6][15] = P_[6][15] + P_[1][15]*SF[4] - P_[2][15]*SF[5] + P_[3][15]*SF[3] + P_[0][15]*SPP[0] + P_[13][15]*SPP[4] - P_[14][15]*SPP[7] - P_[15][15]*SPP[1];
nextP[7][15] = P_[7][15] + P_[4][15]*dt;
nextP[8][15] = P_[8][15] + P_[5][15]*dt;
nextP[9][15] = P_[9][15] + P_[6][15]*dt;
nextP[10][15] = P_[10][15];
nextP[11][15] = P_[11][15];
nextP[12][15] = P_[12][15];
nextP[13][15] = P_[13][15];
nextP[14][15] = P_[14][15];
nextP[15][15] = P_[15][15];
// add process noise that is not from the IMU
for (unsigned i = 13; i <= 15; i++) {
nextP[i][i] += process_noise[i];
}
// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
if (mag_3D_) {
// calculate variances and upper diagonal covariances for earth and body magnetic field states
nextP[0][16] = P_[0][16] + P_[1][16]*SF[9] + P_[2][16]*SF[11] + P_[3][16]*SF[10] + P_[10][16]*SF[14] + P_[11][16]*SF[15] + P_[12][16]*SPP[10];
nextP[1][16] = P_[1][16] + P_[0][16]*SF[8] + P_[2][16]*SF[7] + P_[3][16]*SF[11] - P_[12][16]*SF[15] + P_[11][16]*SPP[10] - (P_[10][16]*q0)/2;
nextP[2][16] = P_[2][16] + P_[0][16]*SF[6] + P_[1][16]*SF[10] + P_[3][16]*SF[8] + P_[12][16]*SF[14] - P_[10][16]*SPP[10] - (P_[11][16]*q0)/2;
nextP[3][16] = P_[3][16] + P_[0][16]*SF[7] + P_[1][16]*SF[6] + P_[2][16]*SF[9] + P_[10][16]*SF[15] - P_[11][16]*SF[14] - (P_[12][16]*q0)/2;
nextP[4][16] = P_[4][16] + P_[0][16]*SF[5] + P_[1][16]*SF[3] - P_[3][16]*SF[4] + P_[2][16]*SPP[0] + P_[13][16]*SPP[3] + P_[14][16]*SPP[6] - P_[15][16]*SPP[9];
nextP[5][16] = P_[5][16] + P_[0][16]*SF[4] + P_[2][16]*SF[3] + P_[3][16]*SF[5] - P_[1][16]*SPP[0] - P_[13][16]*SPP[8] + P_[14][16]*SPP[2] + P_[15][16]*SPP[5];
nextP[6][16] = P_[6][16] + P_[1][16]*SF[4] - P_[2][16]*SF[5] + P_[3][16]*SF[3] + P_[0][16]*SPP[0] + P_[13][16]*SPP[4] - P_[14][16]*SPP[7] - P_[15][16]*SPP[1];
nextP[7][16] = P_[7][16] + P_[4][16]*dt;
nextP[8][16] = P_[8][16] + P_[5][16]*dt;
nextP[9][16] = P_[9][16] + P_[6][16]*dt;
nextP[10][16] = P_[10][16];
nextP[11][16] = P_[11][16];
nextP[12][16] = P_[12][16];
nextP[13][16] = P_[13][16];
nextP[14][16] = P_[14][16];
nextP[15][16] = P_[15][16];
nextP[16][16] = P_[16][16];
nextP[0][17] = P_[0][17] + P_[1][17]*SF[9] + P_[2][17]*SF[11] + P_[3][17]*SF[10] + P_[10][17]*SF[14] + P_[11][17]*SF[15] + P_[12][17]*SPP[10];
nextP[1][17] = P_[1][17] + P_[0][17]*SF[8] + P_[2][17]*SF[7] + P_[3][17]*SF[11] - P_[12][17]*SF[15] + P_[11][17]*SPP[10] - (P_[10][17]*q0)/2;
nextP[2][17] = P_[2][17] + P_[0][17]*SF[6] + P_[1][17]*SF[10] + P_[3][17]*SF[8] + P_[12][17]*SF[14] - P_[10][17]*SPP[10] - (P_[11][17]*q0)/2;
nextP[3][17] = P_[3][17] + P_[0][17]*SF[7] + P_[1][17]*SF[6] + P_[2][17]*SF[9] + P_[10][17]*SF[15] - P_[11][17]*SF[14] - (P_[12][17]*q0)/2;
nextP[4][17] = P_[4][17] + P_[0][17]*SF[5] + P_[1][17]*SF[3] - P_[3][17]*SF[4] + P_[2][17]*SPP[0] + P_[13][17]*SPP[3] + P_[14][17]*SPP[6] - P_[15][17]*SPP[9];
nextP[5][17] = P_[5][17] + P_[0][17]*SF[4] + P_[2][17]*SF[3] + P_[3][17]*SF[5] - P_[1][17]*SPP[0] - P_[13][17]*SPP[8] + P_[14][17]*SPP[2] + P_[15][17]*SPP[5];
nextP[6][17] = P_[6][17] + P_[1][17]*SF[4] - P_[2][17]*SF[5] + P_[3][17]*SF[3] + P_[0][17]*SPP[0] + P_[13][17]*SPP[4] - P_[14][17]*SPP[7] - P_[15][17]*SPP[1];
nextP[7][17] = P_[7][17] + P_[4][17]*dt;
nextP[8][17] = P_[8][17] + P_[5][17]*dt;
nextP[9][17] = P_[9][17] + P_[6][17]*dt;
nextP[10][17] = P_[10][17];
nextP[11][17] = P_[11][17];
nextP[12][17] = P_[12][17];
nextP[13][17] = P_[13][17];
nextP[14][17] = P_[14][17];
nextP[15][17] = P_[15][17];
nextP[16][17] = P_[16][17];
nextP[17][17] = P_[17][17];
nextP[0][18] = P_[0][18] + P_[1][18]*SF[9] + P_[2][18]*SF[11] + P_[3][18]*SF[10] + P_[10][18]*SF[14] + P_[11][18]*SF[15] + P_[12][18]*SPP[10];
nextP[1][18] = P_[1][18] + P_[0][18]*SF[8] + P_[2][18]*SF[7] + P_[3][18]*SF[11] - P_[12][18]*SF[15] + P_[11][18]*SPP[10] - (P_[10][18]*q0)/2;
nextP[2][18] = P_[2][18] + P_[0][18]*SF[6] + P_[1][18]*SF[10] + P_[3][18]*SF[8] + P_[12][18]*SF[14] - P_[10][18]*SPP[10] - (P_[11][18]*q0)/2;
nextP[3][18] = P_[3][18] + P_[0][18]*SF[7] + P_[1][18]*SF[6] + P_[2][18]*SF[9] + P_[10][18]*SF[15] - P_[11][18]*SF[14] - (P_[12][18]*q0)/2;
nextP[4][18] = P_[4][18] + P_[0][18]*SF[5] + P_[1][18]*SF[3] - P_[3][18]*SF[4] + P_[2][18]*SPP[0] + P_[13][18]*SPP[3] + P_[14][18]*SPP[6] - P_[15][18]*SPP[9];
nextP[5][18] = P_[5][18] + P_[0][18]*SF[4] + P_[2][18]*SF[3] + P_[3][18]*SF[5] - P_[1][18]*SPP[0] - P_[13][18]*SPP[8] + P_[14][18]*SPP[2] + P_[15][18]*SPP[5];
nextP[6][18] = P_[6][18] + P_[1][18]*SF[4] - P_[2][18]*SF[5] + P_[3][18]*SF[3] + P_[0][18]*SPP[0] + P_[13][18]*SPP[4] - P_[14][18]*SPP[7] - P_[15][18]*SPP[1];
nextP[7][18] = P_[7][18] + P_[4][18]*dt;
nextP[8][18] = P_[8][18] + P_[5][18]*dt;
nextP[9][18] = P_[9][18] + P_[6][18]*dt;
nextP[10][18] = P_[10][18];
nextP[11][18] = P_[11][18];
nextP[12][18] = P_[12][18];
nextP[13][18] = P_[13][18];
nextP[14][18] = P_[14][18];
nextP[15][18] = P_[15][18];
nextP[16][18] = P_[16][18];
nextP[17][18] = P_[17][18];
nextP[18][18] = P_[18][18];
nextP[0][19] = P_[0][19] + P_[1][19]*SF[9] + P_[2][19]*SF[11] + P_[3][19]*SF[10] + P_[10][19]*SF[14] + P_[11][19]*SF[15] + P_[12][19]*SPP[10];
nextP[1][19] = P_[1][19] + P_[0][19]*SF[8] + P_[2][19]*SF[7] + P_[3][19]*SF[11] - P_[12][19]*SF[15] + P_[11][19]*SPP[10] - (P_[10][19]*q0)/2;
nextP[2][19] = P_[2][19] + P_[0][19]*SF[6] + P_[1][19]*SF[10] + P_[3][19]*SF[8] + P_[12][19]*SF[14] - P_[10][19]*SPP[10] - (P_[11][19]*q0)/2;
nextP[3][19] = P_[3][19] + P_[0][19]*SF[7] + P_[1][19]*SF[6] + P_[2][19]*SF[9] + P_[10][19]*SF[15] - P_[11][19]*SF[14] - (P_[12][19]*q0)/2;
nextP[4][19] = P_[4][19] + P_[0][19]*SF[5] + P_[1][19]*SF[3] - P_[3][19]*SF[4] + P_[2][19]*SPP[0] + P_[13][19]*SPP[3] + P_[14][19]*SPP[6] - P_[15][19]*SPP[9];
nextP[5][19] = P_[5][19] + P_[0][19]*SF[4] + P_[2][19]*SF[3] + P_[3][19]*SF[5] - P_[1][19]*SPP[0] - P_[13][19]*SPP[8] + P_[14][19]*SPP[2] + P_[15][19]*SPP[5];
nextP[6][19] = P_[6][19] + P_[1][19]*SF[4] - P_[2][19]*SF[5] + P_[3][19]*SF[3] + P_[0][19]*SPP[0] + P_[13][19]*SPP[4] - P_[14][19]*SPP[7] - P_[15][19]*SPP[1];
nextP[7][19] = P_[7][19] + P_[4][19]*dt;
nextP[8][19] = P_[8][19] + P_[5][19]*dt;
nextP[9][19] = P_[9][19] + P_[6][19]*dt;
nextP[10][19] = P_[10][19];
nextP[11][19] = P_[11][19];
nextP[12][19] = P_[12][19];
nextP[13][19] = P_[13][19];
nextP[14][19] = P_[14][19];
nextP[15][19] = P_[15][19];
nextP[16][19] = P_[16][19];
nextP[17][19] = P_[17][19];
nextP[18][19] = P_[18][19];
nextP[19][19] = P_[19][19];
nextP[0][20] = P_[0][20] + P_[1][20]*SF[9] + P_[2][20]*SF[11] + P_[3][20]*SF[10] + P_[10][20]*SF[14] + P_[11][20]*SF[15] + P_[12][20]*SPP[10];
nextP[1][20] = P_[1][20] + P_[0][20]*SF[8] + P_[2][20]*SF[7] + P_[3][20]*SF[11] - P_[12][20]*SF[15] + P_[11][20]*SPP[10] - (P_[10][20]*q0)/2;
nextP[2][20] = P_[2][20] + P_[0][20]*SF[6] + P_[1][20]*SF[10] + P_[3][20]*SF[8] + P_[12][20]*SF[14] - P_[10][20]*SPP[10] - (P_[11][20]*q0)/2;
nextP[3][20] = P_[3][20] + P_[0][20]*SF[7] + P_[1][20]*SF[6] + P_[2][20]*SF[9] + P_[10][20]*SF[15] - P_[11][20]*SF[14] - (P_[12][20]*q0)/2;
nextP[4][20] = P_[4][20] + P_[0][20]*SF[5] + P_[1][20]*SF[3] - P_[3][20]*SF[4] + P_[2][20]*SPP[0] + P_[13][20]*SPP[3] + P_[14][20]*SPP[6] - P_[15][20]*SPP[9];
nextP[5][20] = P_[5][20] + P_[0][20]*SF[4] + P_[2][20]*SF[3] + P_[3][20]*SF[5] - P_[1][20]*SPP[0] - P_[13][20]*SPP[8] + P_[14][20]*SPP[2] + P_[15][20]*SPP[5];
nextP[6][20] = P_[6][20] + P_[1][20]*SF[4] - P_[2][20]*SF[5] + P_[3][20]*SF[3] + P_[0][20]*SPP[0] + P_[13][20]*SPP[4] - P_[14][20]*SPP[7] - P_[15][20]*SPP[1];
nextP[7][20] = P_[7][20] + P_[4][20]*dt;
nextP[8][20] = P_[8][20] + P_[5][20]*dt;
nextP[9][20] = P_[9][20] + P_[6][20]*dt;
nextP[10][20] = P_[10][20];
nextP[11][20] = P_[11][20];
nextP[12][20] = P_[12][20];
nextP[13][20] = P_[13][20];
nextP[14][20] = P_[14][20];
nextP[15][20] = P_[15][20];
nextP[16][20] = P_[16][20];
nextP[17][20] = P_[17][20];
nextP[18][20] = P_[18][20];
nextP[19][20] = P_[19][20];
nextP[20][20] = P_[20][20];
nextP[0][21] = P_[0][21] + P_[1][21]*SF[9] + P_[2][21]*SF[11] + P_[3][21]*SF[10] + P_[10][21]*SF[14] + P_[11][21]*SF[15] + P_[12][21]*SPP[10];
nextP[1][21] = P_[1][21] + P_[0][21]*SF[8] + P_[2][21]*SF[7] + P_[3][21]*SF[11] - P_[12][21]*SF[15] + P_[11][21]*SPP[10] - (P_[10][21]*q0)/2;
nextP[2][21] = P_[2][21] + P_[0][21]*SF[6] + P_[1][21]*SF[10] + P_[3][21]*SF[8] + P_[12][21]*SF[14] - P_[10][21]*SPP[10] - (P_[11][21]*q0)/2;
nextP[3][21] = P_[3][21] + P_[0][21]*SF[7] + P_[1][21]*SF[6] + P_[2][21]*SF[9] + P_[10][21]*SF[15] - P_[11][21]*SF[14] - (P_[12][21]*q0)/2;
nextP[4][21] = P_[4][21] + P_[0][21]*SF[5] + P_[1][21]*SF[3] - P_[3][21]*SF[4] + P_[2][21]*SPP[0] + P_[13][21]*SPP[3] + P_[14][21]*SPP[6] - P_[15][21]*SPP[9];
nextP[5][21] = P_[5][21] + P_[0][21]*SF[4] + P_[2][21]*SF[3] + P_[3][21]*SF[5] - P_[1][21]*SPP[0] - P_[13][21]*SPP[8] + P_[14][21]*SPP[2] + P_[15][21]*SPP[5];
nextP[6][21] = P_[6][21] + P_[1][21]*SF[4] - P_[2][21]*SF[5] + P_[3][21]*SF[3] + P_[0][21]*SPP[0] + P_[13][21]*SPP[4] - P_[14][21]*SPP[7] - P_[15][21]*SPP[1];
nextP[7][21] = P_[7][21] + P_[4][21]*dt;
nextP[8][21] = P_[8][21] + P_[5][21]*dt;
nextP[9][21] = P_[9][21] + P_[6][21]*dt;
nextP[10][21] = P_[10][21];
nextP[11][21] = P_[11][21];
nextP[12][21] = P_[12][21];
nextP[13][21] = P_[13][21];
nextP[14][21] = P_[14][21];
nextP[15][21] = P_[15][21];
nextP[16][21] = P_[16][21];
nextP[17][21] = P_[17][21];
nextP[18][21] = P_[18][21];
nextP[19][21] = P_[19][21];
nextP[20][21] = P_[20][21];
nextP[21][21] = P_[21][21];
// add process noise that is not from the IMU
for (unsigned i = 16; i <= 21; i++) {
nextP[i][i] += process_noise[i];
}
}
// stop position covariance growth if our total position variance reaches 100m
if ((P_[7][7] + P_[8][8]) > 1.0e4) {
for (uint8_t i = 7; i <= 8; i++) {
for (uint8_t j = 0; j < k_num_states_; j++) {
nextP[i][j] = P_[i][j];
nextP[j][i] = P_[j][i];
}
}
}
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 1; row < k_num_states_; row++) {
for (unsigned column = 0 ; column < row; column++) {
P_[row][column] = P_[column][row] = nextP[column][row];
}
}
// copy variances (diagonals)
for (unsigned i = 0; i < k_num_states_; i++) {
P_[i][i] = nextP[i][i];
}
fixCovarianceErrors();
}
void ESKF::controlFusionModes() {
gps_data_ready_ = gps_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &gps_sample_delayed_);
vision_data_ready_ = ext_vision_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &ev_sample_delayed_);
flow_data_ready_ = opt_flow_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &opt_flow_sample_delayed_);
mag_data_ready_ = mag_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &mag_sample_delayed_);
R_rng_to_earth_2_2_ = R_to_earth_(2, 0) * sin_tilt_rng_ + R_to_earth_(2, 2) * cos_tilt_rng_;
range_data_ready_ = range_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &range_sample_delayed_) && (R_rng_to_earth_2_2_ > range_cos_max_tilt_);
controlHeightSensorTimeouts();
// For efficiency, fusion of direct state observations for position and velocity is performed sequentially
// in a single function using sensor data from multiple sources
controlVelPosFusion();
controlExternalVisionFusion();
controlGpsFusion();
controlOpticalFlowFusion();
controlMagFusion();
runTerrainEstimator();
}
void ESKF::controlOpticalFlowFusion() {
// Accumulate autopilot gyro data across the same time interval as the flow sensor
imu_del_ang_of_ += imu_sample_delayed_.delta_ang - state_.gyro_bias;
delta_time_of_ += imu_sample_delayed_.delta_ang_dt;
if(flow_data_ready_) {
// we are not yet using flow data
if ((fusion_mask_ & MASK_OPTICAL_FLOW) && (!opt_flow_)) {
opt_flow_ = true;
printf("ESKF commencing optical flow fusion\n");
}
// Only fuse optical flow if valid body rate compensation data is available
if (calcOptFlowBodyRateComp()) {
bool flow_quality_good = (opt_flow_sample_delayed_.quality >= flow_quality_min_);
if (!flow_quality_good && !in_air_) {
// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
flow_rad_xy_comp_ = vec2(0,0);
} else {
// compensate for body motion to give a LOS rate
flow_rad_xy_comp_(0) = opt_flow_sample_delayed_.flowRadXY(0) - opt_flow_sample_delayed_.gyroXY(0);
flow_rad_xy_comp_(1) = opt_flow_sample_delayed_.flowRadXY(1) - opt_flow_sample_delayed_.gyroXY(1);
}
} else {
// don't use this flow data and wait for the next data to arrive
flow_data_ready_ = false;
}
}
// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
if (flow_data_ready_ && (imu_sample_delayed_.time_us > opt_flow_sample_delayed_.time_us - uint32_t(1e6f * opt_flow_sample_delayed_.dt) / 2)) {
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently but use a relaxed time criteria to enable it to coast through bad range finder data
if (opt_flow_ && (time_last_imu_ - time_last_hagl_fuse_ < (uint64_t)10e6)) {
fuseOptFlow();
}
flow_data_ready_ = false;
}
}
// calculate optical flow body angular rate compensation
// returns false if bias corrected body rate data is unavailable
bool ESKF::calcOptFlowBodyRateComp() {
// reset the accumulators if the time interval is too large
if (delta_time_of_ > 1.0f) {
imu_del_ang_of_.setZero();
delta_time_of_ = 0.0f;
return false;
}
// Use the EKF gyro data if optical flow sensor gyro data is not available for clarification of the sign see definition of flowSample and imuSample in common.h
opt_flow_sample_delayed_.gyroXY(0) = -imu_del_ang_of_(0);
opt_flow_sample_delayed_.gyroXY(1) = -imu_del_ang_of_(1);
// reset the accumulators
imu_del_ang_of_.setZero();
delta_time_of_ = 0.0f;
return true;
}
void ESKF::runTerrainEstimator() {
// Perform initialisation check
if (!terrain_initialised_) {
terrain_initialised_ = initHagl();
} else {
// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
// process noise due to errors in vehicle height estimate
terrain_var_ += sq(imu_sample_delayed_.delta_vel_dt * terrain_p_noise_);
// process noise due to terrain gradient
terrain_var_ += sq(imu_sample_delayed_.delta_vel_dt * terrain_gradient_) * (sq(state_.vel(0)) + sq(state_.vel(1)));
// limit the variance to prevent it becoming badly conditioned
terrain_var_ = constrain(terrain_var_, 0.0f, 1e4f);
// Fuse range finder data if available
if (range_data_ready_) {
// determine if we should use the hgt observation
if ((fusion_mask_ & MASK_RANGEFINDER) && (!rng_hgt_)) {
if (time_last_imu_ - time_last_range_ < 2 * RANGE_MAX_INTERVAL) {
rng_hgt_ = true;
printf("ESKF commencing rng fusion\n");
}
}
if(rng_hgt_) {
fuseHagl();
}
// update range sensor angle parameters in case they have changed
// we do this here to avoid doing those calculations at a high rate
sin_tilt_rng_ = sinf(rng_sens_pitch_);
cos_tilt_rng_ = cosf(rng_sens_pitch_);
fuse_height_ = true;
}
//constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
if (terrain_vpos_ - state_.pos(2) < rng_gnd_clearance_) {
terrain_vpos_ = rng_gnd_clearance_ + state_.pos(2);
}
}
}
void ESKF::fuseHagl() {
// If the vehicle is excessively tilted, do not try to fuse range finder observations
if (R_rng_to_earth_2_2_ > range_cos_max_tilt_) {
// get a height above ground measurement from the range finder assuming a flat earth
scalar_t meas_hagl = range_sample_delayed_.rng * R_rng_to_earth_2_2_;
// predict the hagl from the vehicle position and terrain height
scalar_t pred_hagl = terrain_vpos_ - state_.pos(2);
// calculate the innovation
hagl_innov_ = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty
scalar_t obs_variance = fmaxf(P_[9][9], 0.0f) + sq(range_noise_) + sq(range_noise_scaler_ * range_sample_delayed_.rng);
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
hagl_innov_var_ = fmaxf(terrain_var_ + obs_variance, obs_variance);
// perform an innovation consistency check and only fuse data if it passes
scalar_t gate_size = fmaxf(range_innov_gate_, 1.0f);
terr_test_ratio_ = sq(hagl_innov_) / (sq(gate_size) * hagl_innov_var_);
if (terr_test_ratio_ <= 1.0f) {
// calculate the Kalman gain
scalar_t gain = terrain_var_ / hagl_innov_var_;
// correct the state
terrain_vpos_ -= gain * hagl_innov_;
// correct the variance
terrain_var_ = fmaxf(terrain_var_ * (1.0f - gain), 0.0f);
// record last successful fusion event
time_last_hagl_fuse_ = time_last_imu_;
} else {
// If we have been rejecting range data for too long, reset to measurement
if (time_last_imu_ - time_last_hagl_fuse_ > (uint64_t)10E6) {
terrain_vpos_ = state_.pos(2) + meas_hagl;
terrain_var_ = obs_variance;
}
}
}
}
void ESKF::fuseOptFlow() {
scalar_t optflow_test_ratio[2] = {0};
// get latest estimated orientation
scalar_t q0 = state_.quat_nominal.w();
scalar_t q1 = state_.quat_nominal.x();
scalar_t q2 = state_.quat_nominal.y();
scalar_t q3 = state_.quat_nominal.z();
// get latest velocity in earth frame
scalar_t vn = state_.vel(0);
scalar_t ve = state_.vel(1);
scalar_t vd = state_.vel(2);
// calculate the optical flow observation variance
// calculate the observation noise variance - scaling noise linearly across flow quality range
scalar_t R_LOS_best = fmaxf(flow_noise_, 0.05f);
scalar_t R_LOS_worst = fmaxf(flow_noise_qual_min_, 0.05f);
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
scalar_t weighting = (255.0f - (scalar_t)flow_quality_min_);
if (weighting >= 1.0f) {
weighting = constrain(((scalar_t)opt_flow_sample_delayed_.quality - (scalar_t)flow_quality_min_) / weighting, 0.0f, 1.0f);
} else {
weighting = 0.0f;
}
// take the weighted average of the observation noie for the best and wort flow quality
scalar_t R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
scalar_t H_LOS[2][k_num_states_] = {}; // Optical flow observation Jacobians
scalar_t Kfusion[k_num_states_][2] = {}; // Optical flow Kalman gains
// constrain height above ground to be above minimum height when sitting on ground
scalar_t heightAboveGndEst = max((terrain_vpos_ - state_.pos(2)), rng_gnd_clearance_);
mat3 earth_to_body = R_to_earth_.transpose();
// rotate into body frame
vec3 vel_body = earth_to_body * state_.vel;
// calculate range from focal point to centre of image
scalar_t range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis.
vec2 opt_flow_rate;
opt_flow_rate(0) = flow_rad_xy_comp_(0) / opt_flow_sample_delayed_.dt;// + _flow_gyro_bias(0);
opt_flow_rate(1) = flow_rad_xy_comp_(1) / opt_flow_sample_delayed_.dt;// + _flow_gyro_bias(1);
if (opt_flow_rate.norm() < flow_max_rate_) {
flow_innov_[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
flow_innov_[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
} else {
return;
}
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
// Calculate Obser ation Jacobians and Kalman gans for each measurement axis
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (obs_index == 0) {
// calculate X axis observation Jacobian
float t2 = 1.0f / range;
H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
float t3 = q1*vd*2.0f;
float t4 = q0*ve*2.0f;
float t11 = q3*vn*2.0f;
float t5 = t3+t4-t11;
float t6 = q0*q3*2.0f;
float t29 = q1*q2*2.0f;
float t7 = t6-t29;
float t8 = q0*q1*2.0f;
float t9 = q2*q3*2.0f;
float t10 = t8+t9;
float t12 = P_[0][0]*t2*t5;
float t13 = q0*vd*2.0f;
float t14 = q2*vn*2.0f;
float t28 = q1*ve*2.0f;
float t15 = t13+t14-t28;
float t16 = q3*vd*2.0f;
float t17 = q2*ve*2.0f;
float t18 = q1*vn*2.0f;
float t19 = t16+t17+t18;
float t20 = q3*ve*2.0f;
float t21 = q0*vn*2.0f;
float t30 = q2*vd*2.0f;
float t22 = t20+t21-t30;
float t23 = q0*q0;
float t24 = q1*q1;
float t25 = q2*q2;
float t26 = q3*q3;
float t27 = t23-t24+t25-t26;
float t31 = P_[1][1]*t2*t15;
float t32 = P_[6][0]*t2*t10;
float t33 = P_[1][0]*t2*t15;
float t34 = P_[2][0]*t2*t19;
float t35 = P_[5][0]*t2*t27;
float t79 = P_[4][0]*t2*t7;
float t80 = P_[3][0]*t2*t22;
float t36 = t12+t32+t33+t34+t35-t79-t80;
float t37 = t2*t5*t36;
float t38 = P_[6][1]*t2*t10;
float t39 = P_[0][1]*t2*t5;
float t40 = P_[2][1]*t2*t19;
float t41 = P_[5][1]*t2*t27;
float t81 = P_[4][1]*t2*t7;
float t82 = P_[3][1]*t2*t22;
float t42 = t31+t38+t39+t40+t41-t81-t82;
float t43 = t2*t15*t42;
float t44 = P_[6][2]*t2*t10;
float t45 = P_[0][2]*t2*t5;
float t46 = P_[1][2]*t2*t15;
float t47 = P_[2][2]*t2*t19;
float t48 = P_[5][2]*t2*t27;
float t83 = P_[4][2]*t2*t7;
float t84 = P_[3][2]*t2*t22;
float t49 = t44+t45+t46+t47+t48-t83-t84;
float t50 = t2*t19*t49;
float t51 = P_[6][3]*t2*t10;
float t52 = P_[0][3]*t2*t5;
float t53 = P_[1][3]*t2*t15;
float t54 = P_[2][3]*t2*t19;
float t55 = P_[5][3]*t2*t27;
float t85 = P_[4][3]*t2*t7;
float t86 = P_[3][3]*t2*t22;
float t56 = t51+t52+t53+t54+t55-t85-t86;
float t57 = P_[6][5]*t2*t10;
float t58 = P_[0][5]*t2*t5;
float t59 = P_[1][5]*t2*t15;
float t60 = P_[2][5]*t2*t19;
float t61 = P_[5][5]*t2*t27;
float t88 = P_[4][5]*t2*t7;
float t89 = P_[3][5]*t2*t22;
float t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62;
float t64 = P_[6][4]*t2*t10;
float t65 = P_[0][4]*t2*t5;
float t66 = P_[1][4]*t2*t15;
float t67 = P_[2][4]*t2*t19;
float t68 = P_[5][4]*t2*t27;
float t90 = P_[4][4]*t2*t7;
float t91 = P_[3][4]*t2*t22;
float t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = P_[6][6]*t2*t10;
float t71 = P_[0][6]*t2*t5;
float t72 = P_[1][6]*t2*t15;
float t73 = P_[2][6]*t2*t19;
float t74 = P_[5][6]*t2*t27;
float t93 = P_[4][6]*t2*t7;
float t94 = P_[3][6]*t2*t22;
float t75 = t70+t71+t72+t73+t74-t93-t94;
float t76 = t2*t10*t75;
float t87 = t2*t22*t56;
float t92 = t2*t7*t69;
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
float t78;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
flow_innov_var_[0] = t77;
} else {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
// calculate Kalman gains for X-axis observation
Kfusion[0][0] = t78*(t12-P_[0][4]*t2*t7+P_[0][1]*t2*t15+P_[0][6]*t2*t10+P_[0][2]*t2*t19-P_[0][3]*t2*t22+P_[0][5]*t2*t27);
Kfusion[1][0] = t78*(t31+P_[1][0]*t2*t5-P_[1][4]*t2*t7+P_[1][6]*t2*t10+P_[1][2]*t2*t19-P_[1][3]*t2*t22+P_[1][5]*t2*t27);
Kfusion[2][0] = t78*(t47+P_[2][0]*t2*t5-P_[2][4]*t2*t7+P_[2][1]*t2*t15+P_[2][6]*t2*t10-P_[2][3]*t2*t22+P_[2][5]*t2*t27);
Kfusion[3][0] = t78*(-t86+P_[3][0]*t2*t5-P_[3][4]*t2*t7+P_[3][1]*t2*t15+P_[3][6]*t2*t10+P_[3][2]*t2*t19+P_[3][5]*t2*t27);
Kfusion[4][0] = t78*(-t90+P_[4][0]*t2*t5+P_[4][1]*t2*t15+P_[4][6]*t2*t10+P_[4][2]*t2*t19-P_[4][3]*t2*t22+P_[4][5]*t2*t27);
Kfusion[5][0] = t78*(t61+P_[5][0]*t2*t5-P_[5][4]*t2*t7+P_[5][1]*t2*t15+P_[5][6]*t2*t10+P_[5][2]*t2*t19-P_[5][3]*t2*t22);
Kfusion[6][0] = t78*(t70+P_[6][0]*t2*t5-P_[6][4]*t2*t7+P_[6][1]*t2*t15+P_[6][2]*t2*t19-P_[6][3]*t2*t22+P_[6][5]*t2*t27);
Kfusion[7][0] = t78*(P_[7][0]*t2*t5-P_[7][4]*t2*t7+P_[7][1]*t2*t15+P_[7][6]*t2*t10+P_[7][2]*t2*t19-P_[7][3]*t2*t22+P_[7][5]*t2*t27);
Kfusion[8][0] = t78*(P_[8][0]*t2*t5-P_[8][4]*t2*t7+P_[8][1]*t2*t15+P_[8][6]*t2*t10+P_[8][2]*t2*t19-P_[8][3]*t2*t22+P_[8][5]*t2*t27);
Kfusion[9][0] = t78*(P_[9][0]*t2*t5-P_[9][4]*t2*t7+P_[9][1]*t2*t15+P_[9][6]*t2*t10+P_[9][2]*t2*t19-P_[9][3]*t2*t22+P_[9][5]*t2*t27);
Kfusion[10][0] = t78*(P_[10][0]*t2*t5-P_[10][4]*t2*t7+P_[10][1]*t2*t15+P_[10][6]*t2*t10+P_[10][2]*t2*t19-P_[10][3]*t2*t22+P_[10][5]*t2*t27);
Kfusion[11][0] = t78*(P_[11][0]*t2*t5-P_[11][4]*t2*t7+P_[11][1]*t2*t15+P_[11][6]*t2*t10+P_[11][2]*t2*t19-P_[11][3]*t2*t22+P_[11][5]*t2*t27);
Kfusion[12][0] = t78*(P_[12][0]*t2*t5-P_[12][4]*t2*t7+P_[12][1]*t2*t15+P_[12][6]*t2*t10+P_[12][2]*t2*t19-P_[12][3]*t2*t22+P_[12][5]*t2*t27);
Kfusion[13][0] = t78*(P_[13][0]*t2*t5-P_[13][4]*t2*t7+P_[13][1]*t2*t15+P_[13][6]*t2*t10+P_[13][2]*t2*t19-P_[13][3]*t2*t22+P_[13][5]*t2*t27);
Kfusion[14][0] = t78*(P_[14][0]*t2*t5-P_[14][4]*t2*t7+P_[14][1]*t2*t15+P_[14][6]*t2*t10+P_[14][2]*t2*t19-P_[14][3]*t2*t22+P_[14][5]*t2*t27);
Kfusion[15][0] = t78*(P_[15][0]*t2*t5-P_[15][4]*t2*t7+P_[15][1]*t2*t15+P_[15][6]*t2*t10+P_[15][2]*t2*t19-P_[15][3]*t2*t22+P_[15][5]*t2*t27);
// run innovation consistency checks
optflow_test_ratio[0] = sq(flow_innov_[0]) / (sq(max(flow_innov_gate_, 1.0f)) * flow_innov_var_[0]);
} else if (obs_index == 1) {
// calculate Y axis observation Jacobian
float t2 = 1.0f / range;
H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
// calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains
float t3 = q3*ve*2.0f;
float t4 = q0*vn*2.0f;
float t11 = q2*vd*2.0f;
float t5 = t3+t4-t11;
float t6 = q0*q3*2.0f;
float t7 = q1*q2*2.0f;
float t8 = t6+t7;
float t9 = q0*q2*2.0f;
float t28 = q1*q3*2.0f;
float t10 = t9-t28;
float t12 = P_[0][0]*t2*t5;
float t13 = q3*vd*2.0f;
float t14 = q2*ve*2.0f;
float t15 = q1*vn*2.0f;
float t16 = t13+t14+t15;
float t17 = q0*vd*2.0f;
float t18 = q2*vn*2.0f;
float t29 = q1*ve*2.0f;
float t19 = t17+t18-t29;
float t20 = q1*vd*2.0f;
float t21 = q0*ve*2.0f;
float t30 = q3*vn*2.0f;
float t22 = t20+t21-t30;
float t23 = q0*q0;
float t24 = q1*q1;
float t25 = q2*q2;
float t26 = q3*q3;
float t27 = t23+t24-t25-t26;
float t31 = P_[1][1]*t2*t16;
float t32 = P_[5][0]*t2*t8;
float t33 = P_[1][0]*t2*t16;
float t34 = P_[3][0]*t2*t22;
float t35 = P_[4][0]*t2*t27;
float t80 = P_[6][0]*t2*t10;
float t81 = P_[2][0]*t2*t19;
float t36 = t12+t32+t33+t34+t35-t80-t81;
float t37 = t2*t5*t36;
float t38 = P_[5][1]*t2*t8;
float t39 = P_[0][1]*t2*t5;
float t40 = P_[3][1]*t2*t22;
float t41 = P_[4][1]*t2*t27;
float t82 = P_[6][1]*t2*t10;
float t83 = P_[2][1]*t2*t19;
float t42 = t31+t38+t39+t40+t41-t82-t83;
float t43 = t2*t16*t42;
float t44 = P_[5][2]*t2*t8;
float t45 = P_[0][2]*t2*t5;
float t46 = P_[1][2]*t2*t16;
float t47 = P_[3][2]*t2*t22;
float t48 = P_[4][2]*t2*t27;
float t79 = P_[2][2]*t2*t19;
float t84 = P_[6][2]*t2*t10;
float t49 = t44+t45+t46+t47+t48-t79-t84;
float t50 = P_[5][3]*t2*t8;
float t51 = P_[0][3]*t2*t5;
float t52 = P_[1][3]*t2*t16;
float t53 = P_[3][3]*t2*t22;
float t54 = P_[4][3]*t2*t27;
float t86 = P_[6][3]*t2*t10;
float t87 = P_[2][3]*t2*t19;
float t55 = t50+t51+t52+t53+t54-t86-t87;
float t56 = t2*t22*t55;
float t57 = P_[5][4]*t2*t8;
float t58 = P_[0][4]*t2*t5;
float t59 = P_[1][4]*t2*t16;
float t60 = P_[3][4]*t2*t22;
float t61 = P_[4][4]*t2*t27;
float t88 = P_[6][4]*t2*t10;
float t89 = P_[2][4]*t2*t19;
float t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62;
float t64 = P_[5][5]*t2*t8;
float t65 = P_[0][5]*t2*t5;
float t66 = P_[1][5]*t2*t16;
float t67 = P_[3][5]*t2*t22;
float t68 = P_[4][5]*t2*t27;
float t90 = P_[6][5]*t2*t10;
float t91 = P_[2][5]*t2*t19;
float t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = t2*t8*t69;
float t71 = P_[5][6]*t2*t8;
float t72 = P_[0][6]*t2*t5;
float t73 = P_[1][6]*t2*t16;
float t74 = P_[3][6]*t2*t22;
float t75 = P_[4][6]*t2*t27;
float t92 = P_[6][6]*t2*t10;
float t93 = P_[2][6]*t2*t19;
float t76 = t71+t72+t73+t74+t75-t92-t93;
float t85 = t2*t19*t49;
float t94 = t2*t10*t76;
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
float t78;
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
flow_innov_var_[1] = t77;
} else {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
// calculate Kalman gains for Y-axis observation
Kfusion[0][1] = -t78*(t12+P_[0][5]*t2*t8-P_[0][6]*t2*t10+P_[0][1]*t2*t16-P_[0][2]*t2*t19+P_[0][3]*t2*t22+P_[0][4]*t2*t27);
Kfusion[1][1] = -t78*(t31+P_[1][0]*t2*t5+P_[1][5]*t2*t8-P_[1][6]*t2*t10-P_[1][2]*t2*t19+P_[1][3]*t2*t22+P_[1][4]*t2*t27);
Kfusion[2][1] = -t78*(-t79+P_[2][0]*t2*t5+P_[2][5]*t2*t8-P_[2][6]*t2*t10+P_[2][1]*t2*t16+P_[2][3]*t2*t22+P_[2][4]*t2*t27);
Kfusion[3][1] = -t78*(t53+P_[3][0]*t2*t5+P_[3][5]*t2*t8-P_[3][6]*t2*t10+P_[3][1]*t2*t16-P_[3][2]*t2*t19+P_[3][4]*t2*t27);
Kfusion[4][1] = -t78*(t61+P_[4][0]*t2*t5+P_[4][5]*t2*t8-P_[4][6]*t2*t10+P_[4][1]*t2*t16-P_[4][2]*t2*t19+P_[4][3]*t2*t22);
Kfusion[5][1] = -t78*(t64+P_[5][0]*t2*t5-P_[5][6]*t2*t10+P_[5][1]*t2*t16-P_[5][2]*t2*t19+P_[5][3]*t2*t22+P_[5][4]*t2*t27);
Kfusion[6][1] = -t78*(-t92+P_[6][0]*t2*t5+P_[6][5]*t2*t8+P_[6][1]*t2*t16-P_[6][2]*t2*t19+P_[6][3]*t2*t22+P_[6][4]*t2*t27);
Kfusion[7][1] = -t78*(P_[7][0]*t2*t5+P_[7][5]*t2*t8-P_[7][6]*t2*t10+P_[7][1]*t2*t16-P_[7][2]*t2*t19+P_[7][3]*t2*t22+P_[7][4]*t2*t27);
Kfusion[8][1] = -t78*(P_[8][0]*t2*t5+P_[8][5]*t2*t8-P_[8][6]*t2*t10+P_[8][1]*t2*t16-P_[8][2]*t2*t19+P_[8][3]*t2*t22+P_[8][4]*t2*t27);
Kfusion[9][1] = -t78*(P_[9][0]*t2*t5+P_[9][5]*t2*t8-P_[9][6]*t2*t10+P_[9][1]*t2*t16-P_[9][2]*t2*t19+P_[9][3]*t2*t22+P_[9][4]*t2*t27);
Kfusion[10][1] = -t78*(P_[10][0]*t2*t5+P_[10][5]*t2*t8-P_[10][6]*t2*t10+P_[10][1]*t2*t16-P_[10][2]*t2*t19+P_[10][3]*t2*t22+P_[10][4]*t2*t27);
Kfusion[11][1] = -t78*(P_[11][0]*t2*t5+P_[11][5]*t2*t8-P_[11][6]*t2*t10+P_[11][1]*t2*t16-P_[11][2]*t2*t19+P_[11][3]*t2*t22+P_[11][4]*t2*t27);
Kfusion[12][1] = -t78*(P_[12][0]*t2*t5+P_[12][5]*t2*t8-P_[12][6]*t2*t10+P_[12][1]*t2*t16-P_[12][2]*t2*t19+P_[12][3]*t2*t22+P_[12][4]*t2*t27);
Kfusion[13][1] = -t78*(P_[13][0]*t2*t5+P_[13][5]*t2*t8-P_[13][6]*t2*t10+P_[13][1]*t2*t16-P_[13][2]*t2*t19+P_[13][3]*t2*t22+P_[13][4]*t2*t27);
Kfusion[14][1] = -t78*(P_[14][0]*t2*t5+P_[14][5]*t2*t8-P_[14][6]*t2*t10+P_[14][1]*t2*t16-P_[14][2]*t2*t19+P_[14][3]*t2*t22+P_[14][4]*t2*t27);
Kfusion[15][1] = -t78*(P_[15][0]*t2*t5+P_[15][5]*t2*t8-P_[15][6]*t2*t10+P_[15][1]*t2*t16-P_[15][2]*t2*t19+P_[15][3]*t2*t22+P_[15][4]*t2*t27);
// run innovation consistency check
optflow_test_ratio[1] = sq(flow_innov_[1]) / (sq(max(flow_innov_gate_, 1.0f)) * flow_innov_var_[1]);
}
}
// record the innovation test pass/fail
bool flow_fail = false;
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (optflow_test_ratio[obs_index] > 1.0f) {
flow_fail = true;
}
}
// if either axis fails we abort the fusion
if (flow_fail) {
return;
}
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
// copy the Kalman gain vector for the axis we are fusing
float gain[k_num_states_];
for (unsigned row = 0; row <= k_num_states_ - 1; row++) {
gain[row] = Kfusion[row][obs_index];
}
// apply covariance correction via P_new = (I -K*H)*P_
// first calculate expression for KHP
// then calculate P_ - KHP
float KHP[k_num_states_][k_num_states_];
float KH[7];
for (unsigned row = 0; row < k_num_states_; row++) {
KH[0] = gain[row] * H_LOS[obs_index][0];
KH[1] = gain[row] * H_LOS[obs_index][1];
KH[2] = gain[row] * H_LOS[obs_index][2];
KH[3] = gain[row] * H_LOS[obs_index][3];
KH[4] = gain[row] * H_LOS[obs_index][4];
KH[5] = gain[row] * H_LOS[obs_index][5];
KH[6] = gain[row] * H_LOS[obs_index][6];
for (unsigned column = 0; column < k_num_states_; column++) {
float tmp = KH[0] * P_[0][column];
tmp += KH[1] * P_[1][column];
tmp += KH[2] * P_[2][column];
tmp += KH[3] * P_[3][column];
tmp += KH[4] * P_[4][column];
tmp += KH[5] * P_[5][column];
tmp += KH[6] * P_[6][column];
KHP[row][column] = tmp;
}
}
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
for (int i = 0; i < k_num_states_; i++) {
if (P_[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P_, i, i);
zeroCols(P_, i, i);
//flag as unhealthy
healthy = false;
}
}
// only apply covariance and state corrrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < k_num_states_; row++) {
for (unsigned column = 0; column < k_num_states_; column++) {
P_[row][column] = P_[row][column] - KHP[row][column];
}
}
// correct the covariance marix for gross errors
fixCovarianceErrors();
// apply the state corrections
fuse(gain, flow_innov_[obs_index]);
}
}
}
void ESKF::controlMagFusion() {
if(fusion_mask_ & MASK_MAG_INHIBIT) {
mag_use_inhibit_ = true;
fuseHeading();
} else if(mag_data_ready_) {
// determine if we should use the yaw observation
if ((fusion_mask_ & MASK_MAG_HEADING) && (!mag_hdg_)) {
if (time_last_imu_ - time_last_mag_ < 2 * MAG_INTERVAL) {
mag_hdg_ = true;
printf("ESKF commencing mag yaw fusion\n");
}
}
if(mag_hdg_) {
fuseHeading();
}
}
}
void ESKF::controlExternalVisionFusion() {
if(vision_data_ready_) {
// Fuse available NED position data into the main filter
if ((fusion_mask_ & MASK_EV_POS) && (!ev_pos_)) {
// check for an external vision measurement that has fallen behind the fusion time horizon
if (time_last_imu_ - time_last_ext_vision_ < 2 * EV_MAX_INTERVAL) {
ev_pos_ = true;
printf("ESKF commencing external vision position fusion\n");
}
// reset the position if we are not already aiding using GPS, else use a relative position method for fusing the position data
if (gps_pos_) {
//
} else {
resetPosition();
resetVelocity();
}
}
// determine if we should use the yaw observation
if ((fusion_mask_ & MASK_EV_YAW) && (!ev_yaw_)) {
if (time_last_imu_ - time_last_ext_vision_ < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
vec3 euler_init = dcm2vec(quat2dcm(state_.quat_nominal));
// get initial yaw from the observation quaternion
const extVisionSample &ev_newest = ext_vision_buffer_.get_newest();
vec3 euler_obs = dcm2vec(quat2dcm(ev_newest.quatNED));
euler_init(2) = euler_obs(2);
// calculate initial quaternion states for the ekf
state_.quat_nominal = from_axis_angle(euler_init);
ev_yaw_ = true;
printf("ESKF commencing external vision yaw fusion\n");
}
}
// determine if we should use the hgt observation
if ((fusion_mask_ & MASK_EV_HGT) && (!ev_hgt_)) {
// don't start using EV data unless data is arriving frequently
if (time_last_imu_ - time_last_ext_vision_ < 2 * EV_MAX_INTERVAL) {
ev_hgt_ = true;
printf("ESKF commencing external vision hgt fusion\n");
if(rng_hgt_) {
//
} else {
resetHeight();
}
}
}
if (ev_hgt_) {
fuse_height_ = true;
}
if (ev_pos_) {
fuse_pos_ = true;
}
if(fuse_height_ || fuse_pos_) {
fuseVelPosHeight();
fuse_pos_ = fuse_height_ = false;
}
if (ev_yaw_) {
fuseHeading();
}
}
}
void ESKF::controlGpsFusion() {
if (gps_data_ready_) {
if ((fusion_mask_ & MASK_GPS_POS) && (!gps_pos_)) {
gps_pos_ = true;
printf("ESKF commencing GPS pos fusion\n");
}
if(gps_pos_) {
fuse_pos_ = true;
fuse_vert_vel_ = true;
fuse_hor_vel_ = true;
time_last_gps_ = time_last_imu_;
}
if ((fusion_mask_ & MASK_GPS_HGT) && (!gps_hgt_)) {
gps_hgt_ = true;
printf("ESKF commencing GPS hgt fusion\n");
}
if(gps_pos_) {
fuse_height_ = true;
time_last_gps_ = time_last_imu_;
}
}
}
void ESKF::controlVelPosFusion() {
if (!gps_pos_ && !opt_flow_ && !ev_pos_) {
// Fuse synthetic position observations every 200msec
if ((time_last_imu_ - time_last_fake_gps_ > (uint64_t)2e5) || fuse_height_) {
// Reset position and velocity states if we re-commence this aiding method
if ((time_last_imu_ - time_last_fake_gps_) > (uint64_t)4e5) {
resetPosition();
resetVelocity();
if (time_last_fake_gps_ != 0) {
printf("ESKF stopping navigation\n");
}
}
fuse_pos_ = true;
fuse_hor_vel_ = false;
fuse_vert_vel_ = false;
time_last_fake_gps_ = time_last_imu_;
vel_pos_innov_[0] = 0.0f;
vel_pos_innov_[1] = 0.0f;
vel_pos_innov_[2] = 0.0f;
vel_pos_innov_[3] = state_.pos(0) - last_known_posNED_(0);
vel_pos_innov_[4] = state_.pos(1) - last_known_posNED_(1);
// glitch protection is not required so set gate to a large value
posInnovGateNE_ = 100.0f;
}
}
// Fuse available NED velocity and position data into the main filter
if (fuse_height_ || fuse_pos_ || fuse_hor_vel_ || fuse_vert_vel_) {
fuseVelPosHeight();
}
}
void ESKF::controlHeightSensorTimeouts() {
/*
* Handle the case where we have not fused height measurements recently and
* uncertainty exceeds the max allowable. Reset using the best available height
* measurement source, continue using it after the reset and declare the current
* source failed if we have switched.
*/
// check if height has been inertial deadreckoning for too long
bool hgt_fusion_timeout = ((time_last_imu_ - time_last_hgt_fuse_) > 5e6);
// reset the vertical position and velocity states
if ((P_[9][9] > sq(hgt_reset_lim)) && (hgt_fusion_timeout)) {
// boolean that indicates we will do a height reset
bool reset_height = false;
// handle the case where we are using external vision data for height
if (ev_hgt_) {
// check if vision data is available
extVisionSample ev_init = ext_vision_buffer_.get_newest();
bool ev_data_available = ((time_last_imu_ - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
// reset to ev data if it is available
bool reset_to_ev = ev_data_available;
if (reset_to_ev) {
// request a reset
reset_height = true;
} else {
// we have nothing to reset to
reset_height = false;
}
} else if (gps_hgt_) {
// check if gps data is available
gpsSample gps_init = gps_buffer_.get_newest();
bool gps_data_available = ((time_last_imu_ - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
// reset to ev data if it is available
bool reset_to_gps = gps_data_available;
if (reset_to_gps) {
// request a reset
reset_height = true;
} else {
// we have nothing to reset to
reset_height = false;
}
}
// Reset vertical position and velocity states to the last measurement
if (reset_height) {
resetHeight();
// Reset the timout timer
time_last_hgt_fuse_ = time_last_imu_;
}
}
}
void ESKF::resetPosition() {
if (gps_pos_) {
// this reset is only called if we have new gps data at the fusion time horizon
state_.pos(0) = gps_sample_delayed_.pos(0);
state_.pos(1) = gps_sample_delayed_.pos(1);
// use GPS accuracy to reset variances
setDiag(P_, 7, 8, sq(gps_sample_delayed_.hacc));
} else if (ev_pos_) {
// this reset is only called if we have new ev data at the fusion time horizon
state_.pos(0) = ev_sample_delayed_.posNED(0);
state_.pos(1) = ev_sample_delayed_.posNED(1);
// use EV accuracy to reset variances
setDiag(P_, 7, 8, sq(ev_sample_delayed_.posErr));
} else if (opt_flow_) {
if (!in_air_) {
// we are likely starting OF for the first time so reset the horizontal position
state_.pos(0) = 0.0f;
state_.pos(1) = 0.0f;
} else {
// set to the last known position
state_.pos(0) = last_known_posNED_(0);
state_.pos(1) = last_known_posNED_(1);
}
// estimate is relative to initial positon in this mode, so we start with zero error.
zeroCols(P_, 7, 8);
zeroRows(P_, 7, 8);
} else {
// Used when falling back to non-aiding mode of operation
state_.pos(0) = last_known_posNED_(0);
state_.pos(1) = last_known_posNED_(1);
setDiag(P_, 7, 8, sq(pos_noaid_noise_));
}
}
void ESKF::resetVelocity() {
}
void ESKF::resetHeight() {
// reset the vertical position
if (ev_hgt_) {
// initialize vertical position with newest measurement
extVisionSample ev_newest = ext_vision_buffer_.get_newest();
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - imu_sample_delayed_.time_us;
int32_t dt_delayed = ev_sample_delayed_.time_us - imu_sample_delayed_.time_us;
if (std::abs(dt_newest) < std::abs(dt_delayed)) {
state_.pos(2) = ev_newest.posNED(2);
} else {
state_.pos(2) = ev_sample_delayed_.posNED(2);
}
} else if(gps_hgt_) {
// Get the most recent GPS data
const gpsSample &gps_newest = gps_buffer_.get_newest();
if (time_last_imu_ - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
state_.pos(2) = gps_newest.hgt;
// reset the associated covarince values
zeroRows(P_, 9, 9);
zeroCols(P_, 9, 9);
// the state variance is the same as the observation
P_[9][9] = sq(gps_newest.hacc);
}
}
// reset the vertical velocity covariance values
zeroRows(P_, 6, 6);
zeroCols(P_, 6, 6);
// we don't know what the vertical velocity is, so set it to zero
state_.vel(2) = 0.0f;
// Set the variance to a value large enough to allow the state to converge quickly
// that does not destabilise the filter
P_[6][6] = 10.0f;
}
void ESKF::fuseVelPosHeight() {
bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available
bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [PN,PE,PD] observations
scalar_t R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
scalar_t gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
scalar_t Kfusion[k_num_states_] = {}; // Kalman gain vector for any single observation - sequential fusion is used
// calculate innovations, innovations gate sizes and observation variances
if (fuse_hor_vel_) {
// enable fusion for NE velocity axes
fuse_map[0] = fuse_map[1] = true;
velObsVarNE_(1) = velObsVarNE_(0) = sq(fmaxf(gps_sample_delayed_.sacc, gps_vel_noise_));
// Set observation noise variance and innovation consistency check gate size for the NE position observations
R[0] = velObsVarNE_(0);
R[1] = velObsVarNE_(1);
hvelInnovGate_ = fmaxf(vel_innov_gate_, 1.0f);
gate_size[1] = gate_size[0] = hvelInnovGate_;
}
if (fuse_vert_vel_) {
fuse_map[2] = true;
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(gps_vel_noise_, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
R[2] = 1.5f * fmaxf(R[2], gps_sample_delayed_.sacc);
R[2] = R[2] * R[2];
// innovation gate size
gate_size[2] = fmaxf(vel_innov_gate_, 1.0f);
}
if (fuse_pos_) {
fuse_map[3] = fuse_map[4] = true;
if(gps_pos_) {
// calculate observation process noise
scalar_t lower_limit = fmaxf(gps_pos_noise_, 0.01f);
scalar_t upper_limit = fmaxf(pos_noaid_noise_, lower_limit);
posObsNoiseNE_ = constrain(gps_sample_delayed_.hacc, lower_limit, upper_limit);
velObsVarNE_(1) = velObsVarNE_(0) = sq(fmaxf(gps_sample_delayed_.sacc, gps_vel_noise_));
// calculate innovations
vel_pos_innov_[0] = state_.vel(0) - gps_sample_delayed_.vel(0);
vel_pos_innov_[1] = state_.vel(1) - gps_sample_delayed_.vel(1);
vel_pos_innov_[2] = state_.vel(2) - gps_sample_delayed_.vel(2);
vel_pos_innov_[3] = state_.pos(0) - gps_sample_delayed_.pos(0);
vel_pos_innov_[4] = state_.pos(1) - gps_sample_delayed_.pos(1);
// observation 1-STD error
R[3] = sq(posObsNoiseNE_);
// set innovation gate size
gate_size[3] = fmaxf(5.0, 1.0f);
} else if (ev_pos_) {
// calculate innovations
// use the absolute position
vel_pos_innov_[3] = state_.pos(0) - ev_sample_delayed_.posNED(0);
vel_pos_innov_[4] = state_.pos(1) - ev_sample_delayed_.posNED(1);
// observation 1-STD error
R[3] = fmaxf(0.05f, 0.01f);
// innovation gate size
gate_size[3] = fmaxf(5.0f, 1.0f);
} else {
// No observations - use a static position to constrain drift
if (in_air_) {
R[3] = fmaxf(10.0f, 0.5f);
} else {
R[3] = 0.5f;
}
vel_pos_innov_[3] = state_.pos(0) - last_known_posNED_(0);
vel_pos_innov_[4] = state_.pos(1) - last_known_posNED_(1);
// glitch protection is not required so set gate to a large value
gate_size[3] = 100.0f;
vel_pos_innov_[5] = state_.pos(2) - last_known_posNED_(2);
fuse_map[5] = true;
R[5] = 0.5f;
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = 100.0f;
}
// convert North position noise to variance
R[3] = R[3] * R[3];
// copy North axis values to East axis
R[4] = R[3];
gate_size[4] = gate_size[3];
}
if (fuse_height_) {
if(ev_hgt_) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
vel_pos_innov_[5] = state_.pos(2) - ev_sample_delayed_.posNED(2);
// observation variance - defined externally
R[5] = fmaxf(0.05f, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(5.0f, 1.0f);
} else if(gps_hgt_) {
// vertical position innovation - gps measurement has opposite sign to earth z axis
vel_pos_innov_[5] = state_.pos(2) - gps_sample_delayed_.hgt;
// observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
scalar_t lower_limit = fmaxf(gps_pos_noise_, 0.01f);
scalar_t upper_limit = fmaxf(pos_noaid_noise_, lower_limit);
R[5] = 1.5f * constrain(gps_sample_delayed_.vacc, lower_limit, upper_limit);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(5.0, 1.0f);
} else if ((rng_hgt_) && (R_rng_to_earth_2_2_ > range_cos_max_tilt_)) {
fuse_map[5] = true;
// use range finder with tilt correction
vel_pos_innov_[5] = state_.pos(2) - (-max(range_sample_delayed_.rng * R_rng_to_earth_2_2_, rng_gnd_clearance_)) - 0.1f;
// observation variance - user parameter defined
R[5] = fmaxf((sq(range_noise_) + sq(range_noise_scaler_ * range_sample_delayed_.rng)) * sq(R_rng_to_earth_2_2_), 0.01f);
// innovation gate size
gate_size[5] = fmaxf(range_innov_gate_, 1.0f);
}
}
// calculate innovation test ratios
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (fuse_map[obs_index]) {
// compute the innovation variance SK = HPH + R
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
vel_pos_innov_var_[obs_index] = P_[state_index][state_index] + R[obs_index];
// Compute the ratio of innovation to gate size
vel_pos_test_ratio_[obs_index] = sq(vel_pos_innov_[obs_index]) / (sq(gate_size[obs_index]) * vel_pos_innov_var_[obs_index]);
}
}
// check position, velocity and height innovations
// treat 2D position and height as separate sensors
bool pos_check_pass = ((vel_pos_test_ratio_[3] <= 1.0f) && (vel_pos_test_ratio_[4] <= 1.0f));
innov_check_pass_map[3] = innov_check_pass_map[4] = pos_check_pass;
innov_check_pass_map[5] = (vel_pos_test_ratio_[5] <= 1.0f);
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
// skip fusion if not requested or checks have failed
if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
continue;
}
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < k_num_states_; row++) {
Kfusion[row] = P_[row][state_index] / vel_pos_innov_var_[obs_index];
}
// update covarinace matrix via Pnew = (I - KH)P
float KHP[k_num_states_][k_num_states_];
for (unsigned row = 0; row < k_num_states_; row++) {
for (unsigned column = 0; column < k_num_states_; column++) {
KHP[row][column] = Kfusion[row] * P_[state_index][column];
}
}
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
for (int i = 0; i < k_num_states_; i++) {
if (P_[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P_,i,i);
zeroCols(P_,i,i);
//flag as unhealthy
healthy = false;
}
}
// only apply covariance and state corrrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < k_num_states_; row++) {
for (unsigned column = 0; column < k_num_states_; column++) {
P_[row][column] = P_[row][column] - KHP[row][column];
}
}
// correct the covariance marix for gross errors
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, vel_pos_innov_[obs_index]);
}
}
}
void ESKF::updateVision(const quat& q, const vec3& p, uint64_t time_usec, scalar_t dt) {
// transform orientation from (ENU2FLU) to (NED2FRD):
quat q_nb = q_NED2ENU * q * q_FLU2FRD;
// transform position from local ENU to local NED frame
vec3 pos_nb = q_NED2ENU.inverse().toRotationMatrix() * p;
// limit data rate to prevent data being lost
if (time_usec - time_last_ext_vision_ > min_obs_interval_us_) {
extVisionSample ev_sample_new;
// calculate the system time-stamp for the mid point of the integration period
// copy required data
ev_sample_new.angErr = 0.05f;
ev_sample_new.posErr = 0.05f;
ev_sample_new.quatNED = q_nb;
ev_sample_new.posNED = pos_nb;
ev_sample_new.time_us = time_usec - ev_delay_ms_ * 1000;
time_last_ext_vision_ = time_usec;
// push to buffer
ext_vision_buffer_.push(ev_sample_new);
}
}
void ESKF::updateGps(const vec3& v, const vec3& p, uint64_t time_us, scalar_t dt) {
// transform linear velocity from local ENU to body FRD frame
vec3 vel_nb = q_NED2ENU.inverse().toRotationMatrix() * v;
// transform position from local ENU to local NED frame
vec3 pos_nb = q_NED2ENU.inverse().toRotationMatrix() * p;
// check for arrival of new sensor data at the fusion time horizon
if (time_us - time_last_gps_ > min_obs_interval_us_) {
gpsSample gps_sample_new;
gps_sample_new.time_us = time_us - gps_delay_ms_ * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
time_last_gps_ = time_us;
gps_sample_new.time_us = max(gps_sample_new.time_us, imu_sample_delayed_.time_us);
gps_sample_new.vel = vel_nb;
gps_sample_new.hacc = 1.0;
gps_sample_new.vacc = 1.0;
gps_sample_new.sacc = 0.0;
gps_sample_new.pos(0) = pos_nb(0);
gps_sample_new.pos(1) = pos_nb(1);
gps_sample_new.hgt = pos_nb(2);
gps_buffer_.push(gps_sample_new);
}
}
void ESKF::updateOpticalFlow(const vec2& int_xy, const vec2& int_xy_gyro, uint32_t integration_time_us, scalar_t distance, uint8_t quality, uint64_t time_us, scalar_t dt) {
// convert integrated flow and integrated gyro from flu to frd
///< integrated flow in PX4 Body (FRD) coordinates
///< integrated gyro in PX4 Body (FRD) coordinates
vec2 int_xy_b;
vec2 int_xy_gyro_b;
int_xy_b(0) = int_xy(0);
int_xy_b(1) = -int_xy(1);
int_xy_gyro_b = vec2(0,0); //replace embedded camera gyro to imu gyro
// check for arrival of new sensor data at the fusion time horizon
if (time_us - time_last_opt_flow_ > min_obs_interval_us_) {
// check if enough integration time and fail if integration time is less than 50% of min arrival interval because too much data is being lost
scalar_t delta_time = 1e-6f * (scalar_t)integration_time_us;
scalar_t delta_time_min = 5e-7f * (scalar_t)min_obs_interval_us_;
bool delta_time_good = delta_time >= delta_time_min;
if (!delta_time_good) {
// protect against overflow casued by division with very small delta_time
delta_time = delta_time_min;
}
// check magnitude is within sensor limits use this to prevent use of a saturated flow sensor when there are other aiding sources available
scalar_t flow_rate_magnitude;
bool flow_magnitude_good = true;
if (delta_time_good) {
flow_rate_magnitude = int_xy_b.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= flow_max_rate_);
}
bool relying_on_flow = opt_flow_ && !gps_pos_ && !ev_pos_;
// check quality metric
bool flow_quality_good = (quality >= flow_quality_min_);
// Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling
// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !in_air_) {
optFlowSample opt_flow_sample_new;
opt_flow_sample_new.time_us = time_us - flow_delay_ms_ * 1000;
// copy the quality metric returned by the PX4Flow sensor
opt_flow_sample_new.quality = quality;
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
// copy the optical and gyro measured delta angles
opt_flow_sample_new.gyroXY = - vec2(int_xy_gyro_b.x(), int_xy_gyro_b.y());
opt_flow_sample_new.flowRadXY = - vec2(int_xy_b.x(), int_xy_b.y());
// convert integration interval to seconds
opt_flow_sample_new.dt = delta_time;
time_last_opt_flow_ = time_us;
opt_flow_buffer_.push(opt_flow_sample_new);
rangeSample range_sample_new;
range_sample_new.rng = distance;
range_sample_new.time_us = time_us - range_delay_ms_ * 1000;
time_last_range_ = time_us;
range_buffer_.push(range_sample_new);
}
}
}
void ESKF::updateRangeFinder(scalar_t range, uint64_t time_us, scalar_t dt) {
// check for arrival of new range data at the fusion time horizon
if (time_us - time_last_range_ > min_obs_interval_us_) {
rangeSample range_sample_new;
range_sample_new.rng = range;
range_sample_new.time_us = time_us - range_delay_ms_ * 1000;
time_last_range_ = time_us;
range_buffer_.push(range_sample_new);
}
}
void ESKF::updateMagnetometer(const vec3& m, uint64_t time_usec, scalar_t dt) {
// convert FLU to FRD body frame IMU data
vec3 m_nb = q_FLU2FRD.toRotationMatrix() * m;
// limit data rate to prevent data being lost
if ((time_usec - time_last_mag_) > min_obs_interval_us_) {
magSample mag_sample_new;
mag_sample_new.time_us = time_usec - mag_delay_ms_ * 1000;
mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
time_last_mag_ = time_usec;
mag_sample_new.mag = m_nb;
mag_buffer_.push(mag_sample_new);
}
}
void ESKF::updateLandedState(uint8_t landed_state) {
in_air_ = landed_state;
}
bool ESKF::initHagl() {
// get most recent range measurement from buffer
const rangeSample &latest_measurement = range_buffer_.get_newest();
if ((time_last_imu_ - latest_measurement.time_us) < (uint64_t)2e5 && R_rng_to_earth_2_2_ > range_cos_max_tilt_) {
// if we have a fresh measurement, use it to initialise the terrain estimator
terrain_vpos_ = state_.pos(2) + latest_measurement.rng * R_rng_to_earth_2_2_;
// initialise state variance to variance of measurement
terrain_var_ = sq(range_noise_);
// success
return true;
} else if (!in_air_) {
// if on ground we assume a ground clearance
terrain_vpos_ = state_.pos(2) + rng_gnd_clearance_;
// Use the ground clearance value as our uncertainty
terrain_var_ = rng_gnd_clearance_;
// ths is a guess
return false;
} else {
// no information - cannot initialise
return false;
}
}
void ESKF::fuseHeading() {
// assign intermediate state variables
scalar_t q0 = state_.quat_nominal.w();
scalar_t q1 = state_.quat_nominal.x();
scalar_t q2 = state_.quat_nominal.y();
scalar_t q3 = state_.quat_nominal.z();
scalar_t predicted_hdg, measured_hdg;
scalar_t H_YAW[4];
vec3 mag_earth_pred;
// determine if a 321 or 312 Euler sequence is best
if (fabsf(R_to_earth_(2, 0)) < fabsf(R_to_earth_(2, 1))) {
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
scalar_t t9 = q0*q3;
scalar_t t10 = q1*q2;
scalar_t t2 = t9+t10;
scalar_t t3 = q0*q0;
scalar_t t4 = q1*q1;
scalar_t t5 = q2*q2;
scalar_t t6 = q3*q3;
scalar_t t7 = t3+t4-t5-t6;
scalar_t t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
}
scalar_t t11 = t2*t2;
scalar_t t12 = t8*t11*4.0f;
scalar_t t13 = t12+1.0f;
scalar_t t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
return;
}
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
// rotate the magnetometer measurement into earth frame
vec3 euler321 = dcm2vec(quat2dcm(state_.quat_nominal));
predicted_hdg = euler321(2); // we will need the predicted heading to calculate the innovation
// calculate the observed yaw angle
if (mag_hdg_) {
// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
euler321(2) = 0.0f;
mat3 R_to_earth = quat2dcm(from_euler(euler321));
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (mag_3D_) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * mag_sample_delayed_.mag;
} else {
mag_earth_pred = R_to_earth * (mag_sample_delayed_.mag - state_.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + mag_declination_;
} else if (ev_yaw_) {
// calculate the yaw angle for a 321 sequence
// Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m
scalar_t Tbn_1_0 = 2.0f*(ev_sample_delayed_.quatNED.w() * ev_sample_delayed_.quatNED.z() + ev_sample_delayed_.quatNED.x() * ev_sample_delayed_.quatNED.y());
scalar_t Tbn_0_0 = sq(ev_sample_delayed_.quatNED.w()) + sq(ev_sample_delayed_.quatNED.x()) - sq(ev_sample_delayed_.quatNED.y()) - sq(ev_sample_delayed_.quatNED.z());
measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
} else return;
}
else {
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
scalar_t t9 = q0*q3;
scalar_t t10 = q1*q2;
scalar_t t2 = t9-t10;
scalar_t t3 = q0*q0;
scalar_t t4 = q1*q1;
scalar_t t5 = q2*q2;
scalar_t t6 = q3*q3;
scalar_t t7 = t3-t4+t5-t6;
scalar_t t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
}
scalar_t t11 = t2*t2;
scalar_t t12 = t8*t11*4.0f;
scalar_t t13 = t12+1.0f;
scalar_t t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
return;
}
H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f;
scalar_t yaw = atan2f(-R_to_earth_(0, 1), R_to_earth_(1, 1)); // first rotation (yaw)
scalar_t roll = asinf(R_to_earth_(2, 1)); // second rotation (roll)
scalar_t pitch = atan2f(-R_to_earth_(2, 0), R_to_earth_(2, 2)); // third rotation (pitch)
predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation
// calculate the observed yaw angle
if (mag_hdg_) {
// Set the first rotation (yaw) to zero and rotate the measurements into earth frame
yaw = 0.0f;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
mat3 R_to_earth;
scalar_t sy = sinf(yaw);
scalar_t cy = cosf(yaw);
scalar_t sp = sinf(pitch);
scalar_t cp = cosf(pitch);
scalar_t sr = sinf(roll);
scalar_t cr = cosf(roll);
R_to_earth(0,0) = cy*cp-sy*sp*sr;
R_to_earth(0,1) = -sy*cr;
R_to_earth(0,2) = cy*sp+sy*cp*sr;
R_to_earth(1,0) = sy*cp+cy*sp*sr;
R_to_earth(1,1) = cy*cr;
R_to_earth(1,2) = sy*sp-cy*cp*sr;
R_to_earth(2,0) = -sp*cr;
R_to_earth(2,1) = sr;
R_to_earth(2,2) = cp*cr;
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (mag_3D_) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * mag_sample_delayed_.mag;
} else {
mag_earth_pred = R_to_earth * (mag_sample_delayed_.mag - state_.mag_B);
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + mag_declination_;
} else if (ev_yaw_) {
// calculate the yaw angle for a 312 sequence
// Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
scalar_t Tbn_0_1_neg = 2.0f * (ev_sample_delayed_.quatNED.w() * ev_sample_delayed_.quatNED.z() - ev_sample_delayed_.quatNED.x() * ev_sample_delayed_.quatNED.y());
scalar_t Tbn_1_1 = sq(ev_sample_delayed_.quatNED.w()) - sq(ev_sample_delayed_.quatNED.x()) + sq(ev_sample_delayed_.quatNED.y()) - sq(ev_sample_delayed_.quatNED.z());
measured_hdg = atan2f(Tbn_0_1_neg, Tbn_1_1);
} else return;
}
scalar_t R_YAW; // calculate observation variance
if (mag_hdg_) {
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(mag_heading_noise_, 1.0e-2f));
} else if (ev_yaw_)
// using error estimate from external vision data
R_YAW = sq(fmaxf(ev_sample_delayed_.angErr, 1.0e-2f));
else return;
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
// calculate the innovaton variance
scalar_t PH[4];
scalar_t heading_innov_var = R_YAW;
for (unsigned row = 0; row <= 3; row++) {
PH[row] = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
PH[row] += P_[row][col] * H_YAW[col];
}
heading_innov_var += H_YAW[row] * PH[row];
}
scalar_t heading_innov_var_inv;
// check if the innovation variance calculation is badly conditioned
if (heading_innov_var >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault
heading_innov_var_inv = 1.0f / heading_innov_var;
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
// calculate the Kalman gains
// only calculate gains for states we are using
scalar_t Kfusion[k_num_states_] = {};
for (uint8_t row = 0; row < k_num_states_; row++) {
Kfusion[row] = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
Kfusion[row] += P_[row][col] * H_YAW[col];
}
Kfusion[row] *= heading_innov_var_inv;
}
// wrap the heading to the interval between +-pi
measured_hdg = wrap_pi(measured_hdg);
if (mag_use_inhibit_) {
// The magnetomer cannot be trusted but we need to fuse a heading to prevent a badly conditoned covariance matrix developing over time.
if (!vehicle_at_rest_) {
// Vehicle is not at rest so fuse a zero innovation and record the predicted heading to use as an observation when movement ceases.
heading_innov_ = 0.0f;
vehicle_at_rest_prev_ = false;
} else {
// Vehicle is at rest so use the last moving prediciton as an observation to prevent the heading from drifting and to enable yaw gyro bias learning before takeoff.
if (!vehicle_at_rest_prev_ || !mag_use_inhibit_prev_) {
last_static_yaw_ = predicted_hdg;
vehicle_at_rest_prev_ = true;
}
// calculate the innovation
heading_innov_ = predicted_hdg - last_static_yaw_;
R_YAW = 0.01f;
heading_innov_gate_ = 5.0f;
}
} else {
// calculate the innovation
heading_innov_ = predicted_hdg - measured_hdg;
last_static_yaw_ = predicted_hdg;
}
mag_use_inhibit_prev_ = mag_use_inhibit_;
// wrap the innovation to the interval between +-pi
heading_innov_ = wrap_pi(heading_innov_);
// innovation test ratio
scalar_t yaw_test_ratio = sq(heading_innov_) / (sq(heading_innov_gate_) * heading_innov_var);
// set the vision yaw unhealthy if the test fails
if (yaw_test_ratio > 1.0f) {
if(in_air_)
return;
else {
// constrain the innovation to the maximum set by the gate
scalar_t gate_limit = sqrtf(sq(heading_innov_gate_) * heading_innov_var);
heading_innov_ = constrain(heading_innov_, -gate_limit, gate_limit);
}
}
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
float KHP[k_num_states_][k_num_states_];
float KH[4];
for (unsigned row = 0; row < k_num_states_; row++) {
KH[0] = Kfusion[row] * H_YAW[0];
KH[1] = Kfusion[row] * H_YAW[1];
KH[2] = Kfusion[row] * H_YAW[2];
KH[3] = Kfusion[row] * H_YAW[3];
for (unsigned column = 0; column < k_num_states_; column++) {
float tmp = KH[0] * P_[0][column];
tmp += KH[1] * P_[1][column];
tmp += KH[2] * P_[2][column];
tmp += KH[3] * P_[3][column];
KHP[row][column] = tmp;
}
}
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
for (int i = 0; i < k_num_states_; i++) {
if (P_[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P_,i,i);
zeroCols(P_,i,i);
//flag as unhealthy
healthy = false;
}
}
// only apply covariance and state corrrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < k_num_states_; row++) {
for (unsigned column = 0; column < k_num_states_; column++) {
P_[row][column] = P_[row][column] - KHP[row][column];
}
}
// correct the covariance marix for gross errors
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, heading_innov_);
}
}
void ESKF::fixCovarianceErrors() {
scalar_t P_lim[7] = {};
P_lim[0] = 1.0f; // quaternion max var
P_lim[1] = 1e6f; // velocity max var
P_lim[2] = 1e6f; // positiion max var
P_lim[3] = 1.0f; // gyro bias max var
P_lim[4] = 1.0f; // delta velocity z bias max var
P_lim[5] = 1.0f; // earth mag field max var
P_lim[6] = 1.0f; // body mag field max var
for (int i = 0; i <= 3; i++) {
// quaternion states
P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[0]);
}
for (int i = 4; i <= 6; i++) {
// NED velocity states
P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[1]);
}
for (int i = 7; i <= 9; i++) {
// NED position states
P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[2]);
}
for (int i = 10; i <= 12; i++) {
// gyro bias states
P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[3]);
}
// force symmetry on the quaternion, velocity, positon and gyro bias state covariances
makeSymmetrical(P_,0,12);
// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
const scalar_t minSafeStateVar = 1e-9f;
scalar_t maxStateVar = minSafeStateVar;
bool resetRequired = false;
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
if (P_[stateIndex][stateIndex] > maxStateVar) {
maxStateVar = P_[stateIndex][stateIndex];
} else if (P_[stateIndex][stateIndex] < minSafeStateVar) {
resetRequired = true;
}
}
// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
// not exceed 100 and the minimum variance must not fall below the target minimum
// Also limit variance to a maximum equivalent to a 0.1g uncertainty
const scalar_t minStateVarTarget = 5E-8f;
scalar_t minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
P_[stateIndex][stateIndex] = constrain(P_[stateIndex][stateIndex], minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * dt_ekf_avg_));
}
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
if (resetRequired) {
scalar_t delVelBiasVar[3];
// store all delta velocity bias variances
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
delVelBiasVar[stateIndex - 13] = P_[stateIndex][stateIndex];
}
// reset all delta velocity bias covariances
zeroCols(P_, 13, 15);
// restore all delta velocity bias variances
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
P_[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13];
}
}
// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
// calculate accel bias term aligned with the gravity vector
//scalar_t dVel_bias_lim = 0.9f * acc_bias_lim * dt_ekf_avg_;
scalar_t down_dvel_bias = 0.0f;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
down_dvel_bias += state_.accel_bias(axis_index) * R_to_earth_(2, axis_index);
}
// check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation
//bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim && down_dvel_bias * vel_pos_innov_[2] < 0.0f && down_dvel_bias * vel_pos_innov_[5] < 0.0f);
// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
if (time_last_imu_ - time_acc_bias_check_ > (uint64_t)7e6) {
scalar_t varX = P_[13][13];
scalar_t varY = P_[14][14];
scalar_t varZ = P_[15][15];
zeroRows(P_, 13, 15);
zeroCols(P_, 13, 15);
P_[13][13] = varX;
P_[14][14] = varY;
P_[15][15] = varZ;
//ECL_WARN("EKF invalid accel bias - resetting covariance");
} else {
// ensure the covariance values are symmetrical
makeSymmetrical(P_, 13, 15);
}
// magnetic field states
if (!mag_3D_) {
zeroRows(P_, 16, 21);
zeroCols(P_, 16, 21);
} else {
// constrain variances
for (int i = 16; i <= 18; i++) {
P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[5]);
}
for (int i = 19; i <= 21; i++) {
P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[6]);
}
// force symmetry
makeSymmetrical(P_, 16, 21);
}
}
// This function forces the covariance matrix to be symmetric
void ESKF::makeSymmetrical(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last) {
for (unsigned row = first; row <= last; row++) {
for (unsigned column = 0; column < row; column++) {
float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2;
cov_mat[row][column] = tmp;
cov_mat[column][row] = tmp;
}
}
}
// fuse measurement
void ESKF::fuse(scalar_t *K, scalar_t innovation) {
state_.quat_nominal.w() = state_.quat_nominal.w() - K[0] * innovation;
state_.quat_nominal.x() = state_.quat_nominal.x() - K[1] * innovation;
state_.quat_nominal.y() = state_.quat_nominal.y() - K[2] * innovation;
state_.quat_nominal.z() = state_.quat_nominal.z() - K[3] * innovation;
state_.quat_nominal.normalize();
for (unsigned i = 0; i < 3; i++) {
state_.vel(i) = state_.vel(i) - K[i + 4] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
state_.pos(i) = state_.pos(i) - K[i + 7] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
state_.gyro_bias(i) = state_.gyro_bias(i) - K[i + 10] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
state_.accel_bias(i) = state_.accel_bias(i) - K[i + 13] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
state_.mag_I(i) = state_.mag_I(i) - K[i + 16] * innovation;
}
for (unsigned i = 0; i < 3; i++) {
state_.mag_B(i) = state_.mag_B(i) - K[i + 19] * innovation;
}
}
quat ESKF::getQuat() {
// transform orientation from (NED2FRD) to (ENU2FLU)
return q_NED2ENU.conjugate() * state_.quat_nominal * q_FLU2FRD.conjugate();
}
vec3 ESKF::getPosition() {
// transform position from local NED to local ENU frame
return q_NED2ENU.toRotationMatrix() * state_.pos;
}
vec3 ESKF::getVelocity() {
// transform velocity from local NED to local ENU frame
return q_NED2ENU.toRotationMatrix() * state_.vel;
}
// initialise the quaternion covariances using rotation vector variances
void ESKF::initialiseQuatCovariances(const vec3& rot_vec_var) {
// calculate an equivalent rotation vector from the quaternion
scalar_t q0,q1,q2,q3;
if (state_.quat_nominal.w() >= 0.0f) {
q0 = state_.quat_nominal.w();
q1 = state_.quat_nominal.x();
q2 = state_.quat_nominal.y();
q3 = state_.quat_nominal.z();
} else {
q0 = -state_.quat_nominal.w();
q1 = -state_.quat_nominal.x();
q2 = -state_.quat_nominal.y();
q3 = -state_.quat_nominal.z();
}
scalar_t delta = 2.0f*acosf(q0);
scalar_t scaler = (delta/sinf(delta*0.5f));
scalar_t rotX = scaler*q1;
scalar_t rotY = scaler*q2;
scalar_t rotZ = scaler*q3;
// autocode generated using matlab symbolic toolbox
scalar_t t2 = rotX*rotX;
scalar_t t4 = rotY*rotY;
scalar_t t5 = rotZ*rotZ;
scalar_t t6 = t2+t4+t5;
if (t6 > 1e-9f) {
scalar_t t7 = sqrtf(t6);
scalar_t t8 = t7*0.5f;
scalar_t t3 = sinf(t8);
scalar_t t9 = t3*t3;
scalar_t t10 = 1.0f/t6;
scalar_t t11 = 1.0f/sqrtf(t6);
scalar_t t12 = cosf(t8);
scalar_t t13 = 1.0f/powf(t6,1.5f);
scalar_t t14 = t3*t11;
scalar_t t15 = rotX*rotY*t3*t13;
scalar_t t16 = rotX*rotZ*t3*t13;
scalar_t t17 = rotY*rotZ*t3*t13;
scalar_t t18 = t2*t10*t12*0.5f;
scalar_t t27 = t2*t3*t13;
scalar_t t19 = t14+t18-t27;
scalar_t t23 = rotX*rotY*t10*t12*0.5f;
scalar_t t28 = t15-t23;
scalar_t t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
scalar_t t25 = rotX*rotZ*t10*t12*0.5f;
scalar_t t31 = t16-t25;
scalar_t t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;
scalar_t t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;
scalar_t t24 = t15-t23;
scalar_t t26 = t16-t25;
scalar_t t29 = t4*t10*t12*0.5f;
scalar_t t34 = t3*t4*t13;
scalar_t t30 = t14+t29-t34;
scalar_t t32 = t5*t10*t12*0.5f;
scalar_t t40 = t3*t5*t13;
scalar_t t33 = t14+t32-t40;
scalar_t t36 = rotY*rotZ*t10*t12*0.5f;
scalar_t t39 = t17-t36;
scalar_t t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
scalar_t t37 = t15-t23;
scalar_t t38 = t17-t36;
scalar_t t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);
scalar_t t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;
scalar_t t43 = t16-t25;
scalar_t t44 = t17-t36;
// zero all the quaternion covariances
zeroRows(P_,0,3);
zeroCols(P_,0,3);
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
P_[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
P_[0][1] = t22;
P_[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;
P_[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;
P_[1][0] = t22;
P_[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);
P_[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
P_[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
P_[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;
P_[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
P_[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);
P_[2][3] = t42;
P_[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;
P_[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
P_[3][2] = t42;
P_[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);
} else {
// the equations are badly conditioned so use a small angle approximation
P_[0][0] = 0.0f;
P_[0][1] = 0.0f;
P_[0][2] = 0.0f;
P_[0][3] = 0.0f;
P_[1][0] = 0.0f;
P_[1][1] = 0.25f * rot_vec_var(0);
P_[1][2] = 0.0f;
P_[1][3] = 0.0f;
P_[2][0] = 0.0f;
P_[2][1] = 0.0f;
P_[2][2] = 0.25f * rot_vec_var(1);
P_[2][3] = 0.0f;
P_[3][0] = 0.0f;
P_[3][1] = 0.0f;
P_[3][2] = 0.0f;
P_[3][3] = 0.25f * rot_vec_var(2);
}
}
// zero specified range of rows in the state covariance matrix
void ESKF::zeroRows(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last) {
uint8_t row;
for (row = first; row <= last; row++) {
memset(&cov_mat[row][0], 0, sizeof(cov_mat[0][0]) * k_num_states_);
}
}
// zero specified range of columns in the state covariance matrix
void ESKF::zeroCols(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last) {
uint8_t row;
for (row = 0; row <= k_num_states_-1; row++) {
memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first));
}
}
void ESKF::setDiag(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last, scalar_t variance) {
// zero rows and columns
zeroRows(cov_mat, first, last);
zeroCols(cov_mat, first, last);
// set diagonals
for (uint8_t row = first; row <= last; row++) {
cov_mat[row][row] = variance;
}
}
void ESKF::constrainStates() {
state_.quat_nominal.w() = constrain(state_.quat_nominal.w(), -1.0f, 1.0f);
state_.quat_nominal.x() = constrain(state_.quat_nominal.x(), -1.0f, 1.0f);
state_.quat_nominal.y() = constrain(state_.quat_nominal.y(), -1.0f, 1.0f);
state_.quat_nominal.z() = constrain(state_.quat_nominal.z(), -1.0f, 1.0f);
for (int i = 0; i < 3; i++) {
state_.vel(i) = constrain(state_.vel(i), -1000.0f, 1000.0f);
}
for (int i = 0; i < 3; i++) {
state_.pos(i) = constrain(state_.pos(i), -1.e6f, 1.e6f);
}
for (int i = 0; i < 3; i++) {
state_.gyro_bias(i) = constrain(state_.gyro_bias(i), -0.349066f * dt_ekf_avg_, 0.349066f * dt_ekf_avg_);
}
for (int i = 0; i < 3; i++) {
state_.accel_bias(i) = constrain(state_.accel_bias(i), -acc_bias_lim * dt_ekf_avg_, acc_bias_lim * dt_ekf_avg_);
}
for (int i = 0; i < 3; i++) {
state_.mag_I(i) = constrain(state_.mag_I(i), -1.0f, 1.0f);
}
for (int i = 0; i < 3; i++) {
state_.mag_B(i) = constrain(state_.mag_B(i), -0.5f, 0.5f);
}
}
void ESKF::setFusionMask(int fusion_mask) {
fusion_mask_ = fusion_mask;
}
} // namespace eskf
================================================
FILE: src/Node.cpp
================================================
#include <eskf/Node.hpp>
namespace eskf
{
Node::Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) : nh_(pnh), init_(false) {
ROS_INFO("Subscribing to imu.");
subImu_ = nh_.subscribe<sensor_msgs::Imu>("imu", 1000, &Node::inputCallback, this, ros::TransportHints().tcpNoDelay(true));
ROS_INFO("Subscribing to extended state");
subExtendedState_ = nh_.subscribe("extended_state", 1, &Node::extendedStateCallback, this);
int fusion_mask = default_fusion_mask_;
ros::param::get("~fusion_mask", fusion_mask);
if((fusion_mask & MASK_EV_POS) || (fusion_mask & MASK_EV_YAW) || (fusion_mask & MASK_EV_HGT)) {
ROS_INFO("Subscribing to vision");
subVisionPose_ = nh_.subscribe("vision", 1, &Node::visionCallback, this);
}
if((fusion_mask & MASK_GPS_POS) || (fusion_mask & MASK_GPS_VEL) || (fusion_mask & MASK_GPS_HGT)) {
ROS_INFO("Subscribing to gps");
subGpsPose_ = nh_.subscribe("gps", 1, &Node::gpsCallback, this);
}
if(fusion_mask & MASK_OPTICAL_FLOW) {
ROS_INFO("Subscribing to optical_flow");
subOpticalFlowPose_ = nh_.subscribe("optical_flow", 1, &Node::opticalFlowCallback, this);
}
if(fusion_mask & MASK_MAG_HEADING) {
ROS_INFO("Subscribing to mag");
subMagPose_ = nh_.subscribe("mag", 1, &Node::magCallback, this);
}
if(fusion_mask & MASK_RANGEFINDER) {
ROS_INFO("Subscribing to rangefinder");
subRangeFinderPose_ = nh_.subscribe("rangefinder", 1, &Node::rangeFinderCallback, this);
}
eskf_.setFusionMask(fusion_mask);
pubPose_ = nh_.advertise<nav_msgs::Odometry>("pose", 1);
int publish_rate = default_publish_rate_;
ros::param::get("~publish_rate", publish_rate);
pubTimer_ = nh_.createTimer(ros::Duration(1.0f/publish_rate), &Node::publishState, this);
}
void Node::inputCallback(const sensor_msgs::ImuConstPtr& imuMsg) {
vec3 wm = vec3(imuMsg->angular_velocity.x, imuMsg->angular_velocity.y, imuMsg->angular_velocity.z); // measured angular rate
vec3 am = vec3(imuMsg->linear_acceleration.x, imuMsg->linear_acceleration.y, imuMsg->linear_acceleration.z); // measured linear acceleration
if (prevStampImu_.sec != 0) {
const double delta = (imuMsg->header.stamp - prevStampImu_).toSec();
if (!init_) {
init_ = true;
ROS_INFO("Initialized ESKF");
}
// run kalman filter
eskf_.run(wm, am, static_cast<uint64_t>(imuMsg->header.stamp.toSec()*1e6f), delta);
}
prevStampImu_ = imuMsg->header.stamp;
}
void Node::visionCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& poseMsg) {
if(prevStampVisionPose_.sec != 0) {
const double delta = (poseMsg->header.stamp - prevStampVisionPose_).toSec();
// get measurements
quat z_q = quat(poseMsg->pose.pose.orientation.w, poseMsg->pose.pose.orientation.x, poseMsg->pose.pose.orientation.y, poseMsg->pose.pose.orientation.z);
vec3 z_p = vec3(poseMsg->pose.pose.position.x, poseMsg->pose.pose.position.y, poseMsg->pose.pose.position.z);
// update vision
eskf_.updateVision(z_q, z_p, static_cast<uint64_t>(poseMsg->header.stamp.toSec()*1e6f), delta);
}
prevStampVisionPose_ = poseMsg->header.stamp;
}
void Node::gpsCallback(const nav_msgs::OdometryConstPtr& odomMsg) {
if (prevStampGpsPose_.sec != 0) {
const double delta = (odomMsg->header.stamp - prevStampGpsPose_).toSec();
// get gps measurements
vec3 z_v = vec3(odomMsg->twist.twist.linear.x, odomMsg->twist.twist.linear.y, odomMsg->twist.twist.linear.z);
vec3 z_p = vec3(odomMsg->pose.pose.position.x, odomMsg->pose.pose.position.y, odomMsg->pose.pose.position.z);
// update gps
eskf_.updateGps(z_v, z_p, static_cast<uint64_t>(odomMsg->header.stamp.toSec() * 1e6f), delta);
}
prevStampGpsPose_ = odomMsg->header.stamp;
}
void Node::opticalFlowCallback(const mavros_msgs::OpticalFlowRadConstPtr& opticalFlowMsg) {
if (prevStampOpticalFlowPose_.sec != 0) {
const double delta = (opticalFlowMsg->header.stamp - prevStampOpticalFlowPose_).toSec();
// get optical flow measurements
vec2 int_xy = vec2(opticalFlowMsg->integrated_x, opticalFlowMsg->integrated_y);
vec2 int_xy_gyro = vec2(opticalFlowMsg->integrated_xgyro, opticalFlowMsg->integrated_ygyro);
uint32_t integration_time = opticalFlowMsg->integration_time_us;
scalar_t distance = opticalFlowMsg->distance;
uint8_t quality = opticalFlowMsg->quality;
// update optical flow
eskf_.updateOpticalFlow(int_xy, int_xy_gyro, integration_time, distance, quality, static_cast<uint64_t>(opticalFlowMsg->header.stamp.toSec() * 1e6f), delta);
}
prevStampOpticalFlowPose_ = opticalFlowMsg->header.stamp;
}
void Node::magCallback(const sensor_msgs::MagneticFieldConstPtr& magMsg) {
if (prevStampMagPose_.sec != 0) {
const double delta = (magMsg->header.stamp - prevStampMagPose_).toSec();
// get mag measurements
vec3 m = vec3(magMsg->magnetic_field.x * 1e4f, magMsg->magnetic_field.y * 1e4f, magMsg->magnetic_field.z * 1e4f);
eskf_.updateMagnetometer(m, static_cast<uint64_t>(magMsg->header.stamp.toSec() * 1e6f), delta);
}
prevStampMagPose_ = magMsg->header.stamp;
}
void Node::rangeFinderCallback(const sensor_msgs::RangeConstPtr& rangeMsg) {
if (prevStampRangeFinderPose_.sec != 0) {
const double delta = (rangeMsg->header.stamp - prevStampRangeFinderPose_).toSec();
// get rangefinder measurements
eskf_.updateRangeFinder(rangeMsg->range, static_cast<uint64_t>(rangeMsg->header.stamp.toSec() * 1e6f), delta);
}
prevStampRangeFinderPose_ = rangeMsg->header.stamp;
}
void Node::extendedStateCallback(const mavros_msgs::ExtendedStateConstPtr& extendedStateMsg) {
eskf_.updateLandedState(extendedStateMsg->landed_state & mavros_msgs::ExtendedState::LANDED_STATE_IN_AIR);
}
void Node::publishState(const ros::TimerEvent&) {
// get kalman filter result
const quat e2g = eskf_.getQuat();
const vec3 position = eskf_.getPosition();
const vec3 velocity = eskf_.getVelocity();
static size_t trace_id_ = 0;
std_msgs::Header header;
header.frame_id = "/pose";
header.seq = trace_id_++;
header.stamp = ros::Time::now();
nav_msgs::Odometry odom;
odom.header = header;
odom.pose.pose.position.x = position[0];
odom.pose.pose.position.y = position[1];
odom.pose.pose.position.z = position[2];
odom.twist.twist.linear.x = velocity[0];
odom.twist.twist.linear.y = velocity[1];
odom.twist.twist.linear.z = velocity[2];
odom.pose.pose.orientation.w = e2g.w();
odom.pose.pose.orientation.x = e2g.x();
odom.pose.pose.orientation.y = e2g.y();
odom.pose.pose.orientation.z = e2g.z();
pubPose_.publish(odom);
}
}
================================================
FILE: src/eskf.cpp
================================================
#include <eskf/Node.hpp>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "eskf");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
eskf::Node node(nh, pnh);
ros::spin();
return 0;
}
gitextract_sg1e4u5d/
├── CMakeLists.txt
├── README.md
├── include/
│ └── eskf/
│ ├── ESKF.hpp
│ ├── Node.hpp
│ ├── RingBuffer.hpp
│ └── common.h
├── launch/
│ └── eskf.launch
├── package.xml
└── src/
├── ESKF.cpp
├── Node.cpp
└── eskf.cpp
SYMBOL INDEX (25 symbols across 7 files)
FILE: include/eskf/ESKF.hpp
type eskf (line 7) | namespace eskf {
class ESKF (line 9) | class ESKF {
FILE: include/eskf/Node.hpp
type eskf (line 16) | namespace eskf {
class Node (line 18) | class Node {
FILE: include/eskf/RingBuffer.hpp
type eskf (line 6) | namespace eskf {
class RingBuffer (line 9) | class RingBuffer {
method RingBuffer (line 11) | RingBuffer() {
method allocate (line 18) | bool allocate(int size) {
method unallocate (line 43) | void unallocate() {
method push (line 49) | inline void push(data_type sample) {
method data_type (line 69) | inline data_type get_oldest() {
method get_oldest_index (line 73) | unsigned get_oldest_index() {
method data_type (line 77) | inline data_type get_newest() {
method pop_first_older_than (line 81) | inline bool pop_first_older_than(uint64_t timestamp, data_type *samp...
method data_type (line 108) | data_type &operator[](unsigned index) {
method data_type (line 113) | inline data_type get_from_index(unsigned index) {
method push_to_index (line 121) | inline void push_to_index(unsigned index, data_type sample) {
method get_length (line 129) | unsigned get_length() {
FILE: include/eskf/common.h
function namespace (line 37) | namespace eskf {
FILE: src/ESKF.cpp
type eskf (line 10) | namespace eskf {
function quat (line 2452) | quat ESKF::getQuat() {
function vec3 (line 2457) | vec3 ESKF::getPosition() {
function vec3 (line 2462) | vec3 ESKF::getVelocity() {
FILE: src/Node.cpp
type eskf (line 3) | namespace eskf
FILE: src/eskf.cpp
function main (line 3) | int main(int argc, char *argv[])
Condensed preview — 11 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (193K chars).
[
{
"path": "CMakeLists.txt",
"chars": 961,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(eskf)\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n geometry_msgs\n s"
},
{
"path": "README.md",
"chars": 902,
"preview": "# ESKF\nROS Error-State Kalman Filter based on PX4/ecl. Performs GPS/Magnetometer/Vision Pose/Optical Flow/RangeFinder fu"
},
{
"path": "include/eskf/ESKF.hpp",
"chars": 15025,
"preview": "#ifndef ESKF_H_\n#define ESKF_H_\n\n#include <common.h>\n#include <RingBuffer.hpp>\n\nnamespace eskf {\n\n class ESKF {\n publi"
},
{
"path": "include/eskf/Node.hpp",
"chars": 1905,
"preview": "#ifndef ESKF_NODE_HPP_\n#define ESKF_NODE_HPP_\n\n#include <ros/ros.h>\n#include <sensor_msgs/Imu.h>\n#include <sensor_msgs/M"
},
{
"path": "include/eskf/RingBuffer.hpp",
"chars": 3112,
"preview": "#ifndef RINGBUFFER_H_\n#define RINGBUFFER_H_\n\n#include <cstdlib>\n\nnamespace eskf {\n\n template <typename data_type>\n cla"
},
{
"path": "include/eskf/common.h",
"chars": 11351,
"preview": "#ifndef COMMON_H_\n#define COMMON_H_\n\n#include <Eigen/Core>\n#include <Eigen/Dense>\n\n#define EV_MAX_INTERVAL\t\t2e5\t///< Max"
},
{
"path": "launch/eskf.launch",
"chars": 1629,
"preview": "<launch>\n <!-- Name of the node -->\n <arg name=\"eskf_node\" default=\"eskf\"/>\n\n <!-- IMU topic to use -->\n <arg name=\""
},
{
"path": "package.xml",
"chars": 994,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>eskf</name>\n <version>0.0.1</version>\n <description>Error state kalman filter "
},
{
"path": "src/ESKF.cpp",
"chars": 145562,
"preview": "#ifndef NDEBUG\n#define NDEBUG\n#endif\n\n#include <ESKF.hpp>\n#include <cmath>\n\nusing namespace Eigen;\n\nnamespace eskf {\n\n "
},
{
"path": "src/Node.cpp",
"chars": 6590,
"preview": "#include <eskf/Node.hpp>\n\nnamespace eskf\n{\n\nNode::Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) : nh_(pnh)"
},
{
"path": "src/eskf.cpp",
"chars": 195,
"preview": "#include <eskf/Node.hpp>\n\nint main(int argc, char *argv[])\n{\n\tros::init(argc, argv, \"eskf\");\n\tros::NodeHandle nh;\n\tros::"
}
]
About this extraction
This page contains the full source code of the EliaTarasov/ESKF GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 11 files (183.8 KB), approximately 74.7k tokens, and a symbol index with 25 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.