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