├── 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 /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(eskf) 3 | find_package(catkin REQUIRED COMPONENTS 4 | roscpp 5 | geometry_msgs 6 | sensor_msgs 7 | message_filters 8 | message_generation 9 | eigen_conversions 10 | mavros_msgs 11 | ) 12 | 13 | find_package(cmake_modules) 14 | find_package(Eigen3 REQUIRED) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS message_runtime 18 | ) 19 | 20 | # include headers 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ${Eigen_INCLUDE_DIRS} 24 | ${mavros_INCLUDE_DIRS} 25 | include 26 | include/eskf 27 | ) 28 | 29 | # build shared library 30 | add_library(ESKF 31 | src/ESKF.cpp 32 | ) 33 | 34 | set(RELEVANT_LIBRARIES 35 | ESKF 36 | ${catkin_LIBRARIES} 37 | ${mavros_LIBRARIES} 38 | ) 39 | 40 | # rigorous error checking 41 | add_definitions("-Wall -Werror -O3 -std=c++11") 42 | 43 | # build main filter 44 | add_executable(${PROJECT_NAME} 45 | src/eskf.cpp 46 | src/Node.cpp 47 | ) 48 | target_link_libraries(${PROJECT_NAME} ${RELEVANT_LIBRARIES}) 49 | 50 | # ensures messages are generated before hand 51 | add_dependencies(${PROJECT_NAME} 52 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 53 | ${catkin_EXPORTED_TARGETS} 54 | ) 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ESKF 2 | ROS Error-State Kalman Filter based on PX4/ecl. Performs GPS/Magnetometer/Vision Pose/Optical Flow/RangeFinder fusion with IMU 3 | 4 | # Description 5 | Multisensor fusion ROS node with delayed time horizon based on EKF2. 6 | 7 | # Building ESKF 8 | 9 | Prerequisites: 10 | * Eigen - https://bitbucket.org/eigen/eigen 11 | * Mavros - https://github.com/mavlink/mavros 12 | 13 | Steps: 14 | 1. Clone repository in your `catkin` workspace -`git clone https://github.com/EliaTarasov/ESKF.git` 15 | 2. Run `catkin_make` 16 | 3. Edit launch file `launch/eskf.launch` according to topics you want to subscribe. 17 | 4. Run ROS node which provides subscribed topics. 18 | 5. In case you want to use magnetometer for attitude correction make sure IMU has at least 3-4 times higher rate than magnetometer. 19 | If not run `rosrun mavros mavcmd long 511 31 4000 0 0 0 0 0`. 20 | 6. Run `roslaunch eskf eskf.launch` to start eskf node. 21 | 7. Run `echo /eskf/pose` to display results. 22 | -------------------------------------------------------------------------------- /include/eskf/ESKF.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ESKF_H_ 2 | #define ESKF_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace eskf { 8 | 9 | class ESKF { 10 | public: 11 | 12 | static constexpr int k_num_states_ = 22; 13 | 14 | ESKF(); 15 | 16 | void setFusionMask(int fusion_mask); 17 | 18 | void run(const vec3& w, const vec3& a, uint64_t time_us, scalar_t dt); 19 | void updateVision(const quat& q, const vec3& p, uint64_t time_us, scalar_t dt); 20 | void updateGps(const vec3& v, const vec3& p, uint64_t time_us, scalar_t dt); 21 | 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); 22 | void updateRangeFinder(scalar_t range, uint64_t time_us, scalar_t dt); 23 | void updateMagnetometer(const vec3& m, uint64_t time_us, scalar_t dt); 24 | void updateLandedState(uint8_t landed_state); 25 | 26 | quat getQuat(); 27 | vec3 getPosition(); 28 | vec3 getVelocity(); 29 | 30 | private: 31 | 32 | void constrainStates(); 33 | bool initializeFilter(); 34 | void initialiseQuatCovariances(const vec3& rot_vec_var); 35 | void zeroRows(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last); 36 | void zeroCols(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last); 37 | void makeSymmetrical(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last); 38 | void setDiag(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last, scalar_t variance); 39 | void fuse(scalar_t *K, scalar_t innovation); 40 | void fixCovarianceErrors(); 41 | void initialiseCovariance(); 42 | void predictCovariance(); 43 | void fuseVelPosHeight(); 44 | void resetHeight(); 45 | void resetPosition(); 46 | void resetVelocity(); 47 | void fuseHeading(); 48 | void fuseOptFlow(); 49 | void fuseHagl(); 50 | bool initHagl(); 51 | void runTerrainEstimator(); 52 | void controlFusionModes(); 53 | void controlMagFusion(); 54 | void controlOpticalFlowFusion(); 55 | void controlGpsFusion(); 56 | void controlVelPosFusion(); 57 | void controlExternalVisionFusion(); 58 | void controlHeightSensorTimeouts(); 59 | bool calcOptFlowBodyRateComp(); 60 | bool collect_imu(imuSample& imu); 61 | 62 | ///< state vector 63 | state state_; 64 | 65 | ///< FIFO buffers 66 | RingBuffer imu_buffer_; 67 | RingBuffer ext_vision_buffer_; 68 | RingBuffer gps_buffer_; 69 | RingBuffer range_buffer_; 70 | RingBuffer opt_flow_buffer_; 71 | RingBuffer mag_buffer_; 72 | 73 | ///< FIFO buffers lengths 74 | const int obs_buffer_length_ {9}; 75 | const int imu_buffer_length_ {15}; 76 | 77 | ///< delayed samples 78 | imuSample imu_sample_delayed_ {}; // captures the imu sample on the delayed time horizon 79 | extVisionSample ev_sample_delayed_ {}; // captures the external vision sample on the delayed time horizon 80 | gpsSample gps_sample_delayed_ {}; // captures the gps sample on the delayed time horizon 81 | rangeSample range_sample_delayed_{}; // captures the range sample on the delayed time horizon 82 | optFlowSample opt_flow_sample_delayed_ {}; // captures the optical flow sample on the delayed time horizon 83 | magSample mag_sample_delayed_ {}; //captures magnetometer sample on the delayed time horizon 84 | 85 | ///< new samples 86 | imuSample imu_sample_new_ {}; ///< imu sample capturing the newest imu data 87 | 88 | ///< downsampled 89 | imuSample imu_down_sampled_ {}; ///< down sampled imu data (sensor rate -> filter update rate) 90 | quat q_down_sampled_; ///< down sampled rotation data (sensor rate -> filter update rate) 91 | 92 | ///< masks 93 | 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 94 | int fusion_mask_ = MASK_EV_POS && MASK_EV_YAW && MASK_EV_HGT; /// < ekf fusion mask (see launch file), vision pose set by default 95 | 96 | ///< timestamps 97 | uint64_t time_last_opt_flow_ {0}; 98 | uint64_t time_last_ext_vision_ {0}; 99 | uint64_t time_last_gps_ {0}; 100 | uint64_t time_last_imu_ {0}; 101 | uint64_t time_last_mag_ {0}; 102 | uint64_t time_last_hgt_fuse_ {0}; 103 | uint64_t time_last_range_ {0}; 104 | uint64_t time_last_fake_gps_ {0}; 105 | uint64_t time_last_hagl_fuse_{0}; ///< last system time that the hagl measurement failed it's checks (uSec) 106 | uint64_t time_acc_bias_check_{0}; 107 | 108 | ///< sensors delays 109 | scalar_t gps_delay_ms_ {110.0f}; ///< gps measurement delay relative to the IMU (mSec) 110 | scalar_t ev_delay_ms_ {100.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) 111 | 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 112 | scalar_t range_delay_ms_{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) 113 | scalar_t mag_delay_ms_{0.0f}; 114 | 115 | ///< frames rotations 116 | mat3 R_to_earth_; ///< Rotation (DCM) from FRD to NED 117 | 118 | /** 119 | * Quaternion for rotation between ENU and NED frames 120 | * 121 | * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) 122 | * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) 123 | */ 124 | const quat q_NED2ENU = quat(0, 0.70711, 0.70711, 0); 125 | 126 | /** 127 | * Quaternion for rotation between body FLU and body FRD frames 128 | * 129 | * FLU to FRD: +PI rotation about X(forward) 130 | * FRD to FLU: -PI rotation about X(forward) 131 | */ 132 | const quat q_FLU2FRD = quat(0, 1, 0, 0); 133 | 134 | ///< filter times 135 | const unsigned FILTER_UPDATE_PERIOD_MS = 12; ///< ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta 136 | scalar_t dt_ekf_avg_ {0.001f * FILTER_UPDATE_PERIOD_MS}; ///< average update rate of the ekf 137 | 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) 138 | unsigned min_obs_interval_us_ {0}; // minimum time interval between observations that will guarantee data is not lost (usec) 139 | 140 | ///< filter initialisation 141 | bool NED_origin_initialised_; ///< true when the NED origin has been initialised 142 | bool terrain_initialised_; ///< true when the terrain estimator has been intialised 143 | bool filter_initialised_; ///< true when the filter has been initialised 144 | 145 | ///< Covariance 146 | scalar_t P_[k_num_states_][k_num_states_]; ///< System covariance matrix 147 | 148 | // process noise 149 | const scalar_t gyro_bias_p_noise_ {1.0e-3}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) 150 | const scalar_t accel_bias_p_noise_ {3.0e-3}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) 151 | 152 | // input noise 153 | const scalar_t gyro_noise_ {1.5e-2}; ///< IMU angular rate noise used for covariance prediction (rad/sec) 154 | const scalar_t accel_noise_ {3.5e-1}; ///< IMU acceleration noise use for covariance prediction (m/sec**2) 155 | 156 | ///< Measurement (observation) noise 157 | const scalar_t range_noise_{0.1f}; ///< observation noise for range finder measurements (m) 158 | const scalar_t flow_noise_ {0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) 159 | const scalar_t gps_vel_noise_ {5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) 160 | const scalar_t gps_pos_noise_ {0.5f}; ///< minimum allowed observation noise for gps position fusion (m) 161 | const scalar_t pos_noaid_noise_ {10.0f}; ///< observation noise for non-aiding position fusion (m) 162 | const scalar_t vel_noise_ {0.5f}; ///< minimum allowed observation noise for velocity fusion (m/sec) 163 | const scalar_t pos_noise_ {0.5f}; ///< minimum allowed observation noise for position fusion (m) 164 | const scalar_t terrain_p_noise_{5.0f}; ///< process noise for terrain offset (m/sec) 165 | scalar_t posObsNoiseNE_ {0.0f}; ///< 1-STD observtion noise used for the fusion of NE position data (m) 166 | 167 | ///< Measurement (observation) variance 168 | scalar_t terrain_var_{1e4f}; ///< variance of terrain position estimate (m**2) 169 | vec2 velObsVarNE_; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2 170 | 171 | ///< initialization errors 172 | const scalar_t switch_on_gyro_bias_ {0.01f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) 173 | const scalar_t switch_on_accel_bias_ {0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) 174 | const scalar_t initial_tilt_err_ {0.01f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) 175 | 176 | ///< innovation gates 177 | const scalar_t range_innov_gate_{5.0f}; ///< range finder fusion innovation consistency gate size (STD) 178 | const scalar_t range_aid_innov_gate_{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion 179 | const scalar_t flow_innov_gate_{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) 180 | scalar_t heading_innov_gate_ {2.6f}; ///< heading fusion innovation consistency gate size (STD) 181 | const scalar_t posNE_innov_gate_ {5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) 182 | const scalar_t vel_innov_gate_ {5.0f}; ///< GPS velocity innovation consistency gate size (STD) 183 | scalar_t hvelInnovGate_ {1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check 184 | scalar_t posInnovGateNE_ {1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check 185 | 186 | ///< innovations 187 | scalar_t heading_innov_ {0.0f}; ///< innovation of the last heading measurement (rad) 188 | scalar_t hagl_innov_{0.0f}; ///< innovation of the last height above terrain measurement (m) 189 | scalar_t flow_innov_[2] {}; ///< flow measurement innovation (rad/sec) 190 | scalar_t vel_pos_innov_[6] {}; ///< velocity and position innovations: (m/s and m) 191 | 192 | ///< innovation variances 193 | scalar_t hagl_innov_var_{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) 194 | scalar_t vel_pos_innov_var_[6] {}; ///< velocity and position innovation variances: (m**2) 195 | scalar_t flow_innov_var_[2] {}; ///< flow innovation variance ((rad/sec)**2) 196 | 197 | ///< test ratios 198 | scalar_t terr_test_ratio_{0.0f}; // height above terrain measurement innovation consistency check ratio 199 | scalar_t vel_pos_test_ratio_[6] {}; // velocity and position innovation consistency check ratios 200 | 201 | ///< range specific params 202 | const scalar_t rng_gnd_clearance_{0.1f}; ///< minimum valid value for range when on ground (m) 203 | 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. 204 | scalar_t range_noise_scaler_{0.0f}; ///< scaling from range measurement to noise (m/m) 205 | scalar_t vehicle_variance_scaler_{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance 206 | 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) 207 | 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) 208 | int32_t range_aid_{0}; ///< allow switching primary height source to range finder if certian conditions are met 209 | 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 210 | scalar_t terrain_gradient_{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) 211 | scalar_t terrain_vpos_{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) 212 | scalar_t sin_tilt_rng_{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis 213 | scalar_t cos_tilt_rng_{1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis 214 | scalar_t R_rng_to_earth_2_2_{0.0f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame 215 | bool hagl_valid_{false}; ///< true when the height above ground estimate is valid 216 | 217 | ///< optical flow specific params 218 | const scalar_t flow_max_rate_ {2.5}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) 219 | int32_t flow_quality_min_ {1}; 220 | 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) 221 | vec2 flow_rad_xy_comp_ {}; 222 | vec3 flow_gyro_bias_; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) 223 | vec3 imu_del_ang_of_; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) 224 | scalar_t delta_time_of_{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) 225 | 226 | ///< mag specific params 227 | scalar_t mag_declination_{0.0f}; 228 | scalar_t mag_heading_noise_{3.0e-1f}; 229 | scalar_t mage_p_noise_{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) 230 | scalar_t magb_p_noise_{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) 231 | scalar_t mag_noise_{5.0e-2f}; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) 232 | 233 | bool imu_updated_; 234 | vec3 last_known_posNED_; 235 | 236 | static constexpr scalar_t kOneG = 9.80665; /// Earth gravity (m/s^2) 237 | static constexpr scalar_t acc_bias_lim = 0.4; ///< maximum accel bias magnitude (m/sec**2) 238 | static constexpr scalar_t hgt_reset_lim = 0.0f; ///< 239 | static constexpr scalar_t CONSTANTS_ONE_G = 9.80665f; // m/s^2 240 | 241 | bool mag_use_inhibit_{false}; ///< true when magnetomer use is being inhibited 242 | bool mag_use_inhibit_prev_{false}; ///< true when magnetomer use was being inhibited the previous frame 243 | scalar_t last_static_yaw_{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) 244 | 245 | ///< flags on receive updates 246 | bool gps_data_ready_ = false; 247 | bool vision_data_ready_ = false; 248 | bool range_data_ready_ = false; 249 | bool flow_data_ready_ = false; 250 | bool mag_data_ready_ = false; 251 | 252 | ///< flags on fusion modes 253 | bool fuse_pos_ = false; 254 | bool fuse_height_ = false; 255 | bool mag_hdg_ = false; 256 | bool mag_3D_ = false; 257 | bool opt_flow_ = false; 258 | bool ev_pos_ = false; 259 | bool ev_yaw_ = false; 260 | bool ev_hgt_ = false; 261 | bool gps_pos_ = false; 262 | bool gps_vel_ = false; 263 | bool gps_hgt_ = false; 264 | bool fuse_hor_vel_ = false; 265 | bool fuse_vert_vel_ = false; 266 | bool rng_hgt_ = false; 267 | 268 | ///< flags on vehicle's state 269 | bool in_air_ = false; 270 | bool vehicle_at_rest_ = !in_air_; // true when the vehicle is at rest 271 | bool vehicle_at_rest_prev_ {false}; ///< true when the vehicle was at rest the previous time the status was checked 272 | }; 273 | } 274 | 275 | #endif /* defined(ESKF_H_) */ 276 | -------------------------------------------------------------------------------- /include/eskf/Node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ESKF_NODE_HPP_ 2 | #define ESKF_NODE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace eskf { 17 | 18 | class Node { 19 | public: 20 | static constexpr int default_publish_rate_ = 100; 21 | static constexpr int default_fusion_mask_ = MASK_EV; 22 | 23 | Node(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 24 | 25 | private: 26 | ros::NodeHandle nh_; 27 | 28 | // publishers 29 | ros::Publisher pubPose_; 30 | 31 | // subsribers 32 | ros::Subscriber subImu_; 33 | ros::Subscriber subVisionPose_; 34 | ros::Subscriber subGpsPose_; 35 | ros::Subscriber subOpticalFlowPose_; 36 | ros::Subscriber subMagPose_; 37 | ros::Subscriber subExtendedState_; 38 | ros::Subscriber subRangeFinderPose_; 39 | 40 | // implementation 41 | eskf::ESKF eskf_; 42 | ros::Time prevStampImu_; 43 | ros::Time prevStampVisionPose_; 44 | ros::Time prevStampGpsPose_; 45 | ros::Time prevStampOpticalFlowPose_; 46 | ros::Time prevStampMagPose_; 47 | ros::Time prevStampRangeFinderPose_; 48 | ros::Timer pubTimer_; 49 | bool init_; 50 | 51 | // callbacks 52 | void inputCallback(const sensor_msgs::ImuConstPtr&); 53 | void visionCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr&); 54 | void gpsCallback(const nav_msgs::OdometryConstPtr&); 55 | void opticalFlowCallback(const mavros_msgs::OpticalFlowRadConstPtr&); 56 | void magCallback(const sensor_msgs::MagneticFieldConstPtr&); 57 | void extendedStateCallback(const mavros_msgs::ExtendedStateConstPtr&); 58 | void rangeFinderCallback(const sensor_msgs::RangeConstPtr&); 59 | void publishState(const ros::TimerEvent&); 60 | }; 61 | } // namespace eskf 62 | 63 | #endif // ESKF_NODE_HPP_ 64 | -------------------------------------------------------------------------------- /include/eskf/RingBuffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RINGBUFFER_H_ 2 | #define RINGBUFFER_H_ 3 | 4 | #include 5 | 6 | namespace eskf { 7 | 8 | template 9 | class RingBuffer { 10 | public: 11 | RingBuffer() { 12 | _buffer = NULL; 13 | _head = _tail = _size = 0; 14 | _first_write = true; 15 | } 16 | ~RingBuffer() { delete[] _buffer; } 17 | 18 | bool allocate(int size) { 19 | if (size <= 0) { 20 | return false; 21 | } 22 | 23 | if (_buffer != NULL) { 24 | delete[] _buffer; 25 | } 26 | 27 | _buffer = new data_type[size]; 28 | 29 | if (_buffer == NULL) { 30 | return false; 31 | } 32 | 33 | _size = size; 34 | 35 | // set the time elements to zero so that bad data is not retrieved from the buffers 36 | for (unsigned index = 0; index < _size; index++) { 37 | _buffer[index].time_us = 0; 38 | } 39 | _first_write = true; 40 | return true; 41 | } 42 | 43 | void unallocate() { 44 | if (_buffer != NULL) { 45 | delete[] _buffer; 46 | } 47 | } 48 | 49 | inline void push(data_type sample) { 50 | int head_new = _head; 51 | 52 | if (_first_write) { 53 | head_new = _head; 54 | } else { 55 | head_new = (_head + 1) % _size; 56 | } 57 | 58 | _buffer[head_new] = sample; 59 | _head = head_new; 60 | 61 | // move tail if we overwrite it 62 | if (_head == _tail && !_first_write) { 63 | _tail = (_tail + 1) % _size; 64 | } else { 65 | _first_write = false; 66 | } 67 | } 68 | 69 | inline data_type get_oldest() { 70 | return _buffer[_tail]; 71 | } 72 | 73 | unsigned get_oldest_index() { 74 | return _tail; 75 | } 76 | 77 | inline data_type get_newest() { 78 | return _buffer[_head]; 79 | } 80 | 81 | inline bool pop_first_older_than(uint64_t timestamp, data_type *sample) { 82 | // start looking from newest observation data 83 | for (unsigned i = 0; i < _size; i++) { 84 | int index = (_head - i); 85 | index = index < 0 ? _size + index : index; 86 | if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < 100000) { 87 | // TODO Re-evaluate the static cast and usage patterns 88 | memcpy(static_cast(sample), static_cast(&_buffer[index]), sizeof(*sample)); 89 | // Now we can set the tail to the item which comes after the one we removed 90 | // since we don't want to have any older data in the buffer 91 | if (index == static_cast(_head)) { 92 | _tail = _head; 93 | _first_write = true; 94 | } else { 95 | _tail = (index + 1) % _size; 96 | } 97 | _buffer[index].time_us = 0; 98 | return true; 99 | } 100 | if (index == static_cast(_tail)) { 101 | // we have reached the tail and haven't got a match 102 | return false; 103 | } 104 | } 105 | return false; 106 | } 107 | 108 | data_type &operator[](unsigned index) { 109 | return _buffer[index]; 110 | } 111 | 112 | // return data at the specified index 113 | inline data_type get_from_index(unsigned index) { 114 | if (index >= _size) { 115 | index = _size-1; 116 | } 117 | return _buffer[index]; 118 | } 119 | 120 | // push data to the specified index 121 | inline void push_to_index(unsigned index, data_type sample) { 122 | if (index >= _size) { 123 | index = _size-1; 124 | } 125 | _buffer[index] = sample; 126 | } 127 | 128 | // return the length of the buffer 129 | unsigned get_length() { 130 | return _size; 131 | } 132 | 133 | private: 134 | data_type *_buffer; 135 | unsigned _head, _tail, _size; 136 | bool _first_write; 137 | }; 138 | } 139 | 140 | #endif /* defined(RINGBUFFER_H_) */ -------------------------------------------------------------------------------- /include/eskf/common.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H_ 2 | #define COMMON_H_ 3 | 4 | #include 5 | #include 6 | 7 | #define EV_MAX_INTERVAL 2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) 8 | #define GPS_MAX_INTERVAL 1e6 ///< Maximum allowable time interval between external gps system measurements (uSec) 9 | #define OPTICAL_FLOW_INTERVAL 2e5 ///< Maximum allowable time interval between optical flow system measurements (uSec) 10 | #define MAG_INTERVAL 2e5 ///< Maximum allowable time interval between mag system measurements (uSec) 11 | #define RANGE_MAX_INTERVAL 1e6 ///< Maximum allowable time interval between rangefinder system measurements (uSec) 12 | 13 | #define MASK_EV_POS 1<<0 14 | #define MASK_EV_YAW 1<<1 15 | #define MASK_EV_HGT 1<<2 16 | #define MASK_GPS_POS 1<<3 17 | #define MASK_GPS_VEL 1<<4 18 | #define MASK_GPS_HGT 1<<5 19 | #define MASK_MAG_INHIBIT 1<<6 20 | #define MASK_OPTICAL_FLOW 1<<7 21 | #define MASK_RANGEFINDER 1<<8 22 | #define MASK_MAG_HEADING 1<<9 23 | #define MASK_EV (MASK_EV_POS | MASK_EV_YAW | MASK_EV_HGT) 24 | #define MASK_GPS (MASK_GPS_POS | MASK_GPS_VEL | MASK_GPS_HGT) 25 | 26 | // GPS pre-flight check bit locations 27 | #define MASK_GPS_NSATS (1<<0) 28 | #define MASK_GPS_GDOP (1<<1) 29 | #define MASK_GPS_HACC (1<<2) 30 | #define MASK_GPS_VACC (1<<3) 31 | #define MASK_GPS_SACC (1<<4) 32 | #define MASK_GPS_HDRIFT (1<<5) 33 | #define MASK_GPS_VDRIFT (1<<6) 34 | #define MASK_GPS_HSPD (1<<7) 35 | #define MASK_GPS_VSPD (1<<8) 36 | 37 | namespace eskf { 38 | 39 | typedef float scalar_t; 40 | typedef Eigen::Matrix vec3; /// Vector in R3 41 | typedef Eigen::Matrix vec2; /// Vector in R2 42 | typedef Eigen::Matrix mat3; /// Matrix in R3 43 | typedef Eigen::Quaternion quat; /// Member of S4 44 | 45 | /* State vector: 46 | * Attitude quaternion 47 | * NED velocity 48 | * NED position 49 | * Delta Angle bias - rad 50 | * Delta Velocity bias - m/s 51 | */ 52 | 53 | struct state { 54 | quat quat_nominal; ///< quaternion defining the rotaton from NED to XYZ frame 55 | vec3 vel; ///< NED velocity in earth frame in m/s 56 | vec3 pos; ///< NED position in earth frame in m 57 | vec3 gyro_bias; ///< delta angle bias estimate in rad 58 | vec3 accel_bias; ///< delta velocity bias estimate in m/s 59 | vec3 mag_I; ///< NED earth magnetic field in gauss 60 | vec3 mag_B; ///< magnetometer bias estimate in body frame in gauss 61 | }; 62 | 63 | struct imuSample { 64 | vec3 delta_ang; ///< delta angle in body frame (integrated gyro measurements) (rad) 65 | vec3 delta_vel; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec) 66 | scalar_t delta_ang_dt; ///< delta angle integration period (sec) 67 | scalar_t delta_vel_dt; ///< delta velocity integration period (sec) 68 | uint64_t time_us; ///< timestamp of the measurement (uSec) 69 | }; 70 | 71 | struct extVisionSample { 72 | vec3 posNED; ///< measured NED body position relative to the local origin (m) 73 | quat quatNED; ///< measured quaternion orientation defining rotation from NED to body frame 74 | scalar_t posErr; ///< 1-Sigma spherical position accuracy (m) 75 | scalar_t angErr; ///< 1-Sigma angular error (rad) 76 | uint64_t time_us; ///< timestamp of the measurement (uSec) 77 | }; 78 | 79 | struct gpsSample { 80 | vec2 pos; ///< NE earth frame gps horizontal position measurement (m) 81 | vec3 vel; ///< NED earth frame gps velocity measurement (m/sec) 82 | scalar_t hgt; ///< gps height measurement (m) 83 | scalar_t hacc; ///< 1-std horizontal position error (m) 84 | scalar_t vacc; ///< 1-std vertical position error (m) 85 | scalar_t sacc; ///< 1-std speed error (m/sec) 86 | uint64_t time_us; ///< timestamp of the measurement (uSec) 87 | }; 88 | 89 | struct optFlowSample { 90 | uint8_t quality; ///< quality indicator between 0 and 255 91 | vec2 flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive 92 | 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 93 | vec2 gyroXY; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive 94 | scalar_t dt; ///< amount of integration time (sec) 95 | uint64_t time_us; ///< timestamp of the integration period mid-point (uSec) 96 | }; 97 | 98 | struct rangeSample { 99 | scalar_t rng; ///< range (distance to ground) measurement (m) 100 | uint64_t time_us; ///< timestamp of the measurement (uSec) 101 | }; 102 | 103 | struct magSample { 104 | vec3 mag; ///< NED magnetometer body frame measurements (Gauss) 105 | uint64_t time_us; ///< timestamp of the measurement (uSec) 106 | }; 107 | 108 | struct gps_message { 109 | uint64_t time_usec; 110 | int32_t lat; ///< Latitude in 1E-7 degrees 111 | int32_t lon; ///< Longitude in 1E-7 degrees 112 | int32_t alt; ///< Altitude in 1E-3 meters (millimeters) above MSL 113 | float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI]) 114 | float yaw_offset; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET 115 | uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic 116 | float eph; ///< GPS horizontal position accuracy in m 117 | float epv; ///< GPS vertical position accuracy in m 118 | float sacc; ///< GPS speed accuracy in m/s 119 | float vel_m_s; ///< GPS ground speed (m/sec) 120 | float vel_ned[3]; ///< GPS ground speed NED 121 | bool vel_ned_valid; ///< GPS ground speed is valid 122 | uint8_t nsats; ///< number of satellites used 123 | float gdop; ///< geometric dilution of precision 124 | }; 125 | 126 | // publish the status of various GPS quality checks 127 | union gps_check_fail_status_u { 128 | struct { 129 | uint16_t fix : 1; ///< 0 - true if the fix type is insufficient (no 3D solution) 130 | uint16_t nsats : 1; ///< 1 - true if number of satellites used is insufficient 131 | uint16_t gdop : 1; ///< 2 - true if geometric dilution of precision is insufficient 132 | uint16_t hacc : 1; ///< 3 - true if reported horizontal accuracy is insufficient 133 | uint16_t vacc : 1; ///< 4 - true if reported vertical accuracy is insufficient 134 | uint16_t sacc : 1; ///< 5 - true if reported speed accuracy is insufficient 135 | uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground) 136 | uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground) 137 | uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground) 138 | uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive 139 | } flags; 140 | uint16_t value; 141 | }; 142 | 143 | ///< common math functions 144 | template inline T sq(T var) { 145 | return var * var; 146 | } 147 | 148 | template inline T max(T val1, T val2) { 149 | return (val1 > val2) ? val1 : val2; 150 | } 151 | 152 | template 153 | static inline constexpr const Scalar &constrain(const Scalar &val, const Scalar &min_val, const Scalar &max_val) { 154 | return (val < min_val) ? min_val : ((val > max_val) ? max_val : val); 155 | } 156 | 157 | template 158 | Type wrap_pi(Type x) { 159 | while (x >= Type(M_PI)) { 160 | x -= Type(2.0 * M_PI); 161 | } 162 | 163 | while (x < Type(-M_PI)) { 164 | x += Type(2.0 * M_PI); 165 | } 166 | return x; 167 | } 168 | 169 | ///< tf functions 170 | inline quat from_axis_angle(const vec3 &axis, scalar_t theta) { 171 | quat q; 172 | 173 | if (theta < scalar_t(1e-10)) { 174 | q.w() = scalar_t(1.0); 175 | q.x() = q.y() = q.z() = 0; 176 | } 177 | 178 | scalar_t magnitude = sin(theta / 2.0f); 179 | 180 | q.w() = cos(theta / 2.0f); 181 | q.x() = axis(0) * magnitude; 182 | q.y() = axis(1) * magnitude; 183 | q.z() = axis(2) * magnitude; 184 | 185 | return q; 186 | } 187 | 188 | inline quat from_axis_angle(vec3 vec) { 189 | quat q; 190 | scalar_t theta = vec.norm(); 191 | 192 | if (theta < scalar_t(1e-10)) { 193 | q.w() = scalar_t(1.0); 194 | q.x() = q.y() = q.z() = 0; 195 | return q; 196 | } 197 | 198 | vec3 tmp = vec / theta; 199 | return from_axis_angle(tmp, theta); 200 | } 201 | 202 | inline vec3 to_axis_angle(const quat& q) { 203 | scalar_t axis_magnitude = scalar_t(sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z())); 204 | vec3 vec; 205 | vec(0) = q.x(); 206 | vec(1) = q.y(); 207 | vec(2) = q.z(); 208 | 209 | if (axis_magnitude >= scalar_t(1e-10)) { 210 | vec = vec / axis_magnitude; 211 | vec = vec * wrap_pi(scalar_t(2.0) * atan2(axis_magnitude, q.w())); 212 | } 213 | 214 | return vec; 215 | } 216 | 217 | inline mat3 quat2dcm(const quat& q) { 218 | mat3 dcm; 219 | scalar_t a = q.w(); 220 | scalar_t b = q.x(); 221 | scalar_t c = q.y(); 222 | scalar_t d = q.z(); 223 | scalar_t aSq = a * a; 224 | scalar_t bSq = b * b; 225 | scalar_t cSq = c * c; 226 | scalar_t dSq = d * d; 227 | dcm(0, 0) = aSq + bSq - cSq - dSq; 228 | dcm(0, 1) = 2 * (b * c - a * d); 229 | dcm(0, 2) = 2 * (a * c + b * d); 230 | dcm(1, 0) = 2 * (b * c + a * d); 231 | dcm(1, 1) = aSq - bSq + cSq - dSq; 232 | dcm(1, 2) = 2 * (c * d - a * b); 233 | dcm(2, 0) = 2 * (b * d - a * c); 234 | dcm(2, 1) = 2 * (a * b + c * d); 235 | dcm(2, 2) = aSq - bSq - cSq + dSq; 236 | return dcm; 237 | } 238 | 239 | inline vec3 dcm2vec(const mat3& dcm) { 240 | scalar_t phi_val = atan2(dcm(2, 1), dcm(2, 2)); 241 | scalar_t theta_val = asin(-dcm(2, 0)); 242 | scalar_t psi_val = atan2(dcm(1, 0), dcm(0, 0)); 243 | scalar_t pi = M_PI; 244 | 245 | if (fabs(theta_val - pi / 2) < 1.0e-3) { 246 | phi_val = 0.0; 247 | psi_val = atan2(dcm(1, 2), dcm(0, 2)); 248 | } else if (fabs(theta_val + pi / 2) < 1.0e-3) { 249 | phi_val = 0.0; 250 | psi_val = atan2(-dcm(1, 2), -dcm(0, 2)); 251 | } 252 | return vec3(phi_val, theta_val, psi_val); 253 | } 254 | 255 | // calculate the inverse rotation matrix from a quaternion rotation 256 | inline mat3 quat_to_invrotmat(const quat &q) { 257 | scalar_t q00 = q.w() * q.w(); 258 | scalar_t q11 = q.x() * q.x(); 259 | scalar_t q22 = q.y() * q.y(); 260 | scalar_t q33 = q.z() * q.z(); 261 | scalar_t q01 = q.w() * q.x(); 262 | scalar_t q02 = q.w() * q.y(); 263 | scalar_t q03 = q.w() * q.z(); 264 | scalar_t q12 = q.x() * q.y(); 265 | scalar_t q13 = q.x() * q.z(); 266 | scalar_t q23 = q.y() * q.z(); 267 | 268 | mat3 dcm; 269 | dcm(0, 0) = q00 + q11 - q22 - q33; 270 | dcm(1, 1) = q00 - q11 + q22 - q33; 271 | dcm(2, 2) = q00 - q11 - q22 + q33; 272 | dcm(0, 1) = 2.0f * (q12 - q03); 273 | dcm(0, 2) = 2.0f * (q13 + q02); 274 | dcm(1, 0) = 2.0f * (q12 + q03); 275 | dcm(1, 2) = 2.0f * (q23 - q01); 276 | dcm(2, 0) = 2.0f * (q13 - q02); 277 | dcm(2, 1) = 2.0f * (q23 + q01); 278 | 279 | return dcm; 280 | } 281 | 282 | /** 283 | * Constructor from euler angles 284 | * 285 | * This sets the instance to a quaternion representing coordinate transformation from 286 | * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described 287 | * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. 288 | * 289 | * @param euler euler angle instance 290 | */ 291 | inline quat from_euler(const vec3& euler) { 292 | quat q; 293 | scalar_t cosPhi_2 = cos(euler(0) / 2.0); 294 | scalar_t cosTheta_2 = cos(euler(1) / 2.0); 295 | scalar_t cosPsi_2 = cos(euler(2) / 2.0); 296 | scalar_t sinPhi_2 = sin(euler(0) / 2.0); 297 | scalar_t sinTheta_2 = sin(euler(1) / 2.0); 298 | scalar_t sinPsi_2 = sin(euler(2) / 2.0); 299 | q.w() = cosPhi_2 * cosTheta_2 * cosPsi_2 + 300 | sinPhi_2 * sinTheta_2 * sinPsi_2; 301 | q.x() = sinPhi_2 * cosTheta_2 * cosPsi_2 - 302 | cosPhi_2 * sinTheta_2 * sinPsi_2; 303 | q.y() = cosPhi_2 * sinTheta_2 * cosPsi_2 + 304 | sinPhi_2 * cosTheta_2 * sinPsi_2; 305 | q.z() = cosPhi_2 * cosTheta_2 * sinPsi_2 - 306 | sinPhi_2 * sinTheta_2 * cosPsi_2; 307 | return q; 308 | } 309 | } 310 | 311 | #endif -------------------------------------------------------------------------------- /launch/eskf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | eskf 4 | 0.0.1 5 | Error state kalman filter for position and attitude determination 6 | 7 | etarasov 8 | Elia Tarasov 9 | Apache 2 10 | 11 | catkin 12 | 13 | geometry_msgs 14 | roscpp 15 | sensor_msgs 16 | message_filters 17 | message_generation 18 | eigen_conversions 19 | mavros_msgs 20 | 21 | geometry_msgs 22 | roscpp 23 | sensor_msgs 24 | message_filters 25 | message_runtime 26 | eigen_conversions 27 | mavros_msgs 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/ESKF.cpp: -------------------------------------------------------------------------------- 1 | #ifndef NDEBUG 2 | #define NDEBUG 3 | #endif 4 | 5 | #include 6 | #include 7 | 8 | using namespace Eigen; 9 | 10 | namespace eskf { 11 | 12 | ESKF::ESKF() { 13 | // zeros state_ 14 | state_.quat_nominal = quat(1, 0, 0, 0); 15 | state_.vel = vec3(0, 0, 0); 16 | state_.pos = vec3(0, 0, 0); 17 | state_.gyro_bias = vec3(0, 0, 0); 18 | state_.accel_bias = vec3(0, 0, 0); 19 | state_.mag_I.setZero(); 20 | state_.mag_B.setZero(); 21 | 22 | // zeros P_ 23 | for (unsigned i = 0; i < k_num_states_; i++) { 24 | for (unsigned j = 0; j < k_num_states_; j++) { 25 | P_[i][j] = 0.0f; 26 | } 27 | } 28 | 29 | imu_down_sampled_.delta_ang.setZero(); 30 | imu_down_sampled_.delta_vel.setZero(); 31 | imu_down_sampled_.delta_ang_dt = 0.0f; 32 | imu_down_sampled_.delta_vel_dt = 0.0f; 33 | 34 | q_down_sampled_.w() = 1.0f; 35 | q_down_sampled_.x() = 0.0f; 36 | q_down_sampled_.y() = 0.0f; 37 | q_down_sampled_.z() = 0.0f; 38 | 39 | imu_buffer_.allocate(imu_buffer_length_); 40 | for (int index = 0; index < imu_buffer_length_; index++) { 41 | imuSample imu_sample_init = {}; 42 | imu_buffer_.push(imu_sample_init); 43 | } 44 | 45 | ext_vision_buffer_.allocate(obs_buffer_length_); 46 | for (int index = 0; index < obs_buffer_length_; index++) { 47 | extVisionSample ext_vision_sample_init = {}; 48 | ext_vision_buffer_.push(ext_vision_sample_init); 49 | } 50 | 51 | gps_buffer_.allocate(obs_buffer_length_); 52 | for (int index = 0; index < obs_buffer_length_; index++) { 53 | gpsSample gps_sample_init = {}; 54 | gps_buffer_.push(gps_sample_init); 55 | } 56 | 57 | opt_flow_buffer_.allocate(obs_buffer_length_); 58 | for (int index = 0; index < obs_buffer_length_; index++) { 59 | optFlowSample opt_flow_sample_init = {}; 60 | opt_flow_buffer_.push(opt_flow_sample_init); 61 | } 62 | 63 | range_buffer_.allocate(obs_buffer_length_); 64 | for (int index = 0; index < obs_buffer_length_; index++) { 65 | rangeSample range_sample_init = {}; 66 | range_buffer_.push(range_sample_init); 67 | } 68 | 69 | mag_buffer_.allocate(obs_buffer_length_); 70 | for (int index = 0; index < obs_buffer_length_; index++) { 71 | magSample mag_sample_init = {}; 72 | mag_buffer_.push(mag_sample_init); 73 | } 74 | 75 | dt_ekf_avg_ = 0.001f * (scalar_t)(FILTER_UPDATE_PERIOD_MS); 76 | 77 | ///< filter initialisation 78 | NED_origin_initialised_ = false; 79 | filter_initialised_ = false; 80 | terrain_initialised_ = false; 81 | 82 | imu_updated_ = false; 83 | memset(vel_pos_innov_, 0, 6*sizeof(scalar_t)); 84 | last_known_posNED_ = vec3(0, 0, 0); 85 | } 86 | 87 | void ESKF::initialiseCovariance() { 88 | // define the initial angle uncertainty as variances for a rotation vector 89 | 90 | for (unsigned i = 0; i < k_num_states_; i++) { 91 | for (unsigned j = 0; j < k_num_states_; j++) { 92 | P_[i][j] = 0.0f; 93 | } 94 | } 95 | 96 | // calculate average prediction time step in sec 97 | float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; 98 | 99 | vec3 rot_vec_var; 100 | rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(initial_tilt_err_); 101 | 102 | // update the quaternion state covariances 103 | initialiseQuatCovariances(rot_vec_var); 104 | 105 | // velocity 106 | P_[4][4] = sq(fmaxf(vel_noise_, 0.01f)); 107 | P_[5][5] = P_[4][4]; 108 | P_[6][6] = sq(1.5f) * P_[4][4]; 109 | 110 | // position 111 | P_[7][7] = sq(fmaxf(pos_noise_, 0.01f)); 112 | P_[8][8] = P_[7][7]; 113 | P_[9][9] = sq(fmaxf(range_noise_, 0.01f)); 114 | 115 | // gyro bias 116 | P_[10][10] = sq(switch_on_gyro_bias_ * dt); 117 | P_[11][11] = P_[10][10]; 118 | P_[12][12] = P_[10][10]; 119 | 120 | P_[13][13] = sq(switch_on_accel_bias_ * dt); 121 | P_[14][14] = P_[13][13]; 122 | P_[15][15] = P_[13][13]; 123 | // variances for optional states 124 | 125 | // earth frame and body frame magnetic field 126 | // set to observation variance 127 | for (uint8_t index = 16; index <= 21; index ++) { 128 | P_[index][index] = sq(mag_noise_); 129 | } 130 | } 131 | 132 | bool ESKF::initializeFilter() { 133 | scalar_t pitch = 0.0; 134 | scalar_t roll = 0.0; 135 | scalar_t yaw = 0.0; 136 | imuSample imu_init = imu_buffer_.get_newest(); 137 | static vec3 delVel_sum(0, 0, 0); ///< summed delta velocity (m/sec) 138 | delVel_sum += imu_init.delta_vel; 139 | if (delVel_sum.norm() > 0.001) { 140 | delVel_sum.normalize(); 141 | pitch = asin(delVel_sum(0)); 142 | roll = atan2(-delVel_sum(1), -delVel_sum(2)); 143 | } else { 144 | return false; 145 | } 146 | // calculate initial tilt alignment 147 | state_.quat_nominal = AngleAxis(yaw, vec3::UnitZ()) * AngleAxis(pitch, vec3::UnitY()) * AngleAxis(roll, vec3::UnitX()); 148 | // update transformation matrix from body to world frame 149 | R_to_earth_ = quat_to_invrotmat(state_.quat_nominal); 150 | initialiseCovariance(); 151 | return true; 152 | } 153 | 154 | bool ESKF::collect_imu(imuSample &imu) { 155 | // accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long 156 | 157 | // copy imu data to local variables 158 | imu_sample_new_.delta_ang = imu.delta_ang; 159 | imu_sample_new_.delta_vel = imu.delta_vel; 160 | imu_sample_new_.delta_ang_dt = imu.delta_ang_dt; 161 | imu_sample_new_.delta_vel_dt = imu.delta_vel_dt; 162 | imu_sample_new_.time_us = imu.time_us; 163 | 164 | // accumulate the time deltas 165 | imu_down_sampled_.delta_ang_dt += imu.delta_ang_dt; 166 | imu_down_sampled_.delta_vel_dt += imu.delta_vel_dt; 167 | 168 | // use a quaternion to accumulate delta angle data 169 | // this quaternion represents the rotation from the start to end of the accumulation period 170 | quat delta_q(1, 0, 0, 0); 171 | quat res = from_axis_angle(imu.delta_ang); 172 | delta_q = delta_q * res; 173 | q_down_sampled_ = q_down_sampled_ * delta_q; 174 | q_down_sampled_.normalize(); 175 | 176 | // rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame 177 | mat3 delta_R = quat2dcm(delta_q.inverse()); 178 | imu_down_sampled_.delta_vel = delta_R * imu_down_sampled_.delta_vel; 179 | 180 | // accumulate the most recent delta velocity data at the updated rotation frame 181 | // assume effective sample time is halfway between the previous and current rotation frame 182 | imu_down_sampled_.delta_vel += (imu_sample_new_.delta_vel + delta_R * imu_sample_new_.delta_vel) * 0.5f; 183 | 184 | // if the target time delta between filter prediction steps has been exceeded 185 | // write the accumulated IMU data to the ring buffer 186 | scalar_t target_dt = (scalar_t)(FILTER_UPDATE_PERIOD_MS) / 1000; 187 | 188 | if (imu_down_sampled_.delta_ang_dt >= target_dt - imu_collection_time_adj_) { 189 | 190 | // accumulate the amount of time to advance the IMU collection time so that we meet the 191 | // average EKF update rate requirement 192 | imu_collection_time_adj_ += 0.01f * (imu_down_sampled_.delta_ang_dt - target_dt); 193 | imu_collection_time_adj_ = constrain(imu_collection_time_adj_, -0.5f * target_dt, 0.5f * target_dt); 194 | 195 | imu.delta_ang = to_axis_angle(q_down_sampled_); 196 | imu.delta_vel = imu_down_sampled_.delta_vel; 197 | imu.delta_ang_dt = imu_down_sampled_.delta_ang_dt; 198 | imu.delta_vel_dt = imu_down_sampled_.delta_vel_dt; 199 | 200 | imu_down_sampled_.delta_ang.setZero(); 201 | imu_down_sampled_.delta_vel.setZero(); 202 | imu_down_sampled_.delta_ang_dt = 0.0f; 203 | imu_down_sampled_.delta_vel_dt = 0.0f; 204 | q_down_sampled_.w() = 1.0f; 205 | q_down_sampled_.x() = q_down_sampled_.y() = q_down_sampled_.z() = 0.0f; 206 | 207 | return true; 208 | } 209 | 210 | min_obs_interval_us_ = (imu_sample_new_.time_us - imu_sample_delayed_.time_us) / (obs_buffer_length_ - 1); 211 | 212 | return false; 213 | } 214 | 215 | void ESKF::run(const vec3 &w, const vec3 &a, uint64_t time_us, scalar_t dt) { 216 | // convert FLU to FRD body frame IMU data 217 | vec3 gyro_b = q_FLU2FRD.toRotationMatrix() * w; 218 | vec3 accel_b = q_FLU2FRD.toRotationMatrix() * a; 219 | 220 | vec3 delta_ang = vec3(gyro_b.x(), gyro_b.y(), gyro_b.z()) * dt; // current delta angle (rad) 221 | vec3 delta_vel = vec3(accel_b.x(), accel_b.y(), accel_b.z()) * dt; //current delta velocity (m/s) 222 | 223 | // copy data 224 | imuSample imu_sample_new = {}; 225 | imu_sample_new.delta_ang = delta_ang; 226 | imu_sample_new.delta_vel = delta_vel; 227 | imu_sample_new.delta_ang_dt = dt; 228 | imu_sample_new.delta_vel_dt = dt; 229 | imu_sample_new.time_us = time_us; 230 | 231 | time_last_imu_ = time_us; 232 | 233 | if(collect_imu(imu_sample_new)) { 234 | imu_buffer_.push(imu_sample_new); 235 | imu_updated_ = true; 236 | // get the oldest data from the buffer 237 | imu_sample_delayed_ = imu_buffer_.get_oldest(); 238 | } else { 239 | imu_updated_ = false; 240 | return; 241 | } 242 | 243 | if (!filter_initialised_) { 244 | filter_initialised_ = initializeFilter(); 245 | 246 | if (!filter_initialised_) { 247 | return; 248 | } 249 | } 250 | 251 | if(!imu_updated_) return; 252 | 253 | // apply imu bias corrections 254 | vec3 corrected_delta_ang = imu_sample_delayed_.delta_ang - state_.gyro_bias; 255 | vec3 corrected_delta_vel = imu_sample_delayed_.delta_vel - state_.accel_bias; 256 | 257 | // convert the delta angle to a delta quaternion 258 | quat dq; 259 | dq = from_axis_angle(corrected_delta_ang); 260 | // rotate the previous quaternion by the delta quaternion using a quaternion multiplication 261 | state_.quat_nominal = state_.quat_nominal * dq; 262 | // quaternions must be normalised whenever they are modified 263 | state_.quat_nominal.normalize(); 264 | 265 | // save the previous value of velocity so we can use trapezoidal integration 266 | vec3 vel_last = state_.vel; 267 | 268 | // update transformation matrix from body to world frame 269 | R_to_earth_ = quat_to_invrotmat(state_.quat_nominal); 270 | 271 | // Calculate an earth frame delta velocity 272 | vec3 corrected_delta_vel_ef = R_to_earth_ * corrected_delta_vel; 273 | 274 | // calculate the increment in velocity using the current orientation 275 | state_.vel += corrected_delta_vel_ef; 276 | 277 | // compensate for acceleration due to gravity 278 | state_.vel(2) += kOneG * imu_sample_delayed_.delta_vel_dt; 279 | 280 | // predict position states via trapezoidal integration of velocity 281 | state_.pos += (vel_last + state_.vel) * imu_sample_delayed_.delta_vel_dt * 0.5f; 282 | 283 | constrainStates(); 284 | 285 | // calculate an average filter update time 286 | scalar_t input = 0.5f * (imu_sample_delayed_.delta_vel_dt + imu_sample_delayed_.delta_ang_dt); 287 | 288 | // filter and limit input between -50% and +100% of nominal value 289 | input = constrain(input, 0.0005f * (scalar_t)(FILTER_UPDATE_PERIOD_MS), 0.002f * (scalar_t)(FILTER_UPDATE_PERIOD_MS)); 290 | dt_ekf_avg_ = 0.99f * dt_ekf_avg_ + 0.01f * input; 291 | 292 | predictCovariance(); 293 | controlFusionModes(); 294 | } 295 | 296 | void ESKF::predictCovariance() { 297 | // error-state jacobian 298 | // assign intermediate state variables 299 | scalar_t q0 = state_.quat_nominal.w(); 300 | scalar_t q1 = state_.quat_nominal.x(); 301 | scalar_t q2 = state_.quat_nominal.y(); 302 | scalar_t q3 = state_.quat_nominal.z(); 303 | 304 | scalar_t dax = imu_sample_delayed_.delta_ang(0); 305 | scalar_t day = imu_sample_delayed_.delta_ang(1); 306 | scalar_t daz = imu_sample_delayed_.delta_ang(2); 307 | 308 | scalar_t dvx = imu_sample_delayed_.delta_vel(0); 309 | scalar_t dvy = imu_sample_delayed_.delta_vel(1); 310 | scalar_t dvz = imu_sample_delayed_.delta_vel(2); 311 | 312 | scalar_t dax_b = state_.gyro_bias(0); 313 | scalar_t day_b = state_.gyro_bias(1); 314 | scalar_t daz_b = state_.gyro_bias(2); 315 | 316 | scalar_t dvx_b = state_.accel_bias(0); 317 | scalar_t dvy_b = state_.accel_bias(1); 318 | scalar_t dvz_b = state_.accel_bias(2); 319 | 320 | // compute noise variance for stationary processes 321 | scalar_t process_noise[k_num_states_] = {}; 322 | 323 | 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)); 324 | 325 | // 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 326 | scalar_t d_ang_bias_sig = dt * dt * constrain(gyro_bias_p_noise_, 0.0f, 1.0f); 327 | 328 | // 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 329 | scalar_t d_vel_bias_sig = dt * dt * constrain(accel_bias_p_noise_, 0.0f, 1.0f); 330 | 331 | // 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 332 | scalar_t mag_I_sig; 333 | 334 | if (mag_3D_ && (P_[16][16] + P_[17][17] + P_[18][18]) < 0.1f) { 335 | mag_I_sig = dt * constrain(mage_p_noise_, 0.0f, 1.0f); 336 | } else { 337 | mag_I_sig = 0.0f; 338 | } 339 | 340 | // 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 341 | scalar_t mag_B_sig; 342 | 343 | if (mag_3D_ && (P_[19][19] + P_[20][20] + P_[21][21]) < 0.1f) { 344 | mag_B_sig = dt * constrain(magb_p_noise_, 0.0f, 1.0f); 345 | } else { 346 | mag_B_sig = 0.0f; 347 | } 348 | 349 | // Construct the process noise variance diagonal for those states with a stationary process model 350 | // These are kinematic states and their error growth is controlled separately by the IMU noise variances 351 | for (unsigned i = 0; i <= 9; i++) { 352 | process_noise[i] = 0.0; 353 | } 354 | 355 | // delta angle bias states 356 | process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig); 357 | // delta_velocity bias states 358 | process_noise[15] = process_noise[14] = process_noise[13] = sq(d_vel_bias_sig); 359 | // earth frame magnetic field states 360 | process_noise[18] = process_noise[17] = process_noise[16] = sq(mag_I_sig); 361 | // body frame magnetic field states 362 | process_noise[21] = process_noise[20] = process_noise[19] = sq(mag_B_sig); 363 | 364 | // assign IMU noise variances 365 | // inputs to the system are 3 delta angles and 3 delta velocities 366 | scalar_t daxVar, dayVar, dazVar; 367 | scalar_t dvxVar, dvyVar, dvzVar; 368 | daxVar = dayVar = dazVar = sq(dt * gyro_noise_); // gyro prediction variance TODO get variance from sensor 369 | dvxVar = dvyVar = dvzVar = sq(dt * accel_noise_); //accel prediction variance TODO get variance from sensor 370 | 371 | // intermediate calculations 372 | scalar_t SF[21]; 373 | SF[0] = dvz - dvz_b; 374 | SF[1] = dvy - dvy_b; 375 | SF[2] = dvx - dvx_b; 376 | SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0]; 377 | SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2]; 378 | SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1]; 379 | SF[6] = day/2 - day_b/2; 380 | SF[7] = daz/2 - daz_b/2; 381 | SF[8] = dax/2 - dax_b/2; 382 | SF[9] = dax_b/2 - dax/2; 383 | SF[10] = daz_b/2 - daz/2; 384 | SF[11] = day_b/2 - day/2; 385 | SF[12] = 2*q1*SF[1]; 386 | SF[13] = 2*q0*SF[0]; 387 | SF[14] = q1/2; 388 | SF[15] = q2/2; 389 | SF[16] = q3/2; 390 | SF[17] = sq(q3); 391 | SF[18] = sq(q2); 392 | SF[19] = sq(q1); 393 | SF[20] = sq(q0); 394 | 395 | scalar_t SG[8]; 396 | SG[0] = q0/2; 397 | SG[1] = sq(q3); 398 | SG[2] = sq(q2); 399 | SG[3] = sq(q1); 400 | SG[4] = sq(q0); 401 | SG[5] = 2*q2*q3; 402 | SG[6] = 2*q1*q3; 403 | SG[7] = 2*q1*q2; 404 | 405 | scalar_t SQ[11]; 406 | 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); 407 | 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); 408 | 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]); 409 | SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4; 410 | SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4; 411 | SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4; 412 | SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4; 413 | SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2; 414 | SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4; 415 | SQ[9] = sq(SG[0]); 416 | SQ[10] = sq(q1); 417 | 418 | scalar_t SPP[11]; 419 | SPP[0] = SF[12] + SF[13] - 2*q2*SF[2]; 420 | SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; 421 | SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; 422 | SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; 423 | SPP[4] = 2*q0*q2 - 2*q1*q3; 424 | SPP[5] = 2*q0*q1 - 2*q2*q3; 425 | SPP[6] = 2*q0*q3 - 2*q1*q2; 426 | SPP[7] = 2*q0*q1 + 2*q2*q3; 427 | SPP[8] = 2*q0*q3 + 2*q1*q2; 428 | SPP[9] = 2*q0*q2 + 2*q1*q3; 429 | SPP[10] = SF[16]; 430 | 431 | // covariance update 432 | // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states 433 | scalar_t nextP[k_num_states_][k_num_states_]; 434 | 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; 435 | 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; 436 | 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; 437 | 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; 438 | 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; 439 | 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; 440 | 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; 441 | 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; 442 | 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; 443 | 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; 444 | 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]); 445 | 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); 446 | 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); 447 | 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); 448 | 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]); 449 | 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]); 450 | 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); 451 | 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); 452 | 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); 453 | 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]); 454 | 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]); 455 | 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]); 456 | 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); 457 | 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); 458 | 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); 459 | 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]); 460 | 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]); 461 | 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]); 462 | 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]); 463 | 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); 464 | 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); 465 | 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); 466 | 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]); 467 | 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]); 468 | 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]); 469 | nextP[7][7] = P_[7][7] + P_[4][7]*dt + dt*(P_[7][4] + P_[4][4]*dt); 470 | 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]); 471 | 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); 472 | 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); 473 | 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); 474 | 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]); 475 | 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]); 476 | 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]); 477 | nextP[7][8] = P_[7][8] + P_[4][8]*dt + dt*(P_[7][5] + P_[4][5]*dt); 478 | nextP[8][8] = P_[8][8] + P_[5][8]*dt + dt*(P_[8][5] + P_[5][5]*dt); 479 | 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]); 480 | 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); 481 | 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); 482 | 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); 483 | 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]); 484 | 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]); 485 | 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]); 486 | nextP[7][9] = P_[7][9] + P_[4][9]*dt + dt*(P_[7][6] + P_[4][6]*dt); 487 | nextP[8][9] = P_[8][9] + P_[5][9]*dt + dt*(P_[8][6] + P_[5][6]*dt); 488 | nextP[9][9] = P_[9][9] + P_[6][9]*dt + dt*(P_[9][6] + P_[6][6]*dt); 489 | 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]; 490 | 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; 491 | 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; 492 | 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; 493 | 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]; 494 | 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]; 495 | 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]; 496 | nextP[7][10] = P_[7][10] + P_[4][10]*dt; 497 | nextP[8][10] = P_[8][10] + P_[5][10]*dt; 498 | nextP[9][10] = P_[9][10] + P_[6][10]*dt; 499 | nextP[10][10] = P_[10][10]; 500 | 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]; 501 | 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; 502 | 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; 503 | 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; 504 | 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]; 505 | 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]; 506 | 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]; 507 | nextP[7][11] = P_[7][11] + P_[4][11]*dt; 508 | nextP[8][11] = P_[8][11] + P_[5][11]*dt; 509 | nextP[9][11] = P_[9][11] + P_[6][11]*dt; 510 | nextP[10][11] = P_[10][11]; 511 | nextP[11][11] = P_[11][11]; 512 | 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]; 513 | 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; 514 | 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; 515 | 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; 516 | 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]; 517 | 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]; 518 | 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]; 519 | nextP[7][12] = P_[7][12] + P_[4][12]*dt; 520 | nextP[8][12] = P_[8][12] + P_[5][12]*dt; 521 | nextP[9][12] = P_[9][12] + P_[6][12]*dt; 522 | nextP[10][12] = P_[10][12]; 523 | nextP[11][12] = P_[11][12]; 524 | nextP[12][12] = P_[12][12]; 525 | 526 | // add process noise that is not from the IMU 527 | for (unsigned i = 0; i <= 12; i++) { 528 | nextP[i][i] += process_noise[i]; 529 | } 530 | 531 | // calculate variances and upper diagonal covariances for IMU delta velocity bias states 532 | 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]; 533 | 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; 534 | 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; 535 | 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; 536 | 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]; 537 | 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]; 538 | 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]; 539 | nextP[7][13] = P_[7][13] + P_[4][13]*dt; 540 | nextP[8][13] = P_[8][13] + P_[5][13]*dt; 541 | nextP[9][13] = P_[9][13] + P_[6][13]*dt; 542 | nextP[10][13] = P_[10][13]; 543 | nextP[11][13] = P_[11][13]; 544 | nextP[12][13] = P_[12][13]; 545 | nextP[13][13] = P_[13][13]; 546 | 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]; 547 | 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; 548 | 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; 549 | 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; 550 | 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]; 551 | 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]; 552 | 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]; 553 | nextP[7][14] = P_[7][14] + P_[4][14]*dt; 554 | nextP[8][14] = P_[8][14] + P_[5][14]*dt; 555 | nextP[9][14] = P_[9][14] + P_[6][14]*dt; 556 | nextP[10][14] = P_[10][14]; 557 | nextP[11][14] = P_[11][14]; 558 | nextP[12][14] = P_[12][14]; 559 | nextP[13][14] = P_[13][14]; 560 | nextP[14][14] = P_[14][14]; 561 | 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]; 562 | 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; 563 | 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; 564 | 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; 565 | 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]; 566 | 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]; 567 | 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]; 568 | nextP[7][15] = P_[7][15] + P_[4][15]*dt; 569 | nextP[8][15] = P_[8][15] + P_[5][15]*dt; 570 | nextP[9][15] = P_[9][15] + P_[6][15]*dt; 571 | nextP[10][15] = P_[10][15]; 572 | nextP[11][15] = P_[11][15]; 573 | nextP[12][15] = P_[12][15]; 574 | nextP[13][15] = P_[13][15]; 575 | nextP[14][15] = P_[14][15]; 576 | nextP[15][15] = P_[15][15]; 577 | 578 | // add process noise that is not from the IMU 579 | for (unsigned i = 13; i <= 15; i++) { 580 | nextP[i][i] += process_noise[i]; 581 | } 582 | 583 | // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion 584 | if (mag_3D_) { 585 | // calculate variances and upper diagonal covariances for earth and body magnetic field states 586 | 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]; 587 | 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; 588 | 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; 589 | 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; 590 | 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]; 591 | 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]; 592 | 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]; 593 | nextP[7][16] = P_[7][16] + P_[4][16]*dt; 594 | nextP[8][16] = P_[8][16] + P_[5][16]*dt; 595 | nextP[9][16] = P_[9][16] + P_[6][16]*dt; 596 | nextP[10][16] = P_[10][16]; 597 | nextP[11][16] = P_[11][16]; 598 | nextP[12][16] = P_[12][16]; 599 | nextP[13][16] = P_[13][16]; 600 | nextP[14][16] = P_[14][16]; 601 | nextP[15][16] = P_[15][16]; 602 | nextP[16][16] = P_[16][16]; 603 | 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]; 604 | 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; 605 | 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; 606 | 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; 607 | 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]; 608 | 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]; 609 | 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]; 610 | nextP[7][17] = P_[7][17] + P_[4][17]*dt; 611 | nextP[8][17] = P_[8][17] + P_[5][17]*dt; 612 | nextP[9][17] = P_[9][17] + P_[6][17]*dt; 613 | nextP[10][17] = P_[10][17]; 614 | nextP[11][17] = P_[11][17]; 615 | nextP[12][17] = P_[12][17]; 616 | nextP[13][17] = P_[13][17]; 617 | nextP[14][17] = P_[14][17]; 618 | nextP[15][17] = P_[15][17]; 619 | nextP[16][17] = P_[16][17]; 620 | nextP[17][17] = P_[17][17]; 621 | 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]; 622 | 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; 623 | 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; 624 | 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; 625 | 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]; 626 | 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]; 627 | 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]; 628 | nextP[7][18] = P_[7][18] + P_[4][18]*dt; 629 | nextP[8][18] = P_[8][18] + P_[5][18]*dt; 630 | nextP[9][18] = P_[9][18] + P_[6][18]*dt; 631 | nextP[10][18] = P_[10][18]; 632 | nextP[11][18] = P_[11][18]; 633 | nextP[12][18] = P_[12][18]; 634 | nextP[13][18] = P_[13][18]; 635 | nextP[14][18] = P_[14][18]; 636 | nextP[15][18] = P_[15][18]; 637 | nextP[16][18] = P_[16][18]; 638 | nextP[17][18] = P_[17][18]; 639 | nextP[18][18] = P_[18][18]; 640 | 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]; 641 | 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; 642 | 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; 643 | 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; 644 | 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]; 645 | 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]; 646 | 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]; 647 | nextP[7][19] = P_[7][19] + P_[4][19]*dt; 648 | nextP[8][19] = P_[8][19] + P_[5][19]*dt; 649 | nextP[9][19] = P_[9][19] + P_[6][19]*dt; 650 | nextP[10][19] = P_[10][19]; 651 | nextP[11][19] = P_[11][19]; 652 | nextP[12][19] = P_[12][19]; 653 | nextP[13][19] = P_[13][19]; 654 | nextP[14][19] = P_[14][19]; 655 | nextP[15][19] = P_[15][19]; 656 | nextP[16][19] = P_[16][19]; 657 | nextP[17][19] = P_[17][19]; 658 | nextP[18][19] = P_[18][19]; 659 | nextP[19][19] = P_[19][19]; 660 | 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]; 661 | 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; 662 | 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; 663 | 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; 664 | 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]; 665 | 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]; 666 | 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]; 667 | nextP[7][20] = P_[7][20] + P_[4][20]*dt; 668 | nextP[8][20] = P_[8][20] + P_[5][20]*dt; 669 | nextP[9][20] = P_[9][20] + P_[6][20]*dt; 670 | nextP[10][20] = P_[10][20]; 671 | nextP[11][20] = P_[11][20]; 672 | nextP[12][20] = P_[12][20]; 673 | nextP[13][20] = P_[13][20]; 674 | nextP[14][20] = P_[14][20]; 675 | nextP[15][20] = P_[15][20]; 676 | nextP[16][20] = P_[16][20]; 677 | nextP[17][20] = P_[17][20]; 678 | nextP[18][20] = P_[18][20]; 679 | nextP[19][20] = P_[19][20]; 680 | nextP[20][20] = P_[20][20]; 681 | 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]; 682 | 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; 683 | 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; 684 | 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; 685 | 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]; 686 | 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]; 687 | 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]; 688 | nextP[7][21] = P_[7][21] + P_[4][21]*dt; 689 | nextP[8][21] = P_[8][21] + P_[5][21]*dt; 690 | nextP[9][21] = P_[9][21] + P_[6][21]*dt; 691 | nextP[10][21] = P_[10][21]; 692 | nextP[11][21] = P_[11][21]; 693 | nextP[12][21] = P_[12][21]; 694 | nextP[13][21] = P_[13][21]; 695 | nextP[14][21] = P_[14][21]; 696 | nextP[15][21] = P_[15][21]; 697 | nextP[16][21] = P_[16][21]; 698 | nextP[17][21] = P_[17][21]; 699 | nextP[18][21] = P_[18][21]; 700 | nextP[19][21] = P_[19][21]; 701 | nextP[20][21] = P_[20][21]; 702 | nextP[21][21] = P_[21][21]; 703 | 704 | // add process noise that is not from the IMU 705 | for (unsigned i = 16; i <= 21; i++) { 706 | nextP[i][i] += process_noise[i]; 707 | } 708 | } 709 | 710 | // stop position covariance growth if our total position variance reaches 100m 711 | if ((P_[7][7] + P_[8][8]) > 1.0e4) { 712 | for (uint8_t i = 7; i <= 8; i++) { 713 | for (uint8_t j = 0; j < k_num_states_; j++) { 714 | nextP[i][j] = P_[i][j]; 715 | nextP[j][i] = P_[j][i]; 716 | } 717 | } 718 | } 719 | 720 | // covariance matrix is symmetrical, so copy upper half to lower half 721 | for (unsigned row = 1; row < k_num_states_; row++) { 722 | for (unsigned column = 0 ; column < row; column++) { 723 | P_[row][column] = P_[column][row] = nextP[column][row]; 724 | } 725 | } 726 | 727 | // copy variances (diagonals) 728 | for (unsigned i = 0; i < k_num_states_; i++) { 729 | P_[i][i] = nextP[i][i]; 730 | } 731 | 732 | fixCovarianceErrors(); 733 | } 734 | 735 | void ESKF::controlFusionModes() { 736 | 737 | gps_data_ready_ = gps_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &gps_sample_delayed_); 738 | vision_data_ready_ = ext_vision_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &ev_sample_delayed_); 739 | flow_data_ready_ = opt_flow_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &opt_flow_sample_delayed_); 740 | mag_data_ready_ = mag_buffer_.pop_first_older_than(imu_sample_delayed_.time_us, &mag_sample_delayed_); 741 | 742 | R_rng_to_earth_2_2_ = R_to_earth_(2, 0) * sin_tilt_rng_ + R_to_earth_(2, 2) * cos_tilt_rng_; 743 | 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_); 744 | 745 | controlHeightSensorTimeouts(); 746 | 747 | // For efficiency, fusion of direct state observations for position and velocity is performed sequentially 748 | // in a single function using sensor data from multiple sources 749 | controlVelPosFusion(); 750 | 751 | controlExternalVisionFusion(); 752 | controlGpsFusion(); 753 | controlOpticalFlowFusion(); 754 | controlMagFusion(); 755 | 756 | runTerrainEstimator(); 757 | } 758 | 759 | void ESKF::controlOpticalFlowFusion() { 760 | // Accumulate autopilot gyro data across the same time interval as the flow sensor 761 | imu_del_ang_of_ += imu_sample_delayed_.delta_ang - state_.gyro_bias; 762 | delta_time_of_ += imu_sample_delayed_.delta_ang_dt; 763 | 764 | if(flow_data_ready_) { 765 | // we are not yet using flow data 766 | if ((fusion_mask_ & MASK_OPTICAL_FLOW) && (!opt_flow_)) { 767 | opt_flow_ = true; 768 | printf("ESKF commencing optical flow fusion\n"); 769 | } 770 | 771 | // Only fuse optical flow if valid body rate compensation data is available 772 | if (calcOptFlowBodyRateComp()) { 773 | bool flow_quality_good = (opt_flow_sample_delayed_.quality >= flow_quality_min_); 774 | 775 | if (!flow_quality_good && !in_air_) { 776 | // when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate 777 | flow_rad_xy_comp_ = vec2(0,0); 778 | } else { 779 | // compensate for body motion to give a LOS rate 780 | flow_rad_xy_comp_(0) = opt_flow_sample_delayed_.flowRadXY(0) - opt_flow_sample_delayed_.gyroXY(0); 781 | flow_rad_xy_comp_(1) = opt_flow_sample_delayed_.flowRadXY(1) - opt_flow_sample_delayed_.gyroXY(1); 782 | } 783 | } else { 784 | // don't use this flow data and wait for the next data to arrive 785 | flow_data_ready_ = false; 786 | } 787 | } 788 | 789 | // Wait until the midpoint of the flow sample has fallen behind the fusion time horizon 790 | if (flow_data_ready_ && (imu_sample_delayed_.time_us > opt_flow_sample_delayed_.time_us - uint32_t(1e6f * opt_flow_sample_delayed_.dt) / 2)) { 791 | // 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 792 | if (opt_flow_ && (time_last_imu_ - time_last_hagl_fuse_ < (uint64_t)10e6)) { 793 | fuseOptFlow(); 794 | } 795 | flow_data_ready_ = false; 796 | } 797 | } 798 | 799 | // calculate optical flow body angular rate compensation 800 | // returns false if bias corrected body rate data is unavailable 801 | bool ESKF::calcOptFlowBodyRateComp() { 802 | // reset the accumulators if the time interval is too large 803 | if (delta_time_of_ > 1.0f) { 804 | imu_del_ang_of_.setZero(); 805 | delta_time_of_ = 0.0f; 806 | return false; 807 | } 808 | 809 | // 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 810 | opt_flow_sample_delayed_.gyroXY(0) = -imu_del_ang_of_(0); 811 | opt_flow_sample_delayed_.gyroXY(1) = -imu_del_ang_of_(1); 812 | 813 | // reset the accumulators 814 | imu_del_ang_of_.setZero(); 815 | delta_time_of_ = 0.0f; 816 | return true; 817 | } 818 | 819 | void ESKF::runTerrainEstimator() { 820 | // Perform initialisation check 821 | if (!terrain_initialised_) { 822 | terrain_initialised_ = initHagl(); 823 | } else { 824 | // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle 825 | 826 | // process noise due to errors in vehicle height estimate 827 | terrain_var_ += sq(imu_sample_delayed_.delta_vel_dt * terrain_p_noise_); 828 | 829 | // process noise due to terrain gradient 830 | terrain_var_ += sq(imu_sample_delayed_.delta_vel_dt * terrain_gradient_) * (sq(state_.vel(0)) + sq(state_.vel(1))); 831 | 832 | // limit the variance to prevent it becoming badly conditioned 833 | terrain_var_ = constrain(terrain_var_, 0.0f, 1e4f); 834 | 835 | // Fuse range finder data if available 836 | if (range_data_ready_) { 837 | // determine if we should use the hgt observation 838 | if ((fusion_mask_ & MASK_RANGEFINDER) && (!rng_hgt_)) { 839 | if (time_last_imu_ - time_last_range_ < 2 * RANGE_MAX_INTERVAL) { 840 | rng_hgt_ = true; 841 | printf("ESKF commencing rng fusion\n"); 842 | } 843 | } 844 | 845 | if(rng_hgt_) { 846 | fuseHagl(); 847 | } 848 | 849 | // update range sensor angle parameters in case they have changed 850 | // we do this here to avoid doing those calculations at a high rate 851 | sin_tilt_rng_ = sinf(rng_sens_pitch_); 852 | cos_tilt_rng_ = cosf(rng_sens_pitch_); 853 | fuse_height_ = true; 854 | } 855 | 856 | //constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) 857 | if (terrain_vpos_ - state_.pos(2) < rng_gnd_clearance_) { 858 | terrain_vpos_ = rng_gnd_clearance_ + state_.pos(2); 859 | } 860 | } 861 | } 862 | 863 | void ESKF::fuseHagl() { 864 | // If the vehicle is excessively tilted, do not try to fuse range finder observations 865 | if (R_rng_to_earth_2_2_ > range_cos_max_tilt_) { 866 | // get a height above ground measurement from the range finder assuming a flat earth 867 | scalar_t meas_hagl = range_sample_delayed_.rng * R_rng_to_earth_2_2_; 868 | 869 | // predict the hagl from the vehicle position and terrain height 870 | scalar_t pred_hagl = terrain_vpos_ - state_.pos(2); 871 | 872 | // calculate the innovation 873 | hagl_innov_ = pred_hagl - meas_hagl; 874 | 875 | // calculate the observation variance adding the variance of the vehicles own height uncertainty 876 | scalar_t obs_variance = fmaxf(P_[9][9], 0.0f) + sq(range_noise_) + sq(range_noise_scaler_ * range_sample_delayed_.rng); 877 | 878 | // calculate the innovation variance - limiting it to prevent a badly conditioned fusion 879 | hagl_innov_var_ = fmaxf(terrain_var_ + obs_variance, obs_variance); 880 | 881 | // perform an innovation consistency check and only fuse data if it passes 882 | scalar_t gate_size = fmaxf(range_innov_gate_, 1.0f); 883 | terr_test_ratio_ = sq(hagl_innov_) / (sq(gate_size) * hagl_innov_var_); 884 | 885 | if (terr_test_ratio_ <= 1.0f) { 886 | // calculate the Kalman gain 887 | scalar_t gain = terrain_var_ / hagl_innov_var_; 888 | // correct the state 889 | terrain_vpos_ -= gain * hagl_innov_; 890 | // correct the variance 891 | terrain_var_ = fmaxf(terrain_var_ * (1.0f - gain), 0.0f); 892 | // record last successful fusion event 893 | time_last_hagl_fuse_ = time_last_imu_; 894 | } else { 895 | // If we have been rejecting range data for too long, reset to measurement 896 | if (time_last_imu_ - time_last_hagl_fuse_ > (uint64_t)10E6) { 897 | terrain_vpos_ = state_.pos(2) + meas_hagl; 898 | terrain_var_ = obs_variance; 899 | } 900 | } 901 | } 902 | } 903 | 904 | void ESKF::fuseOptFlow() { 905 | scalar_t optflow_test_ratio[2] = {0}; 906 | 907 | // get latest estimated orientation 908 | scalar_t q0 = state_.quat_nominal.w(); 909 | scalar_t q1 = state_.quat_nominal.x(); 910 | scalar_t q2 = state_.quat_nominal.y(); 911 | scalar_t q3 = state_.quat_nominal.z(); 912 | 913 | // get latest velocity in earth frame 914 | scalar_t vn = state_.vel(0); 915 | scalar_t ve = state_.vel(1); 916 | scalar_t vd = state_.vel(2); 917 | 918 | // calculate the optical flow observation variance 919 | // calculate the observation noise variance - scaling noise linearly across flow quality range 920 | scalar_t R_LOS_best = fmaxf(flow_noise_, 0.05f); 921 | scalar_t R_LOS_worst = fmaxf(flow_noise_qual_min_, 0.05f); 922 | 923 | // calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst 924 | scalar_t weighting = (255.0f - (scalar_t)flow_quality_min_); 925 | 926 | if (weighting >= 1.0f) { 927 | weighting = constrain(((scalar_t)opt_flow_sample_delayed_.quality - (scalar_t)flow_quality_min_) / weighting, 0.0f, 1.0f); 928 | } else { 929 | weighting = 0.0f; 930 | } 931 | 932 | // take the weighted average of the observation noie for the best and wort flow quality 933 | scalar_t R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); 934 | 935 | scalar_t H_LOS[2][k_num_states_] = {}; // Optical flow observation Jacobians 936 | scalar_t Kfusion[k_num_states_][2] = {}; // Optical flow Kalman gains 937 | 938 | // constrain height above ground to be above minimum height when sitting on ground 939 | scalar_t heightAboveGndEst = max((terrain_vpos_ - state_.pos(2)), rng_gnd_clearance_); 940 | 941 | mat3 earth_to_body = R_to_earth_.transpose(); 942 | 943 | // rotate into body frame 944 | vec3 vel_body = earth_to_body * state_.vel; 945 | 946 | // calculate range from focal point to centre of image 947 | scalar_t range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view 948 | 949 | // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed 950 | // correct for gyro bias errors in the data used to do the motion compensation 951 | // Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis. 952 | vec2 opt_flow_rate; 953 | opt_flow_rate(0) = flow_rad_xy_comp_(0) / opt_flow_sample_delayed_.dt;// + _flow_gyro_bias(0); 954 | opt_flow_rate(1) = flow_rad_xy_comp_(1) / opt_flow_sample_delayed_.dt;// + _flow_gyro_bias(1); 955 | 956 | if (opt_flow_rate.norm() < flow_max_rate_) { 957 | flow_innov_[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis 958 | flow_innov_[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis 959 | } else { 960 | return; 961 | } 962 | 963 | // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated 964 | // Calculate Obser ation Jacobians and Kalman gans for each measurement axis 965 | for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { 966 | if (obs_index == 0) { 967 | // calculate X axis observation Jacobian 968 | float t2 = 1.0f / range; 969 | H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); 970 | H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); 971 | H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); 972 | H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); 973 | H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f); 974 | H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); 975 | H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f); 976 | 977 | // calculate intermediate variables for the X observaton innovatoin variance and Kalman gains 978 | float t3 = q1*vd*2.0f; 979 | float t4 = q0*ve*2.0f; 980 | float t11 = q3*vn*2.0f; 981 | float t5 = t3+t4-t11; 982 | float t6 = q0*q3*2.0f; 983 | float t29 = q1*q2*2.0f; 984 | float t7 = t6-t29; 985 | float t8 = q0*q1*2.0f; 986 | float t9 = q2*q3*2.0f; 987 | float t10 = t8+t9; 988 | float t12 = P_[0][0]*t2*t5; 989 | float t13 = q0*vd*2.0f; 990 | float t14 = q2*vn*2.0f; 991 | float t28 = q1*ve*2.0f; 992 | float t15 = t13+t14-t28; 993 | float t16 = q3*vd*2.0f; 994 | float t17 = q2*ve*2.0f; 995 | float t18 = q1*vn*2.0f; 996 | float t19 = t16+t17+t18; 997 | float t20 = q3*ve*2.0f; 998 | float t21 = q0*vn*2.0f; 999 | float t30 = q2*vd*2.0f; 1000 | float t22 = t20+t21-t30; 1001 | float t23 = q0*q0; 1002 | float t24 = q1*q1; 1003 | float t25 = q2*q2; 1004 | float t26 = q3*q3; 1005 | float t27 = t23-t24+t25-t26; 1006 | float t31 = P_[1][1]*t2*t15; 1007 | float t32 = P_[6][0]*t2*t10; 1008 | float t33 = P_[1][0]*t2*t15; 1009 | float t34 = P_[2][0]*t2*t19; 1010 | float t35 = P_[5][0]*t2*t27; 1011 | float t79 = P_[4][0]*t2*t7; 1012 | float t80 = P_[3][0]*t2*t22; 1013 | float t36 = t12+t32+t33+t34+t35-t79-t80; 1014 | float t37 = t2*t5*t36; 1015 | float t38 = P_[6][1]*t2*t10; 1016 | float t39 = P_[0][1]*t2*t5; 1017 | float t40 = P_[2][1]*t2*t19; 1018 | float t41 = P_[5][1]*t2*t27; 1019 | float t81 = P_[4][1]*t2*t7; 1020 | float t82 = P_[3][1]*t2*t22; 1021 | float t42 = t31+t38+t39+t40+t41-t81-t82; 1022 | float t43 = t2*t15*t42; 1023 | float t44 = P_[6][2]*t2*t10; 1024 | float t45 = P_[0][2]*t2*t5; 1025 | float t46 = P_[1][2]*t2*t15; 1026 | float t47 = P_[2][2]*t2*t19; 1027 | float t48 = P_[5][2]*t2*t27; 1028 | float t83 = P_[4][2]*t2*t7; 1029 | float t84 = P_[3][2]*t2*t22; 1030 | float t49 = t44+t45+t46+t47+t48-t83-t84; 1031 | float t50 = t2*t19*t49; 1032 | float t51 = P_[6][3]*t2*t10; 1033 | float t52 = P_[0][3]*t2*t5; 1034 | float t53 = P_[1][3]*t2*t15; 1035 | float t54 = P_[2][3]*t2*t19; 1036 | float t55 = P_[5][3]*t2*t27; 1037 | float t85 = P_[4][3]*t2*t7; 1038 | float t86 = P_[3][3]*t2*t22; 1039 | float t56 = t51+t52+t53+t54+t55-t85-t86; 1040 | float t57 = P_[6][5]*t2*t10; 1041 | float t58 = P_[0][5]*t2*t5; 1042 | float t59 = P_[1][5]*t2*t15; 1043 | float t60 = P_[2][5]*t2*t19; 1044 | float t61 = P_[5][5]*t2*t27; 1045 | float t88 = P_[4][5]*t2*t7; 1046 | float t89 = P_[3][5]*t2*t22; 1047 | float t62 = t57+t58+t59+t60+t61-t88-t89; 1048 | float t63 = t2*t27*t62; 1049 | float t64 = P_[6][4]*t2*t10; 1050 | float t65 = P_[0][4]*t2*t5; 1051 | float t66 = P_[1][4]*t2*t15; 1052 | float t67 = P_[2][4]*t2*t19; 1053 | float t68 = P_[5][4]*t2*t27; 1054 | float t90 = P_[4][4]*t2*t7; 1055 | float t91 = P_[3][4]*t2*t22; 1056 | float t69 = t64+t65+t66+t67+t68-t90-t91; 1057 | float t70 = P_[6][6]*t2*t10; 1058 | float t71 = P_[0][6]*t2*t5; 1059 | float t72 = P_[1][6]*t2*t15; 1060 | float t73 = P_[2][6]*t2*t19; 1061 | float t74 = P_[5][6]*t2*t27; 1062 | float t93 = P_[4][6]*t2*t7; 1063 | float t94 = P_[3][6]*t2*t22; 1064 | float t75 = t70+t71+t72+t73+t74-t93-t94; 1065 | float t76 = t2*t10*t75; 1066 | float t87 = t2*t22*t56; 1067 | float t92 = t2*t7*t69; 1068 | float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; 1069 | float t78; 1070 | 1071 | // calculate innovation variance for X axis observation and protect against a badly conditioned calculation 1072 | if (t77 >= R_LOS) { 1073 | t78 = 1.0f / t77; 1074 | flow_innov_var_[0] = t77; 1075 | } else { 1076 | // we need to reinitialise the covariance matrix and abort this fusion step 1077 | initialiseCovariance(); 1078 | return; 1079 | } 1080 | 1081 | // calculate Kalman gains for X-axis observation 1082 | 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); 1083 | 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); 1084 | 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); 1085 | 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); 1086 | 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); 1087 | 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); 1088 | 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); 1089 | 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); 1090 | 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); 1091 | 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); 1092 | 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); 1093 | 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); 1094 | 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); 1095 | 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); 1096 | 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); 1097 | 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); 1098 | 1099 | // run innovation consistency checks 1100 | optflow_test_ratio[0] = sq(flow_innov_[0]) / (sq(max(flow_innov_gate_, 1.0f)) * flow_innov_var_[0]); 1101 | 1102 | } else if (obs_index == 1) { 1103 | 1104 | // calculate Y axis observation Jacobian 1105 | float t2 = 1.0f / range; 1106 | H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); 1107 | H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); 1108 | H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); 1109 | H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); 1110 | H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); 1111 | H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f); 1112 | H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f); 1113 | 1114 | // calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains 1115 | float t3 = q3*ve*2.0f; 1116 | float t4 = q0*vn*2.0f; 1117 | float t11 = q2*vd*2.0f; 1118 | float t5 = t3+t4-t11; 1119 | float t6 = q0*q3*2.0f; 1120 | float t7 = q1*q2*2.0f; 1121 | float t8 = t6+t7; 1122 | float t9 = q0*q2*2.0f; 1123 | float t28 = q1*q3*2.0f; 1124 | float t10 = t9-t28; 1125 | float t12 = P_[0][0]*t2*t5; 1126 | float t13 = q3*vd*2.0f; 1127 | float t14 = q2*ve*2.0f; 1128 | float t15 = q1*vn*2.0f; 1129 | float t16 = t13+t14+t15; 1130 | float t17 = q0*vd*2.0f; 1131 | float t18 = q2*vn*2.0f; 1132 | float t29 = q1*ve*2.0f; 1133 | float t19 = t17+t18-t29; 1134 | float t20 = q1*vd*2.0f; 1135 | float t21 = q0*ve*2.0f; 1136 | float t30 = q3*vn*2.0f; 1137 | float t22 = t20+t21-t30; 1138 | float t23 = q0*q0; 1139 | float t24 = q1*q1; 1140 | float t25 = q2*q2; 1141 | float t26 = q3*q3; 1142 | float t27 = t23+t24-t25-t26; 1143 | float t31 = P_[1][1]*t2*t16; 1144 | float t32 = P_[5][0]*t2*t8; 1145 | float t33 = P_[1][0]*t2*t16; 1146 | float t34 = P_[3][0]*t2*t22; 1147 | float t35 = P_[4][0]*t2*t27; 1148 | float t80 = P_[6][0]*t2*t10; 1149 | float t81 = P_[2][0]*t2*t19; 1150 | float t36 = t12+t32+t33+t34+t35-t80-t81; 1151 | float t37 = t2*t5*t36; 1152 | float t38 = P_[5][1]*t2*t8; 1153 | float t39 = P_[0][1]*t2*t5; 1154 | float t40 = P_[3][1]*t2*t22; 1155 | float t41 = P_[4][1]*t2*t27; 1156 | float t82 = P_[6][1]*t2*t10; 1157 | float t83 = P_[2][1]*t2*t19; 1158 | float t42 = t31+t38+t39+t40+t41-t82-t83; 1159 | float t43 = t2*t16*t42; 1160 | float t44 = P_[5][2]*t2*t8; 1161 | float t45 = P_[0][2]*t2*t5; 1162 | float t46 = P_[1][2]*t2*t16; 1163 | float t47 = P_[3][2]*t2*t22; 1164 | float t48 = P_[4][2]*t2*t27; 1165 | float t79 = P_[2][2]*t2*t19; 1166 | float t84 = P_[6][2]*t2*t10; 1167 | float t49 = t44+t45+t46+t47+t48-t79-t84; 1168 | float t50 = P_[5][3]*t2*t8; 1169 | float t51 = P_[0][3]*t2*t5; 1170 | float t52 = P_[1][3]*t2*t16; 1171 | float t53 = P_[3][3]*t2*t22; 1172 | float t54 = P_[4][3]*t2*t27; 1173 | float t86 = P_[6][3]*t2*t10; 1174 | float t87 = P_[2][3]*t2*t19; 1175 | float t55 = t50+t51+t52+t53+t54-t86-t87; 1176 | float t56 = t2*t22*t55; 1177 | float t57 = P_[5][4]*t2*t8; 1178 | float t58 = P_[0][4]*t2*t5; 1179 | float t59 = P_[1][4]*t2*t16; 1180 | float t60 = P_[3][4]*t2*t22; 1181 | float t61 = P_[4][4]*t2*t27; 1182 | float t88 = P_[6][4]*t2*t10; 1183 | float t89 = P_[2][4]*t2*t19; 1184 | float t62 = t57+t58+t59+t60+t61-t88-t89; 1185 | float t63 = t2*t27*t62; 1186 | float t64 = P_[5][5]*t2*t8; 1187 | float t65 = P_[0][5]*t2*t5; 1188 | float t66 = P_[1][5]*t2*t16; 1189 | float t67 = P_[3][5]*t2*t22; 1190 | float t68 = P_[4][5]*t2*t27; 1191 | float t90 = P_[6][5]*t2*t10; 1192 | float t91 = P_[2][5]*t2*t19; 1193 | float t69 = t64+t65+t66+t67+t68-t90-t91; 1194 | float t70 = t2*t8*t69; 1195 | float t71 = P_[5][6]*t2*t8; 1196 | float t72 = P_[0][6]*t2*t5; 1197 | float t73 = P_[1][6]*t2*t16; 1198 | float t74 = P_[3][6]*t2*t22; 1199 | float t75 = P_[4][6]*t2*t27; 1200 | float t92 = P_[6][6]*t2*t10; 1201 | float t93 = P_[2][6]*t2*t19; 1202 | float t76 = t71+t72+t73+t74+t75-t92-t93; 1203 | float t85 = t2*t19*t49; 1204 | float t94 = t2*t10*t76; 1205 | float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; 1206 | float t78; 1207 | 1208 | // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation 1209 | if (t77 >= R_LOS) { 1210 | t78 = 1.0f / t77; 1211 | flow_innov_var_[1] = t77; 1212 | } else { 1213 | // we need to reinitialise the covariance matrix and abort this fusion step 1214 | initialiseCovariance(); 1215 | return; 1216 | } 1217 | 1218 | // calculate Kalman gains for Y-axis observation 1219 | 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); 1220 | 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); 1221 | 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); 1222 | 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); 1223 | 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); 1224 | 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); 1225 | 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); 1226 | 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); 1227 | 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); 1228 | 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); 1229 | 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); 1230 | 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); 1231 | 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); 1232 | 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); 1233 | 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); 1234 | 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); 1235 | 1236 | // run innovation consistency check 1237 | optflow_test_ratio[1] = sq(flow_innov_[1]) / (sq(max(flow_innov_gate_, 1.0f)) * flow_innov_var_[1]); 1238 | } 1239 | } 1240 | 1241 | // record the innovation test pass/fail 1242 | bool flow_fail = false; 1243 | 1244 | for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { 1245 | if (optflow_test_ratio[obs_index] > 1.0f) { 1246 | flow_fail = true; 1247 | } 1248 | } 1249 | 1250 | // if either axis fails we abort the fusion 1251 | if (flow_fail) { 1252 | return; 1253 | } 1254 | 1255 | for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { 1256 | 1257 | // copy the Kalman gain vector for the axis we are fusing 1258 | float gain[k_num_states_]; 1259 | 1260 | for (unsigned row = 0; row <= k_num_states_ - 1; row++) { 1261 | gain[row] = Kfusion[row][obs_index]; 1262 | } 1263 | 1264 | // apply covariance correction via P_new = (I -K*H)*P_ 1265 | // first calculate expression for KHP 1266 | // then calculate P_ - KHP 1267 | float KHP[k_num_states_][k_num_states_]; 1268 | float KH[7]; 1269 | 1270 | for (unsigned row = 0; row < k_num_states_; row++) { 1271 | KH[0] = gain[row] * H_LOS[obs_index][0]; 1272 | KH[1] = gain[row] * H_LOS[obs_index][1]; 1273 | KH[2] = gain[row] * H_LOS[obs_index][2]; 1274 | KH[3] = gain[row] * H_LOS[obs_index][3]; 1275 | KH[4] = gain[row] * H_LOS[obs_index][4]; 1276 | KH[5] = gain[row] * H_LOS[obs_index][5]; 1277 | KH[6] = gain[row] * H_LOS[obs_index][6]; 1278 | 1279 | for (unsigned column = 0; column < k_num_states_; column++) { 1280 | float tmp = KH[0] * P_[0][column]; 1281 | tmp += KH[1] * P_[1][column]; 1282 | tmp += KH[2] * P_[2][column]; 1283 | tmp += KH[3] * P_[3][column]; 1284 | tmp += KH[4] * P_[4][column]; 1285 | tmp += KH[5] * P_[5][column]; 1286 | tmp += KH[6] * P_[6][column]; 1287 | KHP[row][column] = tmp; 1288 | } 1289 | } 1290 | 1291 | // if the covariance correction will result in a negative variance, then 1292 | // the covariance marix is unhealthy and must be corrected 1293 | bool healthy = true; 1294 | 1295 | for (int i = 0; i < k_num_states_; i++) { 1296 | if (P_[i][i] < KHP[i][i]) { 1297 | // zero rows and columns 1298 | zeroRows(P_, i, i); 1299 | zeroCols(P_, i, i); 1300 | 1301 | //flag as unhealthy 1302 | healthy = false; 1303 | } 1304 | } 1305 | 1306 | // only apply covariance and state corrrections if healthy 1307 | if (healthy) { 1308 | // apply the covariance corrections 1309 | for (unsigned row = 0; row < k_num_states_; row++) { 1310 | for (unsigned column = 0; column < k_num_states_; column++) { 1311 | P_[row][column] = P_[row][column] - KHP[row][column]; 1312 | } 1313 | } 1314 | 1315 | // correct the covariance marix for gross errors 1316 | fixCovarianceErrors(); 1317 | 1318 | // apply the state corrections 1319 | fuse(gain, flow_innov_[obs_index]); 1320 | } 1321 | } 1322 | } 1323 | 1324 | void ESKF::controlMagFusion() { 1325 | if(fusion_mask_ & MASK_MAG_INHIBIT) { 1326 | mag_use_inhibit_ = true; 1327 | fuseHeading(); 1328 | } else if(mag_data_ready_) { 1329 | // determine if we should use the yaw observation 1330 | if ((fusion_mask_ & MASK_MAG_HEADING) && (!mag_hdg_)) { 1331 | if (time_last_imu_ - time_last_mag_ < 2 * MAG_INTERVAL) { 1332 | mag_hdg_ = true; 1333 | printf("ESKF commencing mag yaw fusion\n"); 1334 | } 1335 | } 1336 | 1337 | if(mag_hdg_) { 1338 | fuseHeading(); 1339 | } 1340 | } 1341 | } 1342 | 1343 | void ESKF::controlExternalVisionFusion() { 1344 | if(vision_data_ready_) { 1345 | // Fuse available NED position data into the main filter 1346 | if ((fusion_mask_ & MASK_EV_POS) && (!ev_pos_)) { 1347 | // check for an external vision measurement that has fallen behind the fusion time horizon 1348 | if (time_last_imu_ - time_last_ext_vision_ < 2 * EV_MAX_INTERVAL) { 1349 | ev_pos_ = true; 1350 | printf("ESKF commencing external vision position fusion\n"); 1351 | } 1352 | // reset the position if we are not already aiding using GPS, else use a relative position method for fusing the position data 1353 | if (gps_pos_) { 1354 | // 1355 | } else { 1356 | resetPosition(); 1357 | resetVelocity(); 1358 | } 1359 | } 1360 | 1361 | // determine if we should use the yaw observation 1362 | if ((fusion_mask_ & MASK_EV_YAW) && (!ev_yaw_)) { 1363 | if (time_last_imu_ - time_last_ext_vision_ < 2 * EV_MAX_INTERVAL) { 1364 | // reset the yaw angle to the value from the observaton quaternion 1365 | vec3 euler_init = dcm2vec(quat2dcm(state_.quat_nominal)); 1366 | 1367 | // get initial yaw from the observation quaternion 1368 | const extVisionSample &ev_newest = ext_vision_buffer_.get_newest(); 1369 | vec3 euler_obs = dcm2vec(quat2dcm(ev_newest.quatNED)); 1370 | euler_init(2) = euler_obs(2); 1371 | 1372 | // calculate initial quaternion states for the ekf 1373 | state_.quat_nominal = from_axis_angle(euler_init); 1374 | 1375 | ev_yaw_ = true; 1376 | printf("ESKF commencing external vision yaw fusion\n"); 1377 | } 1378 | } 1379 | 1380 | // determine if we should use the hgt observation 1381 | if ((fusion_mask_ & MASK_EV_HGT) && (!ev_hgt_)) { 1382 | // don't start using EV data unless data is arriving frequently 1383 | if (time_last_imu_ - time_last_ext_vision_ < 2 * EV_MAX_INTERVAL) { 1384 | ev_hgt_ = true; 1385 | printf("ESKF commencing external vision hgt fusion\n"); 1386 | if(rng_hgt_) { 1387 | // 1388 | } else { 1389 | resetHeight(); 1390 | } 1391 | } 1392 | } 1393 | 1394 | if (ev_hgt_) { 1395 | fuse_height_ = true; 1396 | } 1397 | 1398 | if (ev_pos_) { 1399 | fuse_pos_ = true; 1400 | } 1401 | 1402 | if(fuse_height_ || fuse_pos_) { 1403 | fuseVelPosHeight(); 1404 | fuse_pos_ = fuse_height_ = false; 1405 | } 1406 | 1407 | if (ev_yaw_) { 1408 | fuseHeading(); 1409 | } 1410 | } 1411 | } 1412 | 1413 | void ESKF::controlGpsFusion() { 1414 | if (gps_data_ready_) { 1415 | if ((fusion_mask_ & MASK_GPS_POS) && (!gps_pos_)) { 1416 | gps_pos_ = true; 1417 | printf("ESKF commencing GPS pos fusion\n"); 1418 | } 1419 | if(gps_pos_) { 1420 | fuse_pos_ = true; 1421 | fuse_vert_vel_ = true; 1422 | fuse_hor_vel_ = true; 1423 | time_last_gps_ = time_last_imu_; 1424 | } 1425 | if ((fusion_mask_ & MASK_GPS_HGT) && (!gps_hgt_)) { 1426 | gps_hgt_ = true; 1427 | printf("ESKF commencing GPS hgt fusion\n"); 1428 | } 1429 | if(gps_pos_) { 1430 | fuse_height_ = true; 1431 | time_last_gps_ = time_last_imu_; 1432 | } 1433 | } 1434 | } 1435 | 1436 | void ESKF::controlVelPosFusion() { 1437 | if (!gps_pos_ && !opt_flow_ && !ev_pos_) { 1438 | // Fuse synthetic position observations every 200msec 1439 | if ((time_last_imu_ - time_last_fake_gps_ > (uint64_t)2e5) || fuse_height_) { 1440 | // Reset position and velocity states if we re-commence this aiding method 1441 | if ((time_last_imu_ - time_last_fake_gps_) > (uint64_t)4e5) { 1442 | resetPosition(); 1443 | resetVelocity(); 1444 | 1445 | if (time_last_fake_gps_ != 0) { 1446 | printf("ESKF stopping navigation\n"); 1447 | } 1448 | } 1449 | 1450 | fuse_pos_ = true; 1451 | fuse_hor_vel_ = false; 1452 | fuse_vert_vel_ = false; 1453 | time_last_fake_gps_ = time_last_imu_; 1454 | 1455 | vel_pos_innov_[0] = 0.0f; 1456 | vel_pos_innov_[1] = 0.0f; 1457 | vel_pos_innov_[2] = 0.0f; 1458 | vel_pos_innov_[3] = state_.pos(0) - last_known_posNED_(0); 1459 | vel_pos_innov_[4] = state_.pos(1) - last_known_posNED_(1); 1460 | 1461 | // glitch protection is not required so set gate to a large value 1462 | posInnovGateNE_ = 100.0f; 1463 | } 1464 | } 1465 | 1466 | // Fuse available NED velocity and position data into the main filter 1467 | if (fuse_height_ || fuse_pos_ || fuse_hor_vel_ || fuse_vert_vel_) { 1468 | fuseVelPosHeight(); 1469 | } 1470 | } 1471 | 1472 | void ESKF::controlHeightSensorTimeouts() { 1473 | /* 1474 | * Handle the case where we have not fused height measurements recently and 1475 | * uncertainty exceeds the max allowable. Reset using the best available height 1476 | * measurement source, continue using it after the reset and declare the current 1477 | * source failed if we have switched. 1478 | */ 1479 | 1480 | // check if height has been inertial deadreckoning for too long 1481 | bool hgt_fusion_timeout = ((time_last_imu_ - time_last_hgt_fuse_) > 5e6); 1482 | 1483 | // reset the vertical position and velocity states 1484 | if ((P_[9][9] > sq(hgt_reset_lim)) && (hgt_fusion_timeout)) { 1485 | // boolean that indicates we will do a height reset 1486 | bool reset_height = false; 1487 | 1488 | // handle the case where we are using external vision data for height 1489 | if (ev_hgt_) { 1490 | // check if vision data is available 1491 | extVisionSample ev_init = ext_vision_buffer_.get_newest(); 1492 | bool ev_data_available = ((time_last_imu_ - ev_init.time_us) < 2 * EV_MAX_INTERVAL); 1493 | 1494 | // reset to ev data if it is available 1495 | bool reset_to_ev = ev_data_available; 1496 | 1497 | if (reset_to_ev) { 1498 | // request a reset 1499 | reset_height = true; 1500 | } else { 1501 | // we have nothing to reset to 1502 | reset_height = false; 1503 | } 1504 | } else if (gps_hgt_) { 1505 | // check if gps data is available 1506 | gpsSample gps_init = gps_buffer_.get_newest(); 1507 | bool gps_data_available = ((time_last_imu_ - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); 1508 | 1509 | // reset to ev data if it is available 1510 | bool reset_to_gps = gps_data_available; 1511 | 1512 | if (reset_to_gps) { 1513 | // request a reset 1514 | reset_height = true; 1515 | } else { 1516 | // we have nothing to reset to 1517 | reset_height = false; 1518 | } 1519 | } 1520 | 1521 | // Reset vertical position and velocity states to the last measurement 1522 | if (reset_height) { 1523 | resetHeight(); 1524 | // Reset the timout timer 1525 | time_last_hgt_fuse_ = time_last_imu_; 1526 | } 1527 | } 1528 | } 1529 | 1530 | void ESKF::resetPosition() { 1531 | if (gps_pos_) { 1532 | // this reset is only called if we have new gps data at the fusion time horizon 1533 | state_.pos(0) = gps_sample_delayed_.pos(0); 1534 | state_.pos(1) = gps_sample_delayed_.pos(1); 1535 | 1536 | // use GPS accuracy to reset variances 1537 | setDiag(P_, 7, 8, sq(gps_sample_delayed_.hacc)); 1538 | 1539 | } else if (ev_pos_) { 1540 | // this reset is only called if we have new ev data at the fusion time horizon 1541 | state_.pos(0) = ev_sample_delayed_.posNED(0); 1542 | state_.pos(1) = ev_sample_delayed_.posNED(1); 1543 | 1544 | // use EV accuracy to reset variances 1545 | setDiag(P_, 7, 8, sq(ev_sample_delayed_.posErr)); 1546 | 1547 | } else if (opt_flow_) { 1548 | if (!in_air_) { 1549 | // we are likely starting OF for the first time so reset the horizontal position 1550 | state_.pos(0) = 0.0f; 1551 | state_.pos(1) = 0.0f; 1552 | } else { 1553 | // set to the last known position 1554 | state_.pos(0) = last_known_posNED_(0); 1555 | state_.pos(1) = last_known_posNED_(1); 1556 | } 1557 | // estimate is relative to initial positon in this mode, so we start with zero error. 1558 | zeroCols(P_, 7, 8); 1559 | zeroRows(P_, 7, 8); 1560 | } else { 1561 | // Used when falling back to non-aiding mode of operation 1562 | state_.pos(0) = last_known_posNED_(0); 1563 | state_.pos(1) = last_known_posNED_(1); 1564 | setDiag(P_, 7, 8, sq(pos_noaid_noise_)); 1565 | } 1566 | } 1567 | 1568 | void ESKF::resetVelocity() { 1569 | 1570 | } 1571 | 1572 | void ESKF::resetHeight() { 1573 | // reset the vertical position 1574 | if (ev_hgt_) { 1575 | // initialize vertical position with newest measurement 1576 | extVisionSample ev_newest = ext_vision_buffer_.get_newest(); 1577 | 1578 | // use the most recent data if it's time offset from the fusion time horizon is smaller 1579 | int32_t dt_newest = ev_newest.time_us - imu_sample_delayed_.time_us; 1580 | int32_t dt_delayed = ev_sample_delayed_.time_us - imu_sample_delayed_.time_us; 1581 | 1582 | if (std::abs(dt_newest) < std::abs(dt_delayed)) { 1583 | state_.pos(2) = ev_newest.posNED(2); 1584 | } else { 1585 | state_.pos(2) = ev_sample_delayed_.posNED(2); 1586 | } 1587 | } else if(gps_hgt_) { 1588 | // Get the most recent GPS data 1589 | const gpsSample &gps_newest = gps_buffer_.get_newest(); 1590 | if (time_last_imu_ - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) { 1591 | state_.pos(2) = gps_newest.hgt; 1592 | 1593 | // reset the associated covarince values 1594 | zeroRows(P_, 9, 9); 1595 | zeroCols(P_, 9, 9); 1596 | 1597 | // the state variance is the same as the observation 1598 | P_[9][9] = sq(gps_newest.hacc); 1599 | } 1600 | } 1601 | 1602 | // reset the vertical velocity covariance values 1603 | zeroRows(P_, 6, 6); 1604 | zeroCols(P_, 6, 6); 1605 | 1606 | // we don't know what the vertical velocity is, so set it to zero 1607 | state_.vel(2) = 0.0f; 1608 | 1609 | // Set the variance to a value large enough to allow the state to converge quickly 1610 | // that does not destabilise the filter 1611 | P_[6][6] = 10.0f; 1612 | } 1613 | 1614 | void ESKF::fuseVelPosHeight() { 1615 | bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available 1616 | bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [PN,PE,PD] observations 1617 | scalar_t R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD] 1618 | scalar_t gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations 1619 | scalar_t Kfusion[k_num_states_] = {}; // Kalman gain vector for any single observation - sequential fusion is used 1620 | 1621 | // calculate innovations, innovations gate sizes and observation variances 1622 | if (fuse_hor_vel_) { 1623 | // enable fusion for NE velocity axes 1624 | fuse_map[0] = fuse_map[1] = true; 1625 | velObsVarNE_(1) = velObsVarNE_(0) = sq(fmaxf(gps_sample_delayed_.sacc, gps_vel_noise_)); 1626 | 1627 | // Set observation noise variance and innovation consistency check gate size for the NE position observations 1628 | R[0] = velObsVarNE_(0); 1629 | R[1] = velObsVarNE_(1); 1630 | 1631 | hvelInnovGate_ = fmaxf(vel_innov_gate_, 1.0f); 1632 | 1633 | gate_size[1] = gate_size[0] = hvelInnovGate_; 1634 | } 1635 | 1636 | if (fuse_vert_vel_) { 1637 | fuse_map[2] = true; 1638 | // observation variance - use receiver reported accuracy with parameter setting the minimum value 1639 | R[2] = fmaxf(gps_vel_noise_, 0.01f); 1640 | // use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP 1641 | R[2] = 1.5f * fmaxf(R[2], gps_sample_delayed_.sacc); 1642 | R[2] = R[2] * R[2]; 1643 | // innovation gate size 1644 | gate_size[2] = fmaxf(vel_innov_gate_, 1.0f); 1645 | } 1646 | 1647 | if (fuse_pos_) { 1648 | fuse_map[3] = fuse_map[4] = true; 1649 | 1650 | if(gps_pos_) { 1651 | // calculate observation process noise 1652 | scalar_t lower_limit = fmaxf(gps_pos_noise_, 0.01f); 1653 | scalar_t upper_limit = fmaxf(pos_noaid_noise_, lower_limit); 1654 | posObsNoiseNE_ = constrain(gps_sample_delayed_.hacc, lower_limit, upper_limit); 1655 | velObsVarNE_(1) = velObsVarNE_(0) = sq(fmaxf(gps_sample_delayed_.sacc, gps_vel_noise_)); 1656 | 1657 | // calculate innovations 1658 | vel_pos_innov_[0] = state_.vel(0) - gps_sample_delayed_.vel(0); 1659 | vel_pos_innov_[1] = state_.vel(1) - gps_sample_delayed_.vel(1); 1660 | vel_pos_innov_[2] = state_.vel(2) - gps_sample_delayed_.vel(2); 1661 | vel_pos_innov_[3] = state_.pos(0) - gps_sample_delayed_.pos(0); 1662 | vel_pos_innov_[4] = state_.pos(1) - gps_sample_delayed_.pos(1); 1663 | 1664 | // observation 1-STD error 1665 | R[3] = sq(posObsNoiseNE_); 1666 | 1667 | // set innovation gate size 1668 | gate_size[3] = fmaxf(5.0, 1.0f); 1669 | 1670 | } else if (ev_pos_) { 1671 | // calculate innovations 1672 | // use the absolute position 1673 | vel_pos_innov_[3] = state_.pos(0) - ev_sample_delayed_.posNED(0); 1674 | vel_pos_innov_[4] = state_.pos(1) - ev_sample_delayed_.posNED(1); 1675 | 1676 | // observation 1-STD error 1677 | R[3] = fmaxf(0.05f, 0.01f); 1678 | 1679 | // innovation gate size 1680 | gate_size[3] = fmaxf(5.0f, 1.0f); 1681 | 1682 | } else { 1683 | // No observations - use a static position to constrain drift 1684 | if (in_air_) { 1685 | R[3] = fmaxf(10.0f, 0.5f); 1686 | } else { 1687 | R[3] = 0.5f; 1688 | } 1689 | vel_pos_innov_[3] = state_.pos(0) - last_known_posNED_(0); 1690 | vel_pos_innov_[4] = state_.pos(1) - last_known_posNED_(1); 1691 | 1692 | // glitch protection is not required so set gate to a large value 1693 | gate_size[3] = 100.0f; 1694 | 1695 | vel_pos_innov_[5] = state_.pos(2) - last_known_posNED_(2); 1696 | fuse_map[5] = true; 1697 | R[5] = 0.5f; 1698 | R[5] = R[5] * R[5]; 1699 | // innovation gate size 1700 | gate_size[5] = 100.0f; 1701 | } 1702 | 1703 | // convert North position noise to variance 1704 | R[3] = R[3] * R[3]; 1705 | 1706 | // copy North axis values to East axis 1707 | R[4] = R[3]; 1708 | gate_size[4] = gate_size[3]; 1709 | } 1710 | 1711 | if (fuse_height_) { 1712 | if(ev_hgt_) { 1713 | fuse_map[5] = true; 1714 | // calculate the innovation assuming the external vision observaton is in local NED frame 1715 | vel_pos_innov_[5] = state_.pos(2) - ev_sample_delayed_.posNED(2); 1716 | // observation variance - defined externally 1717 | R[5] = fmaxf(0.05f, 0.01f); 1718 | R[5] = R[5] * R[5]; 1719 | // innovation gate size 1720 | gate_size[5] = fmaxf(5.0f, 1.0f); 1721 | } else if(gps_hgt_) { 1722 | // vertical position innovation - gps measurement has opposite sign to earth z axis 1723 | vel_pos_innov_[5] = state_.pos(2) - gps_sample_delayed_.hgt; 1724 | // observation variance - receiver defined and parameter limited 1725 | // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP 1726 | scalar_t lower_limit = fmaxf(gps_pos_noise_, 0.01f); 1727 | scalar_t upper_limit = fmaxf(pos_noaid_noise_, lower_limit); 1728 | R[5] = 1.5f * constrain(gps_sample_delayed_.vacc, lower_limit, upper_limit); 1729 | R[5] = R[5] * R[5]; 1730 | // innovation gate size 1731 | gate_size[5] = fmaxf(5.0, 1.0f); 1732 | } else if ((rng_hgt_) && (R_rng_to_earth_2_2_ > range_cos_max_tilt_)) { 1733 | fuse_map[5] = true; 1734 | // use range finder with tilt correction 1735 | vel_pos_innov_[5] = state_.pos(2) - (-max(range_sample_delayed_.rng * R_rng_to_earth_2_2_, rng_gnd_clearance_)) - 0.1f; 1736 | // observation variance - user parameter defined 1737 | R[5] = fmaxf((sq(range_noise_) + sq(range_noise_scaler_ * range_sample_delayed_.rng)) * sq(R_rng_to_earth_2_2_), 0.01f); 1738 | // innovation gate size 1739 | gate_size[5] = fmaxf(range_innov_gate_, 1.0f); 1740 | } 1741 | } 1742 | 1743 | // calculate innovation test ratios 1744 | for (unsigned obs_index = 0; obs_index < 6; obs_index++) { 1745 | if (fuse_map[obs_index]) { 1746 | // compute the innovation variance SK = HPH + R 1747 | unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state 1748 | vel_pos_innov_var_[obs_index] = P_[state_index][state_index] + R[obs_index]; 1749 | // Compute the ratio of innovation to gate size 1750 | vel_pos_test_ratio_[obs_index] = sq(vel_pos_innov_[obs_index]) / (sq(gate_size[obs_index]) * vel_pos_innov_var_[obs_index]); 1751 | } 1752 | } 1753 | 1754 | // check position, velocity and height innovations 1755 | // treat 2D position and height as separate sensors 1756 | bool pos_check_pass = ((vel_pos_test_ratio_[3] <= 1.0f) && (vel_pos_test_ratio_[4] <= 1.0f)); 1757 | innov_check_pass_map[3] = innov_check_pass_map[4] = pos_check_pass; 1758 | innov_check_pass_map[5] = (vel_pos_test_ratio_[5] <= 1.0f); 1759 | 1760 | for (unsigned obs_index = 0; obs_index < 6; obs_index++) { 1761 | // skip fusion if not requested or checks have failed 1762 | if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) { 1763 | continue; 1764 | } 1765 | 1766 | unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state 1767 | 1768 | // calculate kalman gain K = PHS, where S = 1/innovation variance 1769 | for (int row = 0; row < k_num_states_; row++) { 1770 | Kfusion[row] = P_[row][state_index] / vel_pos_innov_var_[obs_index]; 1771 | } 1772 | 1773 | // update covarinace matrix via Pnew = (I - KH)P 1774 | float KHP[k_num_states_][k_num_states_]; 1775 | for (unsigned row = 0; row < k_num_states_; row++) { 1776 | for (unsigned column = 0; column < k_num_states_; column++) { 1777 | KHP[row][column] = Kfusion[row] * P_[state_index][column]; 1778 | } 1779 | } 1780 | 1781 | // if the covariance correction will result in a negative variance, then 1782 | // the covariance marix is unhealthy and must be corrected 1783 | bool healthy = true; 1784 | for (int i = 0; i < k_num_states_; i++) { 1785 | if (P_[i][i] < KHP[i][i]) { 1786 | // zero rows and columns 1787 | zeroRows(P_,i,i); 1788 | zeroCols(P_,i,i); 1789 | 1790 | //flag as unhealthy 1791 | healthy = false; 1792 | } 1793 | } 1794 | 1795 | // only apply covariance and state corrrections if healthy 1796 | if (healthy) { 1797 | // apply the covariance corrections 1798 | for (unsigned row = 0; row < k_num_states_; row++) { 1799 | for (unsigned column = 0; column < k_num_states_; column++) { 1800 | P_[row][column] = P_[row][column] - KHP[row][column]; 1801 | } 1802 | } 1803 | 1804 | // correct the covariance marix for gross errors 1805 | fixCovarianceErrors(); 1806 | 1807 | // apply the state corrections 1808 | fuse(Kfusion, vel_pos_innov_[obs_index]); 1809 | } 1810 | } 1811 | } 1812 | 1813 | void ESKF::updateVision(const quat& q, const vec3& p, uint64_t time_usec, scalar_t dt) { 1814 | // transform orientation from (ENU2FLU) to (NED2FRD): 1815 | quat q_nb = q_NED2ENU * q * q_FLU2FRD; 1816 | 1817 | // transform position from local ENU to local NED frame 1818 | vec3 pos_nb = q_NED2ENU.inverse().toRotationMatrix() * p; 1819 | 1820 | // limit data rate to prevent data being lost 1821 | if (time_usec - time_last_ext_vision_ > min_obs_interval_us_) { 1822 | extVisionSample ev_sample_new; 1823 | // calculate the system time-stamp for the mid point of the integration period 1824 | // copy required data 1825 | ev_sample_new.angErr = 0.05f; 1826 | ev_sample_new.posErr = 0.05f; 1827 | ev_sample_new.quatNED = q_nb; 1828 | ev_sample_new.posNED = pos_nb; 1829 | ev_sample_new.time_us = time_usec - ev_delay_ms_ * 1000; 1830 | time_last_ext_vision_ = time_usec; 1831 | // push to buffer 1832 | ext_vision_buffer_.push(ev_sample_new); 1833 | } 1834 | } 1835 | 1836 | void ESKF::updateGps(const vec3& v, const vec3& p, uint64_t time_us, scalar_t dt) { 1837 | // transform linear velocity from local ENU to body FRD frame 1838 | vec3 vel_nb = q_NED2ENU.inverse().toRotationMatrix() * v; 1839 | 1840 | // transform position from local ENU to local NED frame 1841 | vec3 pos_nb = q_NED2ENU.inverse().toRotationMatrix() * p; 1842 | 1843 | // check for arrival of new sensor data at the fusion time horizon 1844 | if (time_us - time_last_gps_ > min_obs_interval_us_) { 1845 | gpsSample gps_sample_new; 1846 | gps_sample_new.time_us = time_us - gps_delay_ms_ * 1000; 1847 | 1848 | gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; 1849 | time_last_gps_ = time_us; 1850 | 1851 | gps_sample_new.time_us = max(gps_sample_new.time_us, imu_sample_delayed_.time_us); 1852 | gps_sample_new.vel = vel_nb; 1853 | gps_sample_new.hacc = 1.0; 1854 | gps_sample_new.vacc = 1.0; 1855 | gps_sample_new.sacc = 0.0; 1856 | 1857 | gps_sample_new.pos(0) = pos_nb(0); 1858 | gps_sample_new.pos(1) = pos_nb(1); 1859 | gps_sample_new.hgt = pos_nb(2); 1860 | gps_buffer_.push(gps_sample_new); 1861 | } 1862 | } 1863 | 1864 | 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) { 1865 | // convert integrated flow and integrated gyro from flu to frd 1866 | ///< integrated flow in PX4 Body (FRD) coordinates 1867 | ///< integrated gyro in PX4 Body (FRD) coordinates 1868 | vec2 int_xy_b; 1869 | vec2 int_xy_gyro_b; 1870 | 1871 | int_xy_b(0) = int_xy(0); 1872 | int_xy_b(1) = -int_xy(1); 1873 | 1874 | int_xy_gyro_b = vec2(0,0); //replace embedded camera gyro to imu gyro 1875 | 1876 | // check for arrival of new sensor data at the fusion time horizon 1877 | if (time_us - time_last_opt_flow_ > min_obs_interval_us_) { 1878 | // 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 1879 | scalar_t delta_time = 1e-6f * (scalar_t)integration_time_us; 1880 | scalar_t delta_time_min = 5e-7f * (scalar_t)min_obs_interval_us_; 1881 | bool delta_time_good = delta_time >= delta_time_min; 1882 | 1883 | if (!delta_time_good) { 1884 | // protect against overflow casued by division with very small delta_time 1885 | delta_time = delta_time_min; 1886 | } 1887 | 1888 | // check magnitude is within sensor limits use this to prevent use of a saturated flow sensor when there are other aiding sources available 1889 | scalar_t flow_rate_magnitude; 1890 | bool flow_magnitude_good = true; 1891 | if (delta_time_good) { 1892 | flow_rate_magnitude = int_xy_b.norm() / delta_time; 1893 | flow_magnitude_good = (flow_rate_magnitude <= flow_max_rate_); 1894 | } 1895 | 1896 | bool relying_on_flow = opt_flow_ && !gps_pos_ && !ev_pos_; 1897 | 1898 | // check quality metric 1899 | bool flow_quality_good = (quality >= flow_quality_min_); 1900 | // Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling 1901 | // If flow quality fails checks on ground, assume zero flow rate after body rate compensation 1902 | if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !in_air_) { 1903 | optFlowSample opt_flow_sample_new; 1904 | opt_flow_sample_new.time_us = time_us - flow_delay_ms_ * 1000; 1905 | 1906 | // copy the quality metric returned by the PX4Flow sensor 1907 | opt_flow_sample_new.quality = quality; 1908 | 1909 | // 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. 1910 | // copy the optical and gyro measured delta angles 1911 | opt_flow_sample_new.gyroXY = - vec2(int_xy_gyro_b.x(), int_xy_gyro_b.y()); 1912 | opt_flow_sample_new.flowRadXY = - vec2(int_xy_b.x(), int_xy_b.y()); 1913 | 1914 | // convert integration interval to seconds 1915 | opt_flow_sample_new.dt = delta_time; 1916 | 1917 | time_last_opt_flow_ = time_us; 1918 | 1919 | opt_flow_buffer_.push(opt_flow_sample_new); 1920 | 1921 | rangeSample range_sample_new; 1922 | range_sample_new.rng = distance; 1923 | range_sample_new.time_us = time_us - range_delay_ms_ * 1000; 1924 | time_last_range_ = time_us; 1925 | 1926 | range_buffer_.push(range_sample_new); 1927 | } 1928 | } 1929 | } 1930 | 1931 | void ESKF::updateRangeFinder(scalar_t range, uint64_t time_us, scalar_t dt) { 1932 | // check for arrival of new range data at the fusion time horizon 1933 | if (time_us - time_last_range_ > min_obs_interval_us_) { 1934 | rangeSample range_sample_new; 1935 | range_sample_new.rng = range; 1936 | range_sample_new.time_us = time_us - range_delay_ms_ * 1000; 1937 | time_last_range_ = time_us; 1938 | 1939 | range_buffer_.push(range_sample_new); 1940 | } 1941 | } 1942 | 1943 | void ESKF::updateMagnetometer(const vec3& m, uint64_t time_usec, scalar_t dt) { 1944 | // convert FLU to FRD body frame IMU data 1945 | vec3 m_nb = q_FLU2FRD.toRotationMatrix() * m; 1946 | 1947 | // limit data rate to prevent data being lost 1948 | if ((time_usec - time_last_mag_) > min_obs_interval_us_) { 1949 | magSample mag_sample_new; 1950 | mag_sample_new.time_us = time_usec - mag_delay_ms_ * 1000; 1951 | 1952 | mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; 1953 | time_last_mag_ = time_usec; 1954 | mag_sample_new.mag = m_nb; 1955 | mag_buffer_.push(mag_sample_new); 1956 | } 1957 | } 1958 | 1959 | void ESKF::updateLandedState(uint8_t landed_state) { 1960 | in_air_ = landed_state; 1961 | } 1962 | 1963 | bool ESKF::initHagl() { 1964 | // get most recent range measurement from buffer 1965 | const rangeSample &latest_measurement = range_buffer_.get_newest(); 1966 | 1967 | if ((time_last_imu_ - latest_measurement.time_us) < (uint64_t)2e5 && R_rng_to_earth_2_2_ > range_cos_max_tilt_) { 1968 | // if we have a fresh measurement, use it to initialise the terrain estimator 1969 | terrain_vpos_ = state_.pos(2) + latest_measurement.rng * R_rng_to_earth_2_2_; 1970 | // initialise state variance to variance of measurement 1971 | terrain_var_ = sq(range_noise_); 1972 | // success 1973 | return true; 1974 | } else if (!in_air_) { 1975 | // if on ground we assume a ground clearance 1976 | terrain_vpos_ = state_.pos(2) + rng_gnd_clearance_; 1977 | // Use the ground clearance value as our uncertainty 1978 | terrain_var_ = rng_gnd_clearance_; 1979 | // ths is a guess 1980 | return false; 1981 | } else { 1982 | // no information - cannot initialise 1983 | return false; 1984 | } 1985 | } 1986 | 1987 | void ESKF::fuseHeading() { 1988 | // assign intermediate state variables 1989 | scalar_t q0 = state_.quat_nominal.w(); 1990 | scalar_t q1 = state_.quat_nominal.x(); 1991 | scalar_t q2 = state_.quat_nominal.y(); 1992 | scalar_t q3 = state_.quat_nominal.z(); 1993 | 1994 | scalar_t predicted_hdg, measured_hdg; 1995 | scalar_t H_YAW[4]; 1996 | vec3 mag_earth_pred; 1997 | 1998 | // determine if a 321 or 312 Euler sequence is best 1999 | if (fabsf(R_to_earth_(2, 0)) < fabsf(R_to_earth_(2, 1))) { 2000 | // calculate observation jacobian when we are observing the first rotation in a 321 sequence 2001 | scalar_t t9 = q0*q3; 2002 | scalar_t t10 = q1*q2; 2003 | scalar_t t2 = t9+t10; 2004 | scalar_t t3 = q0*q0; 2005 | scalar_t t4 = q1*q1; 2006 | scalar_t t5 = q2*q2; 2007 | scalar_t t6 = q3*q3; 2008 | scalar_t t7 = t3+t4-t5-t6; 2009 | scalar_t t8 = t7*t7; 2010 | if (t8 > 1e-6f) { 2011 | t8 = 1.0f/t8; 2012 | } else { 2013 | return; 2014 | } 2015 | scalar_t t11 = t2*t2; 2016 | scalar_t t12 = t8*t11*4.0f; 2017 | scalar_t t13 = t12+1.0f; 2018 | scalar_t t14; 2019 | if (fabsf(t13) > 1e-6f) { 2020 | t14 = 1.0f/t13; 2021 | } else { 2022 | return; 2023 | } 2024 | 2025 | H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; 2026 | H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; 2027 | H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; 2028 | H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; 2029 | 2030 | // rotate the magnetometer measurement into earth frame 2031 | vec3 euler321 = dcm2vec(quat2dcm(state_.quat_nominal)); 2032 | predicted_hdg = euler321(2); // we will need the predicted heading to calculate the innovation 2033 | 2034 | // calculate the observed yaw angle 2035 | if (mag_hdg_) { 2036 | // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle 2037 | euler321(2) = 0.0f; 2038 | mat3 R_to_earth = quat2dcm(from_euler(euler321)); 2039 | 2040 | // rotate the magnetometer measurements into earth frame using a zero yaw angle 2041 | if (mag_3D_) { 2042 | // don't apply bias corrections if we are learning them 2043 | mag_earth_pred = R_to_earth * mag_sample_delayed_.mag; 2044 | } else { 2045 | mag_earth_pred = R_to_earth * (mag_sample_delayed_.mag - state_.mag_B); 2046 | } 2047 | 2048 | // the angle of the projection onto the horizontal gives the yaw angle 2049 | measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + mag_declination_; 2050 | 2051 | } else if (ev_yaw_) { 2052 | 2053 | // calculate the yaw angle for a 321 sequence 2054 | // Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m 2055 | 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()); 2056 | 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()); 2057 | measured_hdg = atan2f(Tbn_1_0,Tbn_0_0); 2058 | 2059 | } else return; 2060 | } 2061 | else { 2062 | // calculate observaton jacobian when we are observing a rotation in a 312 sequence 2063 | scalar_t t9 = q0*q3; 2064 | scalar_t t10 = q1*q2; 2065 | scalar_t t2 = t9-t10; 2066 | scalar_t t3 = q0*q0; 2067 | scalar_t t4 = q1*q1; 2068 | scalar_t t5 = q2*q2; 2069 | scalar_t t6 = q3*q3; 2070 | scalar_t t7 = t3-t4+t5-t6; 2071 | scalar_t t8 = t7*t7; 2072 | if (t8 > 1e-6f) { 2073 | t8 = 1.0f/t8; 2074 | } else { 2075 | return; 2076 | } 2077 | scalar_t t11 = t2*t2; 2078 | scalar_t t12 = t8*t11*4.0f; 2079 | scalar_t t13 = t12+1.0f; 2080 | scalar_t t14; 2081 | if (fabsf(t13) > 1e-6f) { 2082 | t14 = 1.0f/t13; 2083 | } else { 2084 | return; 2085 | } 2086 | 2087 | H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f; 2088 | H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f; 2089 | H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f; 2090 | H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f; 2091 | 2092 | scalar_t yaw = atan2f(-R_to_earth_(0, 1), R_to_earth_(1, 1)); // first rotation (yaw) 2093 | scalar_t roll = asinf(R_to_earth_(2, 1)); // second rotation (roll) 2094 | scalar_t pitch = atan2f(-R_to_earth_(2, 0), R_to_earth_(2, 2)); // third rotation (pitch) 2095 | 2096 | predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation 2097 | 2098 | // calculate the observed yaw angle 2099 | if (mag_hdg_) { 2100 | // Set the first rotation (yaw) to zero and rotate the measurements into earth frame 2101 | yaw = 0.0f; 2102 | 2103 | // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence 2104 | // Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m 2105 | mat3 R_to_earth; 2106 | scalar_t sy = sinf(yaw); 2107 | scalar_t cy = cosf(yaw); 2108 | scalar_t sp = sinf(pitch); 2109 | scalar_t cp = cosf(pitch); 2110 | scalar_t sr = sinf(roll); 2111 | scalar_t cr = cosf(roll); 2112 | R_to_earth(0,0) = cy*cp-sy*sp*sr; 2113 | R_to_earth(0,1) = -sy*cr; 2114 | R_to_earth(0,2) = cy*sp+sy*cp*sr; 2115 | R_to_earth(1,0) = sy*cp+cy*sp*sr; 2116 | R_to_earth(1,1) = cy*cr; 2117 | R_to_earth(1,2) = sy*sp-cy*cp*sr; 2118 | R_to_earth(2,0) = -sp*cr; 2119 | R_to_earth(2,1) = sr; 2120 | R_to_earth(2,2) = cp*cr; 2121 | 2122 | // rotate the magnetometer measurements into earth frame using a zero yaw angle 2123 | if (mag_3D_) { 2124 | // don't apply bias corrections if we are learning them 2125 | mag_earth_pred = R_to_earth * mag_sample_delayed_.mag; 2126 | } else { 2127 | mag_earth_pred = R_to_earth * (mag_sample_delayed_.mag - state_.mag_B); 2128 | } 2129 | 2130 | // the angle of the projection onto the horizontal gives the yaw angle 2131 | measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + mag_declination_; 2132 | 2133 | } else if (ev_yaw_) { 2134 | // calculate the yaw angle for a 312 sequence 2135 | // Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m 2136 | 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()); 2137 | 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()); 2138 | measured_hdg = atan2f(Tbn_0_1_neg, Tbn_1_1); 2139 | 2140 | } else return; 2141 | } 2142 | 2143 | scalar_t R_YAW; // calculate observation variance 2144 | if (mag_hdg_) { 2145 | // using magnetic heading tuning parameter 2146 | R_YAW = sq(fmaxf(mag_heading_noise_, 1.0e-2f)); 2147 | } else if (ev_yaw_) 2148 | // using error estimate from external vision data 2149 | R_YAW = sq(fmaxf(ev_sample_delayed_.angErr, 1.0e-2f)); 2150 | else return; 2151 | 2152 | // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero 2153 | // calculate the innovaton variance 2154 | scalar_t PH[4]; 2155 | scalar_t heading_innov_var = R_YAW; 2156 | for (unsigned row = 0; row <= 3; row++) { 2157 | PH[row] = 0.0f; 2158 | for (uint8_t col = 0; col <= 3; col++) { 2159 | PH[row] += P_[row][col] * H_YAW[col]; 2160 | } 2161 | heading_innov_var += H_YAW[row] * PH[row]; 2162 | } 2163 | 2164 | scalar_t heading_innov_var_inv; 2165 | 2166 | // check if the innovation variance calculation is badly conditioned 2167 | if (heading_innov_var >= R_YAW) { 2168 | // the innovation variance contribution from the state covariances is not negative, no fault 2169 | heading_innov_var_inv = 1.0f / heading_innov_var; 2170 | } else { 2171 | // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned 2172 | // we reinitialise the covariance matrix and abort this fusion step 2173 | initialiseCovariance(); 2174 | return; 2175 | } 2176 | 2177 | // calculate the Kalman gains 2178 | // only calculate gains for states we are using 2179 | scalar_t Kfusion[k_num_states_] = {}; 2180 | 2181 | for (uint8_t row = 0; row < k_num_states_; row++) { 2182 | Kfusion[row] = 0.0f; 2183 | for (uint8_t col = 0; col <= 3; col++) { 2184 | Kfusion[row] += P_[row][col] * H_YAW[col]; 2185 | } 2186 | Kfusion[row] *= heading_innov_var_inv; 2187 | } 2188 | 2189 | // wrap the heading to the interval between +-pi 2190 | measured_hdg = wrap_pi(measured_hdg); 2191 | 2192 | if (mag_use_inhibit_) { 2193 | // The magnetomer cannot be trusted but we need to fuse a heading to prevent a badly conditoned covariance matrix developing over time. 2194 | if (!vehicle_at_rest_) { 2195 | // Vehicle is not at rest so fuse a zero innovation and record the predicted heading to use as an observation when movement ceases. 2196 | heading_innov_ = 0.0f; 2197 | vehicle_at_rest_prev_ = false; 2198 | } else { 2199 | // 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. 2200 | if (!vehicle_at_rest_prev_ || !mag_use_inhibit_prev_) { 2201 | last_static_yaw_ = predicted_hdg; 2202 | vehicle_at_rest_prev_ = true; 2203 | } 2204 | // calculate the innovation 2205 | heading_innov_ = predicted_hdg - last_static_yaw_; 2206 | R_YAW = 0.01f; 2207 | heading_innov_gate_ = 5.0f; 2208 | } 2209 | } else { 2210 | // calculate the innovation 2211 | heading_innov_ = predicted_hdg - measured_hdg; 2212 | last_static_yaw_ = predicted_hdg; 2213 | } 2214 | mag_use_inhibit_prev_ = mag_use_inhibit_; 2215 | 2216 | // wrap the innovation to the interval between +-pi 2217 | heading_innov_ = wrap_pi(heading_innov_); 2218 | 2219 | // innovation test ratio 2220 | scalar_t yaw_test_ratio = sq(heading_innov_) / (sq(heading_innov_gate_) * heading_innov_var); 2221 | 2222 | // set the vision yaw unhealthy if the test fails 2223 | if (yaw_test_ratio > 1.0f) { 2224 | if(in_air_) 2225 | return; 2226 | else { 2227 | // constrain the innovation to the maximum set by the gate 2228 | scalar_t gate_limit = sqrtf(sq(heading_innov_gate_) * heading_innov_var); 2229 | heading_innov_ = constrain(heading_innov_, -gate_limit, gate_limit); 2230 | } 2231 | } 2232 | 2233 | // apply covariance correction via P_new = (I -K*H)*P 2234 | // first calculate expression for KHP 2235 | // then calculate P - KHP 2236 | float KHP[k_num_states_][k_num_states_]; 2237 | float KH[4]; 2238 | for (unsigned row = 0; row < k_num_states_; row++) { 2239 | 2240 | KH[0] = Kfusion[row] * H_YAW[0]; 2241 | KH[1] = Kfusion[row] * H_YAW[1]; 2242 | KH[2] = Kfusion[row] * H_YAW[2]; 2243 | KH[3] = Kfusion[row] * H_YAW[3]; 2244 | 2245 | for (unsigned column = 0; column < k_num_states_; column++) { 2246 | float tmp = KH[0] * P_[0][column]; 2247 | tmp += KH[1] * P_[1][column]; 2248 | tmp += KH[2] * P_[2][column]; 2249 | tmp += KH[3] * P_[3][column]; 2250 | KHP[row][column] = tmp; 2251 | } 2252 | } 2253 | 2254 | // if the covariance correction will result in a negative variance, then 2255 | // the covariance marix is unhealthy and must be corrected 2256 | bool healthy = true; 2257 | for (int i = 0; i < k_num_states_; i++) { 2258 | if (P_[i][i] < KHP[i][i]) { 2259 | // zero rows and columns 2260 | zeroRows(P_,i,i); 2261 | zeroCols(P_,i,i); 2262 | 2263 | //flag as unhealthy 2264 | healthy = false; 2265 | } 2266 | } 2267 | 2268 | // only apply covariance and state corrrections if healthy 2269 | if (healthy) { 2270 | // apply the covariance corrections 2271 | for (unsigned row = 0; row < k_num_states_; row++) { 2272 | for (unsigned column = 0; column < k_num_states_; column++) { 2273 | P_[row][column] = P_[row][column] - KHP[row][column]; 2274 | } 2275 | } 2276 | 2277 | // correct the covariance marix for gross errors 2278 | fixCovarianceErrors(); 2279 | 2280 | // apply the state corrections 2281 | fuse(Kfusion, heading_innov_); 2282 | } 2283 | } 2284 | 2285 | void ESKF::fixCovarianceErrors() { 2286 | scalar_t P_lim[7] = {}; 2287 | P_lim[0] = 1.0f; // quaternion max var 2288 | P_lim[1] = 1e6f; // velocity max var 2289 | P_lim[2] = 1e6f; // positiion max var 2290 | P_lim[3] = 1.0f; // gyro bias max var 2291 | P_lim[4] = 1.0f; // delta velocity z bias max var 2292 | P_lim[5] = 1.0f; // earth mag field max var 2293 | P_lim[6] = 1.0f; // body mag field max var 2294 | 2295 | for (int i = 0; i <= 3; i++) { 2296 | // quaternion states 2297 | P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[0]); 2298 | } 2299 | 2300 | for (int i = 4; i <= 6; i++) { 2301 | // NED velocity states 2302 | P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[1]); 2303 | } 2304 | 2305 | for (int i = 7; i <= 9; i++) { 2306 | // NED position states 2307 | P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[2]); 2308 | } 2309 | 2310 | for (int i = 10; i <= 12; i++) { 2311 | // gyro bias states 2312 | P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[3]); 2313 | } 2314 | 2315 | // force symmetry on the quaternion, velocity, positon and gyro bias state covariances 2316 | makeSymmetrical(P_,0,12); 2317 | 2318 | // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum 2319 | const scalar_t minSafeStateVar = 1e-9f; 2320 | scalar_t maxStateVar = minSafeStateVar; 2321 | bool resetRequired = false; 2322 | 2323 | for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { 2324 | if (P_[stateIndex][stateIndex] > maxStateVar) { 2325 | maxStateVar = P_[stateIndex][stateIndex]; 2326 | } else if (P_[stateIndex][stateIndex] < minSafeStateVar) { 2327 | resetRequired = true; 2328 | } 2329 | } 2330 | 2331 | // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must 2332 | // not exceed 100 and the minimum variance must not fall below the target minimum 2333 | // Also limit variance to a maximum equivalent to a 0.1g uncertainty 2334 | const scalar_t minStateVarTarget = 5E-8f; 2335 | scalar_t minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); 2336 | 2337 | for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { 2338 | P_[stateIndex][stateIndex] = constrain(P_[stateIndex][stateIndex], minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * dt_ekf_avg_)); 2339 | } 2340 | 2341 | // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero 2342 | if (resetRequired) { 2343 | scalar_t delVelBiasVar[3]; 2344 | 2345 | // store all delta velocity bias variances 2346 | for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { 2347 | delVelBiasVar[stateIndex - 13] = P_[stateIndex][stateIndex]; 2348 | } 2349 | 2350 | // reset all delta velocity bias covariances 2351 | zeroCols(P_, 13, 15); 2352 | 2353 | // restore all delta velocity bias variances 2354 | for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { 2355 | P_[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13]; 2356 | } 2357 | } 2358 | 2359 | // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong 2360 | // calculate accel bias term aligned with the gravity vector 2361 | //scalar_t dVel_bias_lim = 0.9f * acc_bias_lim * dt_ekf_avg_; 2362 | scalar_t down_dvel_bias = 0.0f; 2363 | 2364 | for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { 2365 | down_dvel_bias += state_.accel_bias(axis_index) * R_to_earth_(2, axis_index); 2366 | } 2367 | 2368 | // check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation 2369 | //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); 2370 | 2371 | // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of 2372 | // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue 2373 | if (time_last_imu_ - time_acc_bias_check_ > (uint64_t)7e6) { 2374 | scalar_t varX = P_[13][13]; 2375 | scalar_t varY = P_[14][14]; 2376 | scalar_t varZ = P_[15][15]; 2377 | zeroRows(P_, 13, 15); 2378 | zeroCols(P_, 13, 15); 2379 | P_[13][13] = varX; 2380 | P_[14][14] = varY; 2381 | P_[15][15] = varZ; 2382 | //ECL_WARN("EKF invalid accel bias - resetting covariance"); 2383 | } else { 2384 | // ensure the covariance values are symmetrical 2385 | makeSymmetrical(P_, 13, 15); 2386 | } 2387 | 2388 | // magnetic field states 2389 | if (!mag_3D_) { 2390 | zeroRows(P_, 16, 21); 2391 | zeroCols(P_, 16, 21); 2392 | } else { 2393 | // constrain variances 2394 | for (int i = 16; i <= 18; i++) { 2395 | P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[5]); 2396 | } 2397 | 2398 | for (int i = 19; i <= 21; i++) { 2399 | P_[i][i] = constrain(P_[i][i], 0.0f, P_lim[6]); 2400 | } 2401 | 2402 | // force symmetry 2403 | makeSymmetrical(P_, 16, 21); 2404 | } 2405 | } 2406 | 2407 | // This function forces the covariance matrix to be symmetric 2408 | void ESKF::makeSymmetrical(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last) { 2409 | for (unsigned row = first; row <= last; row++) { 2410 | for (unsigned column = 0; column < row; column++) { 2411 | float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2; 2412 | cov_mat[row][column] = tmp; 2413 | cov_mat[column][row] = tmp; 2414 | } 2415 | } 2416 | } 2417 | 2418 | // fuse measurement 2419 | void ESKF::fuse(scalar_t *K, scalar_t innovation) { 2420 | state_.quat_nominal.w() = state_.quat_nominal.w() - K[0] * innovation; 2421 | state_.quat_nominal.x() = state_.quat_nominal.x() - K[1] * innovation; 2422 | state_.quat_nominal.y() = state_.quat_nominal.y() - K[2] * innovation; 2423 | state_.quat_nominal.z() = state_.quat_nominal.z() - K[3] * innovation; 2424 | 2425 | state_.quat_nominal.normalize(); 2426 | 2427 | for (unsigned i = 0; i < 3; i++) { 2428 | state_.vel(i) = state_.vel(i) - K[i + 4] * innovation; 2429 | } 2430 | 2431 | for (unsigned i = 0; i < 3; i++) { 2432 | state_.pos(i) = state_.pos(i) - K[i + 7] * innovation; 2433 | } 2434 | 2435 | for (unsigned i = 0; i < 3; i++) { 2436 | state_.gyro_bias(i) = state_.gyro_bias(i) - K[i + 10] * innovation; 2437 | } 2438 | 2439 | for (unsigned i = 0; i < 3; i++) { 2440 | state_.accel_bias(i) = state_.accel_bias(i) - K[i + 13] * innovation; 2441 | } 2442 | 2443 | for (unsigned i = 0; i < 3; i++) { 2444 | state_.mag_I(i) = state_.mag_I(i) - K[i + 16] * innovation; 2445 | } 2446 | 2447 | for (unsigned i = 0; i < 3; i++) { 2448 | state_.mag_B(i) = state_.mag_B(i) - K[i + 19] * innovation; 2449 | } 2450 | } 2451 | 2452 | quat ESKF::getQuat() { 2453 | // transform orientation from (NED2FRD) to (ENU2FLU) 2454 | return q_NED2ENU.conjugate() * state_.quat_nominal * q_FLU2FRD.conjugate(); 2455 | } 2456 | 2457 | vec3 ESKF::getPosition() { 2458 | // transform position from local NED to local ENU frame 2459 | return q_NED2ENU.toRotationMatrix() * state_.pos; 2460 | } 2461 | 2462 | vec3 ESKF::getVelocity() { 2463 | // transform velocity from local NED to local ENU frame 2464 | return q_NED2ENU.toRotationMatrix() * state_.vel; 2465 | } 2466 | 2467 | // initialise the quaternion covariances using rotation vector variances 2468 | void ESKF::initialiseQuatCovariances(const vec3& rot_vec_var) { 2469 | // calculate an equivalent rotation vector from the quaternion 2470 | scalar_t q0,q1,q2,q3; 2471 | if (state_.quat_nominal.w() >= 0.0f) { 2472 | q0 = state_.quat_nominal.w(); 2473 | q1 = state_.quat_nominal.x(); 2474 | q2 = state_.quat_nominal.y(); 2475 | q3 = state_.quat_nominal.z(); 2476 | } else { 2477 | q0 = -state_.quat_nominal.w(); 2478 | q1 = -state_.quat_nominal.x(); 2479 | q2 = -state_.quat_nominal.y(); 2480 | q3 = -state_.quat_nominal.z(); 2481 | } 2482 | scalar_t delta = 2.0f*acosf(q0); 2483 | scalar_t scaler = (delta/sinf(delta*0.5f)); 2484 | scalar_t rotX = scaler*q1; 2485 | scalar_t rotY = scaler*q2; 2486 | scalar_t rotZ = scaler*q3; 2487 | 2488 | // autocode generated using matlab symbolic toolbox 2489 | scalar_t t2 = rotX*rotX; 2490 | scalar_t t4 = rotY*rotY; 2491 | scalar_t t5 = rotZ*rotZ; 2492 | scalar_t t6 = t2+t4+t5; 2493 | if (t6 > 1e-9f) { 2494 | scalar_t t7 = sqrtf(t6); 2495 | scalar_t t8 = t7*0.5f; 2496 | scalar_t t3 = sinf(t8); 2497 | scalar_t t9 = t3*t3; 2498 | scalar_t t10 = 1.0f/t6; 2499 | scalar_t t11 = 1.0f/sqrtf(t6); 2500 | scalar_t t12 = cosf(t8); 2501 | scalar_t t13 = 1.0f/powf(t6,1.5f); 2502 | scalar_t t14 = t3*t11; 2503 | scalar_t t15 = rotX*rotY*t3*t13; 2504 | scalar_t t16 = rotX*rotZ*t3*t13; 2505 | scalar_t t17 = rotY*rotZ*t3*t13; 2506 | scalar_t t18 = t2*t10*t12*0.5f; 2507 | scalar_t t27 = t2*t3*t13; 2508 | scalar_t t19 = t14+t18-t27; 2509 | scalar_t t23 = rotX*rotY*t10*t12*0.5f; 2510 | scalar_t t28 = t15-t23; 2511 | scalar_t t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f; 2512 | scalar_t t25 = rotX*rotZ*t10*t12*0.5f; 2513 | scalar_t t31 = t16-t25; 2514 | scalar_t t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f; 2515 | scalar_t t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f; 2516 | scalar_t t24 = t15-t23; 2517 | scalar_t t26 = t16-t25; 2518 | scalar_t t29 = t4*t10*t12*0.5f; 2519 | scalar_t t34 = t3*t4*t13; 2520 | scalar_t t30 = t14+t29-t34; 2521 | scalar_t t32 = t5*t10*t12*0.5f; 2522 | scalar_t t40 = t3*t5*t13; 2523 | scalar_t t33 = t14+t32-t40; 2524 | scalar_t t36 = rotY*rotZ*t10*t12*0.5f; 2525 | scalar_t t39 = t17-t36; 2526 | scalar_t t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f; 2527 | scalar_t t37 = t15-t23; 2528 | scalar_t t38 = t17-t36; 2529 | scalar_t t41 = rot_vec_var(0)*(t15-t23)*(t16-t25); 2530 | scalar_t t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39; 2531 | scalar_t t43 = t16-t25; 2532 | scalar_t t44 = t17-t36; 2533 | 2534 | // zero all the quaternion covariances 2535 | zeroRows(P_,0,3); 2536 | zeroCols(P_,0,3); 2537 | 2538 | // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox 2539 | 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; 2540 | P_[0][1] = t22; 2541 | 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; 2542 | 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; 2543 | P_[1][0] = t22; 2544 | P_[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26); 2545 | 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; 2546 | 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; 2547 | 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; 2548 | P_[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; 2549 | P_[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38); 2550 | P_[2][3] = t42; 2551 | 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; 2552 | P_[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; 2553 | P_[3][2] = t42; 2554 | P_[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44); 2555 | } else { 2556 | // the equations are badly conditioned so use a small angle approximation 2557 | P_[0][0] = 0.0f; 2558 | P_[0][1] = 0.0f; 2559 | P_[0][2] = 0.0f; 2560 | P_[0][3] = 0.0f; 2561 | P_[1][0] = 0.0f; 2562 | P_[1][1] = 0.25f * rot_vec_var(0); 2563 | P_[1][2] = 0.0f; 2564 | P_[1][3] = 0.0f; 2565 | P_[2][0] = 0.0f; 2566 | P_[2][1] = 0.0f; 2567 | P_[2][2] = 0.25f * rot_vec_var(1); 2568 | P_[2][3] = 0.0f; 2569 | P_[3][0] = 0.0f; 2570 | P_[3][1] = 0.0f; 2571 | P_[3][2] = 0.0f; 2572 | P_[3][3] = 0.25f * rot_vec_var(2); 2573 | } 2574 | } 2575 | 2576 | // zero specified range of rows in the state covariance matrix 2577 | void ESKF::zeroRows(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last) { 2578 | uint8_t row; 2579 | for (row = first; row <= last; row++) { 2580 | memset(&cov_mat[row][0], 0, sizeof(cov_mat[0][0]) * k_num_states_); 2581 | } 2582 | } 2583 | 2584 | // zero specified range of columns in the state covariance matrix 2585 | void ESKF::zeroCols(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last) { 2586 | uint8_t row; 2587 | for (row = 0; row <= k_num_states_-1; row++) { 2588 | memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first)); 2589 | } 2590 | } 2591 | 2592 | void ESKF::setDiag(scalar_t (&cov_mat)[k_num_states_][k_num_states_], uint8_t first, uint8_t last, scalar_t variance) { 2593 | // zero rows and columns 2594 | zeroRows(cov_mat, first, last); 2595 | zeroCols(cov_mat, first, last); 2596 | 2597 | // set diagonals 2598 | for (uint8_t row = first; row <= last; row++) { 2599 | cov_mat[row][row] = variance; 2600 | } 2601 | } 2602 | 2603 | void ESKF::constrainStates() { 2604 | state_.quat_nominal.w() = constrain(state_.quat_nominal.w(), -1.0f, 1.0f); 2605 | state_.quat_nominal.x() = constrain(state_.quat_nominal.x(), -1.0f, 1.0f); 2606 | state_.quat_nominal.y() = constrain(state_.quat_nominal.y(), -1.0f, 1.0f); 2607 | state_.quat_nominal.z() = constrain(state_.quat_nominal.z(), -1.0f, 1.0f); 2608 | 2609 | for (int i = 0; i < 3; i++) { 2610 | state_.vel(i) = constrain(state_.vel(i), -1000.0f, 1000.0f); 2611 | } 2612 | 2613 | for (int i = 0; i < 3; i++) { 2614 | state_.pos(i) = constrain(state_.pos(i), -1.e6f, 1.e6f); 2615 | } 2616 | 2617 | for (int i = 0; i < 3; i++) { 2618 | state_.gyro_bias(i) = constrain(state_.gyro_bias(i), -0.349066f * dt_ekf_avg_, 0.349066f * dt_ekf_avg_); 2619 | } 2620 | 2621 | for (int i = 0; i < 3; i++) { 2622 | state_.accel_bias(i) = constrain(state_.accel_bias(i), -acc_bias_lim * dt_ekf_avg_, acc_bias_lim * dt_ekf_avg_); 2623 | } 2624 | 2625 | for (int i = 0; i < 3; i++) { 2626 | state_.mag_I(i) = constrain(state_.mag_I(i), -1.0f, 1.0f); 2627 | } 2628 | 2629 | for (int i = 0; i < 3; i++) { 2630 | state_.mag_B(i) = constrain(state_.mag_B(i), -0.5f, 0.5f); 2631 | } 2632 | } 2633 | 2634 | void ESKF::setFusionMask(int fusion_mask) { 2635 | fusion_mask_ = fusion_mask; 2636 | } 2637 | } // namespace eskf 2638 | -------------------------------------------------------------------------------- /src/Node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace eskf 4 | { 5 | 6 | Node::Node(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) : nh_(pnh), init_(false) { 7 | ROS_INFO("Subscribing to imu."); 8 | subImu_ = nh_.subscribe("imu", 1000, &Node::inputCallback, this, ros::TransportHints().tcpNoDelay(true)); 9 | 10 | ROS_INFO("Subscribing to extended state"); 11 | subExtendedState_ = nh_.subscribe("extended_state", 1, &Node::extendedStateCallback, this); 12 | 13 | int fusion_mask = default_fusion_mask_; 14 | ros::param::get("~fusion_mask", fusion_mask); 15 | 16 | if((fusion_mask & MASK_EV_POS) || (fusion_mask & MASK_EV_YAW) || (fusion_mask & MASK_EV_HGT)) { 17 | ROS_INFO("Subscribing to vision"); 18 | subVisionPose_ = nh_.subscribe("vision", 1, &Node::visionCallback, this); 19 | } 20 | if((fusion_mask & MASK_GPS_POS) || (fusion_mask & MASK_GPS_VEL) || (fusion_mask & MASK_GPS_HGT)) { 21 | ROS_INFO("Subscribing to gps"); 22 | subGpsPose_ = nh_.subscribe("gps", 1, &Node::gpsCallback, this); 23 | } 24 | if(fusion_mask & MASK_OPTICAL_FLOW) { 25 | ROS_INFO("Subscribing to optical_flow"); 26 | subOpticalFlowPose_ = nh_.subscribe("optical_flow", 1, &Node::opticalFlowCallback, this); 27 | } 28 | if(fusion_mask & MASK_MAG_HEADING) { 29 | ROS_INFO("Subscribing to mag"); 30 | subMagPose_ = nh_.subscribe("mag", 1, &Node::magCallback, this); 31 | } 32 | if(fusion_mask & MASK_RANGEFINDER) { 33 | ROS_INFO("Subscribing to rangefinder"); 34 | subRangeFinderPose_ = nh_.subscribe("rangefinder", 1, &Node::rangeFinderCallback, this); 35 | } 36 | 37 | eskf_.setFusionMask(fusion_mask); 38 | 39 | pubPose_ = nh_.advertise("pose", 1); 40 | 41 | int publish_rate = default_publish_rate_; 42 | ros::param::get("~publish_rate", publish_rate); 43 | pubTimer_ = nh_.createTimer(ros::Duration(1.0f/publish_rate), &Node::publishState, this); 44 | } 45 | 46 | void Node::inputCallback(const sensor_msgs::ImuConstPtr& imuMsg) { 47 | 48 | vec3 wm = vec3(imuMsg->angular_velocity.x, imuMsg->angular_velocity.y, imuMsg->angular_velocity.z); // measured angular rate 49 | vec3 am = vec3(imuMsg->linear_acceleration.x, imuMsg->linear_acceleration.y, imuMsg->linear_acceleration.z); // measured linear acceleration 50 | 51 | if (prevStampImu_.sec != 0) { 52 | const double delta = (imuMsg->header.stamp - prevStampImu_).toSec(); 53 | 54 | if (!init_) { 55 | init_ = true; 56 | ROS_INFO("Initialized ESKF"); 57 | } 58 | 59 | // run kalman filter 60 | eskf_.run(wm, am, static_cast(imuMsg->header.stamp.toSec()*1e6f), delta); 61 | } 62 | prevStampImu_ = imuMsg->header.stamp; 63 | } 64 | 65 | void Node::visionCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& poseMsg) { 66 | if(prevStampVisionPose_.sec != 0) { 67 | const double delta = (poseMsg->header.stamp - prevStampVisionPose_).toSec(); 68 | // get measurements 69 | quat z_q = quat(poseMsg->pose.pose.orientation.w, poseMsg->pose.pose.orientation.x, poseMsg->pose.pose.orientation.y, poseMsg->pose.pose.orientation.z); 70 | vec3 z_p = vec3(poseMsg->pose.pose.position.x, poseMsg->pose.pose.position.y, poseMsg->pose.pose.position.z); 71 | // update vision 72 | eskf_.updateVision(z_q, z_p, static_cast(poseMsg->header.stamp.toSec()*1e6f), delta); 73 | } 74 | prevStampVisionPose_ = poseMsg->header.stamp; 75 | } 76 | 77 | void Node::gpsCallback(const nav_msgs::OdometryConstPtr& odomMsg) { 78 | if (prevStampGpsPose_.sec != 0) { 79 | const double delta = (odomMsg->header.stamp - prevStampGpsPose_).toSec(); 80 | // get gps measurements 81 | vec3 z_v = vec3(odomMsg->twist.twist.linear.x, odomMsg->twist.twist.linear.y, odomMsg->twist.twist.linear.z); 82 | vec3 z_p = vec3(odomMsg->pose.pose.position.x, odomMsg->pose.pose.position.y, odomMsg->pose.pose.position.z); 83 | // update gps 84 | eskf_.updateGps(z_v, z_p, static_cast(odomMsg->header.stamp.toSec() * 1e6f), delta); 85 | } 86 | prevStampGpsPose_ = odomMsg->header.stamp; 87 | } 88 | 89 | void Node::opticalFlowCallback(const mavros_msgs::OpticalFlowRadConstPtr& opticalFlowMsg) { 90 | if (prevStampOpticalFlowPose_.sec != 0) { 91 | const double delta = (opticalFlowMsg->header.stamp - prevStampOpticalFlowPose_).toSec(); 92 | // get optical flow measurements 93 | vec2 int_xy = vec2(opticalFlowMsg->integrated_x, opticalFlowMsg->integrated_y); 94 | vec2 int_xy_gyro = vec2(opticalFlowMsg->integrated_xgyro, opticalFlowMsg->integrated_ygyro); 95 | uint32_t integration_time = opticalFlowMsg->integration_time_us; 96 | scalar_t distance = opticalFlowMsg->distance; 97 | uint8_t quality = opticalFlowMsg->quality; 98 | // update optical flow 99 | eskf_.updateOpticalFlow(int_xy, int_xy_gyro, integration_time, distance, quality, static_cast(opticalFlowMsg->header.stamp.toSec() * 1e6f), delta); 100 | } 101 | prevStampOpticalFlowPose_ = opticalFlowMsg->header.stamp; 102 | } 103 | 104 | void Node::magCallback(const sensor_msgs::MagneticFieldConstPtr& magMsg) { 105 | if (prevStampMagPose_.sec != 0) { 106 | const double delta = (magMsg->header.stamp - prevStampMagPose_).toSec(); 107 | // get mag measurements 108 | vec3 m = vec3(magMsg->magnetic_field.x * 1e4f, magMsg->magnetic_field.y * 1e4f, magMsg->magnetic_field.z * 1e4f); 109 | eskf_.updateMagnetometer(m, static_cast(magMsg->header.stamp.toSec() * 1e6f), delta); 110 | } 111 | prevStampMagPose_ = magMsg->header.stamp; 112 | } 113 | 114 | void Node::rangeFinderCallback(const sensor_msgs::RangeConstPtr& rangeMsg) { 115 | if (prevStampRangeFinderPose_.sec != 0) { 116 | const double delta = (rangeMsg->header.stamp - prevStampRangeFinderPose_).toSec(); 117 | // get rangefinder measurements 118 | eskf_.updateRangeFinder(rangeMsg->range, static_cast(rangeMsg->header.stamp.toSec() * 1e6f), delta); 119 | } 120 | prevStampRangeFinderPose_ = rangeMsg->header.stamp; 121 | } 122 | 123 | void Node::extendedStateCallback(const mavros_msgs::ExtendedStateConstPtr& extendedStateMsg) { 124 | eskf_.updateLandedState(extendedStateMsg->landed_state & mavros_msgs::ExtendedState::LANDED_STATE_IN_AIR); 125 | } 126 | 127 | void Node::publishState(const ros::TimerEvent&) { 128 | 129 | // get kalman filter result 130 | const quat e2g = eskf_.getQuat(); 131 | const vec3 position = eskf_.getPosition(); 132 | const vec3 velocity = eskf_.getVelocity(); 133 | 134 | static size_t trace_id_ = 0; 135 | std_msgs::Header header; 136 | header.frame_id = "/pose"; 137 | header.seq = trace_id_++; 138 | header.stamp = ros::Time::now(); 139 | 140 | nav_msgs::Odometry odom; 141 | odom.header = header; 142 | odom.pose.pose.position.x = position[0]; 143 | odom.pose.pose.position.y = position[1]; 144 | odom.pose.pose.position.z = position[2]; 145 | odom.twist.twist.linear.x = velocity[0]; 146 | odom.twist.twist.linear.y = velocity[1]; 147 | odom.twist.twist.linear.z = velocity[2]; 148 | odom.pose.pose.orientation.w = e2g.w(); 149 | odom.pose.pose.orientation.x = e2g.x(); 150 | odom.pose.pose.orientation.y = e2g.y(); 151 | odom.pose.pose.orientation.z = e2g.z(); 152 | 153 | pubPose_.publish(odom); 154 | } 155 | 156 | } -------------------------------------------------------------------------------- /src/eskf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char *argv[]) 4 | { 5 | ros::init(argc, argv, "eskf"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle pnh("~"); 8 | eskf::Node node(nh, pnh); 9 | ros::spin(); 10 | return 0; 11 | } 12 | --------------------------------------------------------------------------------