From 1d1da12859bbebeb0a0ff6938b941cd4057f2574 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 26 Nov 2017 16:07:47 -0500
Subject: [PATCH] delete exampales ekf_att_pos_estimator
---
cmake/configs/nuttx_mindpx-v2_default.cmake | 3 -
cmake/configs/nuttx_nxphlite-v3_default.cmake | 3 -
cmake/configs/nuttx_px4fmu-v2_default.cmake | 3 -
cmake/configs/nuttx_px4fmu-v3_default.cmake | 3 -
cmake/configs/nuttx_px4fmu-v4_default.cmake | 3 -
.../configs/nuttx_px4fmu-v4pro_default.cmake | 3 -
cmake/configs/nuttx_px4fmu-v5_default.cmake | 3 -
cmake/configs/posix_sitl_default.cmake | 3 -
.../AttitudePositionEstimatorEKF.h | 369 --
.../ekf_att_pos_estimator/CMakeLists.txt | 47 -
.../ekf_att_pos_estimator_main.cpp | 1645 --------
.../ekf_att_pos_estimator_params.c | 307 --
.../estimator_22states.cpp | 3330 -----------------
.../estimator_22states.h | 424 ---
.../estimator_utilities.cpp | 235 --
.../estimator_utilities.h | 135 -
16 files changed, 6516 deletions(-)
delete mode 100644 src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
delete mode 100644 src/examples/ekf_att_pos_estimator/CMakeLists.txt
delete mode 100644 src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
delete mode 100644 src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
delete mode 100644 src/examples/ekf_att_pos_estimator/estimator_22states.cpp
delete mode 100644 src/examples/ekf_att_pos_estimator/estimator_22states.h
delete mode 100644 src/examples/ekf_att_pos_estimator/estimator_utilities.cpp
delete mode 100644 src/examples/ekf_att_pos_estimator/estimator_utilities.h
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 246efab752..60abf8332b 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -190,7 +190,4 @@ set(config_module_list
# Hardware test
#examples/hwtest
-
- # EKF
- #examples/ekf_att_pos_estimator
)
\ No newline at end of file
diff --git a/cmake/configs/nuttx_nxphlite-v3_default.cmake b/cmake/configs/nuttx_nxphlite-v3_default.cmake
index b40fc35ff6..cf7a6d6e16 100644
--- a/cmake/configs/nuttx_nxphlite-v3_default.cmake
+++ b/cmake/configs/nuttx_nxphlite-v3_default.cmake
@@ -206,7 +206,4 @@ set(config_module_list
# Hardware test
examples/hwtest
-
- # EKF
- examples/ekf_att_pos_estimator
)
\ No newline at end of file
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 7d567b5534..75cb0d4db3 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -205,7 +205,4 @@ set(config_module_list
# Hardware test
#examples/hwtest
-
- # EKF
- #examples/ekf_att_pos_estimator
)
diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake
index b04766e063..f3da02644d 100644
--- a/cmake/configs/nuttx_px4fmu-v3_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake
@@ -209,7 +209,4 @@ set(config_module_list
# Hardware test
examples/hwtest
-
- # EKF
- examples/ekf_att_pos_estimator
)
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 5b098a8c7e..cc1f2d67de 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -202,7 +202,4 @@ set(config_module_list
# Hardware test
examples/hwtest
-
- # EKF
- examples/ekf_att_pos_estimator
)
\ No newline at end of file
diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
index 7425609426..abb3c996ab 100644
--- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
@@ -201,7 +201,4 @@ set(config_module_list
# Hardware test
#examples/hwtest
-
- # EKF
- #examples/ekf_att_pos_estimator
)
\ No newline at end of file
diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake
index 43e18cf58c..449907b3d2 100644
--- a/cmake/configs/nuttx_px4fmu-v5_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake
@@ -202,7 +202,4 @@ set(config_module_list
# Hardware test
#examples/hwtest
-
- # EKF
- #examples/ekf_att_pos_estimator
)
\ No newline at end of file
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 96460b5ee8..4963329ea7 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -182,9 +182,6 @@ set(config_module_list
# Hardware test
#examples/hwtest
-
- # EKF
- examples/ekf_att_pos_estimator
)
# Default config_sitl_rcS_dir (posix_sitl_default), this is overwritten later
diff --git a/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
deleted file mode 100644
index ab997e08bf..0000000000
--- a/src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ /dev/null
@@ -1,369 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file AttitudePositionEstimatorEKF.h
- * Implementation of the attitude and position estimator. This is a PX4 wrapper around
- * the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp)
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Johan Jansen <jnsn.johan@gmail.com>
- */
-
-#pragma once
-
-#include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/wind_estimate.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/distance_sensor.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
-
-#include <geo/geo.h>
-#include <terrain_estimation/terrain_estimator.h>
-#include <systemlib/perf_counter.h>
-#include "estimator_22states.h"
-
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-
-//Forward declaration
-class AttPosEKF;
-
-class AttitudePositionEstimatorEKF : public control::SuperBlock
-{
-public:
- /**
- * Constructor
- */
- AttitudePositionEstimatorEKF();
-
- /* we do not want people ever copying this class */
- AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete;
- AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete;
-
- /**
- * Destructor, also kills the sensors task.
- */
- ~AttitudePositionEstimatorEKF();
-
- /**
- * Start the sensors task.
- *
- * @return OK on success.
- */
- int start();
-
- /**
- * Task status
- *
- * @return true if the mainloop is running
- */
- bool task_running() { return _task_running; }
-
- /**
- * Print the current status.
- */
- void print_status();
-
- /**
- * Trip the filter by feeding it NaN values.
- */
- int trip_nan();
-
- /**
- * Enable logging.
- *
- * @param enable Set to true to enable logging, false to disable
- */
- int enable_logging(bool enable);
-
- /**
- * Set debug level.
- *
- * @param debug Desired debug level - 0 to disable.
- */
- int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
-
- static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
-
-private:
- bool _task_should_exit; /**< if true, sensor task should exit */
- bool _task_running; /**< if true, task is running in its mainloop */
- int _estimator_task; /**< task handle for sensor task */
-
- int _sensor_combined_sub;
- int _distance_sub; /**< distance measurement */
- int _airspeed_sub; /**< airspeed subscription */
- int _baro_sub; /**< barometer subscription */
- int _gps_sub; /**< GPS subscription */
- int _vehicle_status_sub;
- int _vehicle_land_detected_sub;
- int _params_sub; /**< notification of parameter updates */
- int _manual_control_sub; /**< notification of manual control updates */
- int _home_sub; /**< home position as defined by commander / user */
- int _landDetectorSub;
- int _armedSub;
-
- orb_advert_t _att_pub; /**< vehicle attitude */
- orb_advert_t _global_pos_pub; /**< global position */
- orb_advert_t _local_pos_pub; /**< position in local frame */
- orb_advert_t _estimator_status_pub; /**< status of the estimator */
- orb_advert_t _wind_pub; /**< wind estimate */
-
- struct vehicle_attitude_s _att; /**< vehicle attitude */
- struct gyro_report _gyro;
- struct accel_report _accel;
- struct mag_report _mag;
- struct airspeed_s _airspeed; /**< airspeed */
- struct baro_report _baro; /**< baro readings */
- struct vehicle_status_s _vehicle_status;
- struct vehicle_land_detected_s _vehicle_land_detected;
- struct vehicle_global_position_s _global_pos; /**< global vehicle position */
- struct vehicle_local_position_s _local_pos; /**< local vehicle position */
- struct vehicle_gps_position_s _gps; /**< GPS position */
- struct wind_estimate_s _wind; /**< wind estimate */
- struct distance_sensor_s _distance; /**< distance estimate */
- struct actuator_armed_s _armed;
-
- hrt_abstime _last_accel;
- hrt_abstime _last_mag;
- unsigned _prediction_steps;
- uint64_t _prediction_last;
-
- struct sensor_combined_s _sensor_combined;
-
- struct map_projection_reference_s _pos_ref;
-
- float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
- float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
- hrt_abstime _last_debug_print = 0;
-
- perf_counter_t _loop_perf; ///< loop performance counter
- perf_counter_t _loop_intvl; ///< loop rate counter
- perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
- perf_counter_t _perf_mag; ///<local performance counter for mag updates
- perf_counter_t _perf_gps; ///<local performance counter for gps updates
- perf_counter_t _perf_baro; ///<local performance counter for baro updates
- perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
- perf_counter_t _perf_reset; ///<local performance counter for filter resets
-
- float _gps_alt_filt;
- float _baro_alt_filt;
- float _covariancePredictionDt; ///< time lapsed since last covariance prediction
- bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
- uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
- bool _baro_init;
- bool _gps_initialized;
- hrt_abstime _filter_start_time;
- hrt_abstime _last_sensor_timestamp;
- hrt_abstime _distance_last_valid;
- bool _data_good; ///< all required filter data is ok
- bool _ekf_logging; ///< log EKF state
- unsigned _debug; ///< debug level - default 0
- bool _was_landed; ///< indicates if was landed in previous iteration
-
- bool _newHgtData;
- bool _newAdsData;
- bool _newDataMag;
- bool _newRangeData;
-
- orb_advert_t _mavlink_log_pub;
-
- control::BlockParamFloat _mag_offset_x;
- control::BlockParamFloat _mag_offset_y;
- control::BlockParamFloat _mag_offset_z;
-
- struct {
- int32_t vel_delay_ms;
- int32_t pos_delay_ms;
- int32_t height_delay_ms;
- int32_t mag_delay_ms;
- int32_t tas_delay_ms;
- float velne_noise;
- float veld_noise;
- float posne_noise;
- float posd_noise;
- float mag_noise;
- float gyro_pnoise;
- float acc_pnoise;
- float gbias_pnoise;
- float abias_pnoise;
- float mage_pnoise;
- float magb_pnoise;
- float eas_noise;
- float pos_stddev_threshold;
- int32_t airspeed_disabled;
- } _parameters; /**< local copies of interesting parameters */
-
- struct {
- param_t vel_delay_ms;
- param_t pos_delay_ms;
- param_t height_delay_ms;
- param_t mag_delay_ms;
- param_t tas_delay_ms;
- param_t velne_noise;
- param_t veld_noise;
- param_t posne_noise;
- param_t posd_noise;
- param_t mag_noise;
- param_t gyro_pnoise;
- param_t acc_pnoise;
- param_t gbias_pnoise;
- param_t abias_pnoise;
- param_t mage_pnoise;
- param_t magb_pnoise;
- param_t eas_noise;
- param_t pos_stddev_threshold;
- param_t airspeed_disabled;
- } _parameter_handles; /**< handles for interesting parameters */
-
- AttPosEKF *_ekf;
-
- TerrainEstimator *_terrain_estimator;
-
- /* Low pass filter for attitude rates */
- math::LowPassFilter2p _LP_att_P;
- math::LowPassFilter2p _LP_att_Q;
- math::LowPassFilter2p _LP_att_R;
-
-private:
- /**
- * Update our local parameter cache.
- */
- int parameters_update();
-
- /**
- * Update control outputs
- *
- */
- void control_update();
-
- /**
- * Check for changes in land detected.
- */
- void vehicle_status_poll();
-
- /**
- * Check for changes in land detected.
- */
- void vehicle_land_detected_poll();
-
- /**
- * Shim for calling task_main from task_create.
- */
- static void task_main_trampoline(int argc, char *argv[]);
-
- /**
- * Main filter task.
- */
- void task_main();
-
- /**
- * Check filter sanity state
- *
- * @return zero if ok, non-zero for a filter error condition.
- */
- int check_filter_state();
-
- /**
- * @brief
- * Publish the euler and quaternions for attitude estimation
- **/
- void publishAttitude();
-
- /**
- * @brief
- * Publish local position relative to reference point where filter was initialized
- **/
- void publishLocalPosition();
-
- /**
- * @brief
- * Publish global position estimation (WSG84 and AMSL).
- * A global position estimate is only valid if we have a good GPS fix
- **/
- void publishGlobalPosition();
-
- /**
- * @brief
- * Publish wind estimates for north and east in m/s
- **/
- void publishWindEstimate();
-
- /**
- * @brief
- * Runs the sensor fusion step of the filter. The parameters determine which of the sensors
- * are fused with each other
- **/
- void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
- const bool fuseBaro, const bool fuseAirSpeed);
-
- /**
- * @brief
- * Initialize first time good GPS fix so we can get a reference point to calculate local position from
- * Should only be required to call once
- **/
- void initializeGPS();
-
- /**
- * Initialize the reference position for the local coordinate frame
- */
- void initReferencePosition(hrt_abstime timestamp, bool gps_valid,
- double lat, double lon, float gps_alt, float baro_alt);
-
- /**
- * @brief
- * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
- * flags to true (e.g newDataGps)
- **/
- void pollData();
-};
diff --git a/src/examples/ekf_att_pos_estimator/CMakeLists.txt b/src/examples/ekf_att_pos_estimator/CMakeLists.txt
deleted file mode 100644
index 5821274d2e..0000000000
--- a/src/examples/ekf_att_pos_estimator/CMakeLists.txt
+++ /dev/null
@@ -1,47 +0,0 @@
-############################################################################
-#
-# Copyright (c) 2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
- MODULE examples__ekf_att_pos_estimator
- MAIN ekf_att_pos_estimator
- COMPILE_FLAGS
- STACK_MAIN 3000
- STACK_MAX 3400
- SRCS
- ekf_att_pos_estimator_main.cpp
- estimator_22states.cpp
- estimator_utilities.cpp
- DEPENDS
- platforms__common
- )
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
deleted file mode 100644
index 24523db8e1..0000000000
--- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ /dev/null
@@ -1,1645 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ekf_att_pos_estimator_main.cpp
- * Implementation of the attitude and position estimator.
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Johan Jansen <jnsn.johan@gmail.com>
- */
-
-#include "AttitudePositionEstimatorEKF.h"
-#include "estimator_22states.h"
-
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <px4_tasks.h>
-#include <px4_posix.h>
-#include <px4_time.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <float.h>
-
-#include <arch/board/board.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/mavlink_log.h>
-#include <mathlib/mathlib.h>
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
-#include <platforms/px4_defines.h>
-#include <uORB/topics/parameter_update.h>
-
-static uint64_t IMUusec = 0;
-
-//Constants
-static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds
-static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds
-static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure
-
-// These are unused
-#if 0
-static constexpr unsigned MAG_SWITCH_HYSTERESIS =
- 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds)
-static constexpr unsigned GYRO_SWITCH_HYSTERESIS =
- 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds)
-static constexpr unsigned ACCEL_SWITCH_HYSTERESIS =
- 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds)
-#endif
-
-static constexpr float EPH_LARGE_VALUE = 1000.0f;
-static constexpr float EPV_LARGE_VALUE = 1000.0f;
-
-/**
- * estimator app start / stop handling function
- *
- * @ingroup apps
- */
-extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
-
-uint32_t millis();
-uint64_t getMicros();
-uint32_t getMillis();
-
-uint32_t millis()
-{
- return getMillis();
-}
-
-uint32_t getMillis()
-{
- return getMicros() / 1000;
-}
-
-uint64_t getMicros()
-{
- return IMUusec;
-}
-
-namespace estimator
-{
-
-AttitudePositionEstimatorEKF *g_estimator = nullptr;
-}
-
-AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
- SuperBlock(nullptr, "PE"),
- _task_should_exit(false),
- _task_running(false),
- _estimator_task(-1),
-
- /* subscriptions */
- _sensor_combined_sub(-1),
- _distance_sub(-1),
- _airspeed_sub(-1),
- _baro_sub(-1),
- _gps_sub(-1),
- _vehicle_status_sub(-1),
- _vehicle_land_detected_sub(-1),
- _params_sub(-1),
- _manual_control_sub(-1),
- _home_sub(-1),
- _armedSub(-1),
-
- /* publications */
- _att_pub(nullptr),
- _global_pos_pub(nullptr),
- _local_pos_pub(nullptr),
- _estimator_status_pub(nullptr),
- _wind_pub(nullptr),
-
- _att{},
- _gyro{},
- _accel{},
- _mag{},
- _airspeed{},
- _baro{},
- _vehicle_status{},
- _vehicle_land_detected{},
- _global_pos{},
- _local_pos{},
- _gps{},
- _wind{},
- _distance{},
- _armed{},
-
- _last_accel(0),
- _last_mag(0),
- _prediction_steps(0),
- _prediction_last(0),
-
- _sensor_combined{},
-
- _pos_ref{},
- _filter_ref_offset(0.0f),
- _baro_gps_offset(0.0f),
-
- /* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "ekf_dt")),
-#if 0
- _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
- _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
- _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
- _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
- _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
- _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-#else
- _loop_intvl(nullptr),
- _perf_gyro(nullptr),
- _perf_mag(nullptr),
- _perf_gps(nullptr),
- _perf_baro(nullptr),
- _perf_airspeed(nullptr),
-#endif
- _perf_reset(perf_alloc(PC_COUNT, "ekf_rst")),
-
- /* states */
- _gps_alt_filt(0.0f),
- _baro_alt_filt(0.0f),
- _covariancePredictionDt(0.0f),
- _gpsIsGood(false),
- _previousGPSTimestamp(0),
- _baro_init(false),
- _gps_initialized(false),
- _filter_start_time(0),
- _last_sensor_timestamp(hrt_absolute_time()),
- _distance_last_valid(0),
- _data_good(false),
- _ekf_logging(true),
- _debug(0),
- _was_landed(true),
-
- _newHgtData(false),
- _newAdsData(false),
- _newDataMag(false),
- _newRangeData(false),
- _mavlink_log_pub(nullptr),
-
- _mag_offset_x(this, "MAGB_X"),
- _mag_offset_y(this, "MAGB_Y"),
- _mag_offset_z(this, "MAGB_Z"),
- _parameters{},
- _parameter_handles{},
- _ekf(nullptr),
- _terrain_estimator(nullptr),
-
- _LP_att_P(250.0f, 20.0f),
- _LP_att_Q(250.0f, 20.0f),
- _LP_att_R(250.0f, 20.0f)
-{
- _terrain_estimator = new TerrainEstimator();
-
- _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
- _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
- _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
- _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS");
- _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS");
- _parameter_handles.velne_noise = param_find("PE_VELNE_NOISE");
- _parameter_handles.veld_noise = param_find("PE_VELD_NOISE");
- _parameter_handles.posne_noise = param_find("PE_POSNE_NOISE");
- _parameter_handles.posd_noise = param_find("PE_POSD_NOISE");
- _parameter_handles.mag_noise = param_find("PE_MAG_NOISE");
- _parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE");
- _parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE");
- _parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE");
- _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
- _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
- _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
- _parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
- _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
- _parameter_handles.airspeed_disabled = param_find("FW_AIRSPD_MODE");
-
- /* indicate consumers that the current position data is not valid */
- _gps.eph = 10000.0f;
- _gps.epv = 10000.0f;
-
- /* fetch initial parameter values */
- parameters_update();
-}
-
-AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
-{
- if (_estimator_task != -1) {
-
- /* task wakes up every 100ms or so at the longest */
- _task_should_exit = true;
-
- /* wait for a second for the task to quit at our request */
- unsigned i = 0;
-
- do {
- /* wait 20ms */
- usleep(20000);
-
- /* if we have given up, kill it */
- if (++i > 50) {
- px4_task_delete(_estimator_task);
- break;
- }
- } while (_estimator_task != -1);
- }
-
- delete _terrain_estimator;
- delete _ekf;
-
- estimator::g_estimator = nullptr;
-}
-
-int AttitudePositionEstimatorEKF::enable_logging(bool logging)
-{
- _ekf_logging = logging;
-
- return 0;
-}
-
-int AttitudePositionEstimatorEKF::parameters_update()
-{
- /* update C++ param system */
- updateParams();
-
- param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
- param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
- param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms));
- param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms));
- param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms));
- param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise));
- param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise));
- param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise));
- param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise));
- param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise));
- param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise));
- param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise));
- param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise));
- param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
- param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
- param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
- param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
- param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
- param_get(_parameter_handles.airspeed_disabled, &_parameters.airspeed_disabled);
-
- if (_ekf) {
- // _ekf->yawVarScale = 1.0f;
- // _ekf->windVelSigma = 0.1f;
- _ekf->dAngBiasSigma = _parameters.gbias_pnoise;
- _ekf->dVelBiasSigma = _parameters.abias_pnoise;
- _ekf->magEarthSigma = _parameters.mage_pnoise;
- _ekf->magBodySigma = _parameters.magb_pnoise;
- // _ekf->gndHgtSigma = 0.02f;
- _ekf->vneSigma = _parameters.velne_noise;
- _ekf->vdSigma = _parameters.veld_noise;
- _ekf->posNeSigma = _parameters.posne_noise;
- _ekf->posDSigma = _parameters.posd_noise;
- _ekf->magMeasurementSigma = _parameters.mag_noise;
- _ekf->gyroProcessNoise = _parameters.gyro_pnoise;
- _ekf->accelProcessNoise = _parameters.acc_pnoise;
- _ekf->airspeedMeasurementSigma = _parameters.eas_noise;
- _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
-#if 0
- // Initially disable loading until
- // convergence is flight-test proven
- _ekf->magBias.x = _mag_offset_x.get();
- _ekf->magBias.y = _mag_offset_y.get();
- _ekf->magBias.z = _mag_offset_z.get();
-#endif
- }
-
- return OK;
-}
-
-void AttitudePositionEstimatorEKF::vehicle_status_poll()
-{
- bool updated;
-
- orb_check(_vehicle_status_sub, &updated);
-
- if (updated) {
-
- orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
-
- // Tell EKF that the vehicle is a fixed wing or multi-rotor
- _ekf->setIsFixedWing(!_vehicle_status.is_rotary_wing);
- }
-}
-
-void AttitudePositionEstimatorEKF::vehicle_land_detected_poll()
-{
- bool updated;
-
- orb_check(_vehicle_land_detected_sub, &updated);
-
- if (updated) {
-
- orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
-
- // Save params on landed and previously not landed
- if (_vehicle_land_detected.landed && !_was_landed) {
- _mag_offset_x.set(_ekf->magBias.x);
- _mag_offset_x.commit();
- _mag_offset_y.set(_ekf->magBias.y);
- _mag_offset_y.commit();
- _mag_offset_z.set(_ekf->magBias.z);
- _mag_offset_z.commit();
- }
-
- _was_landed = _vehicle_land_detected.landed;
- }
-}
-
-int AttitudePositionEstimatorEKF::check_filter_state()
-{
- /*
- * CHECK IF THE INPUT DATA IS SANE
- */
-
- struct ekf_status_report ekf_report;
-
- int check = _ekf->CheckAndBound(&ekf_report);
-
- const char *const feedback[] = { nullptr,
- "NaN in states, resetting",
- "stale sensor data, resetting",
- "got initial position lock",
- "excessive gyro offsets",
- "velocity diverted, check accel config",
- "excessive covariances",
- "unknown condition, resetting"
- };
-
- // Print out error condition
- if (check) {
- unsigned warn_index = static_cast<unsigned>(check);
- unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
-
- if (max_warn_index < warn_index) {
- warn_index = max_warn_index;
- }
-
- // Do not warn about accel offset if we have no position updates
- if (!(warn_index == 5 && _ekf->staticMode)) {
- mavlink_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
- }
- }
-
- struct estimator_status_s rep;
-
- memset(&rep, 0, sizeof(rep));
-
- // If error flag is set, we got a filter reset
- if (check && ekf_report.error) {
-
- // Count the reset condition
- perf_count(_perf_reset);
- // GPS is in scaled integers, convert
- double lat = _gps.lat / 1.0e7;
- double lon = _gps.lon / 1.0e7;
- float gps_alt = _gps.alt / 1e3f;
-
- // Set up height correctly
- orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-
- initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude);
-
- } else if (_ekf_logging) {
- _ekf->GetFilterState(&ekf_report);
- }
-
- if (_ekf_logging || check) {
- rep.timestamp = hrt_absolute_time();
-
- rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0);
- rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1);
- rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2);
- rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3);
- rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4);
- rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5);
- rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6);
- rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7);
-
- rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0);
- rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
- rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
- rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
- rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
- rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
- rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
- rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
-
- rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
- rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
- rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2);
- rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3);
-
- if (_debug > 10) {
-
- if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
- PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s",
- ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
- ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
- ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
- ((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
- }
-
- if (rep.timeout_flags) {
- PX4_INFO("timeout: %s%s%s%s",
- ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
- ((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
- ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
- ((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
- }
- }
-
- // Copy all states or at least all that we can fit
- size_t ekf_n_states = ekf_report.n_states;
- size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
- rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
-
- // Copy diagonal elemnts of covariance matrix
- float covariances[28];
- _ekf->get_covariance(covariances);
-
- for (size_t i = 0; i < rep.n_states; i++) {
- rep.states[i] = ekf_report.states[i];
- rep.covariances[i] = covariances[i];
- }
-
-
-
- if (_estimator_status_pub != nullptr) {
- orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
-
- } else {
- _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
- }
- }
-
- return check;
-}
-
-void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[])
-{
- estimator::g_estimator->task_main();
-}
-
-void AttitudePositionEstimatorEKF::task_main()
-{
- _ekf = new AttPosEKF();
-
- if (!_ekf) {
- PX4_ERR("OUT OF MEM!");
- return;
- }
-
- _filter_start_time = hrt_absolute_time();
-
- /*
- * do subscriptions
- */
- _distance_sub = orb_subscribe(ORB_ID(distance_sensor));
- _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
- _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
- _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
- _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
- _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
- _params_sub = orb_subscribe(ORB_ID(parameter_update));
- _home_sub = orb_subscribe(ORB_ID(home_position));
- _armedSub = orb_subscribe(ORB_ID(actuator_armed));
-
- _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-
- /* sets also parameters in the EKF object */
- parameters_update();
-
- /* wakeup source(s) */
- px4_pollfd_struct_t fds[2];
-
- /* Setup of loop */
- fds[0].fd = _params_sub;
- fds[0].events = POLLIN;
-
- fds[1].fd = _sensor_combined_sub;
- fds[1].events = POLLIN;
-
- _gps.vel_n_m_s = 0.0f;
- _gps.vel_e_m_s = 0.0f;
- _gps.vel_d_m_s = 0.0f;
-
- _task_running = true;
-
- while (!_task_should_exit) {
-
- /* wait for up to 100ms for data */
- int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
-
- /* timed out - periodic check for _task_should_exit, etc. */
- if (pret == 0) {
- continue;
- }
-
- /* this is undesirable but not much we can do - might want to flag unhappy status */
- if (pret < 0) {
- warn("POLL ERR %d, %d", pret, errno);
- continue;
- }
-
- perf_begin(_loop_perf);
- perf_count(_loop_intvl);
-
- /* only update parameters if they changed */
- if (fds[0].revents & POLLIN) {
- /* read from param to clear updated flag */
- struct parameter_update_s update;
- orb_copy(ORB_ID(parameter_update), _params_sub, &update);
-
- /* update parameters from storage */
- parameters_update();
- }
-
- /* only run estimator if gyro updated */
- if (fds[1].revents & POLLIN) {
-
- /* check vehicle status for changes to publication state */
- bool prev_hil = (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
- vehicle_status_poll();
- vehicle_land_detected_poll();
-
- perf_count(_perf_gyro);
-
- /* Reset baro reference if switching to HIL, reset sensor states */
- if (!prev_hil && (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)) {
- /* system is in HIL now, wait for measurements to come in one last round */
- usleep(60000);
-
- /* now read all sensor publications to ensure all real sensor data is purged */
- orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
-
- _baro_init = false;
- _gps_initialized = false;
-
- _last_sensor_timestamp = hrt_absolute_time();
-
- _ekf->ZeroVariables();
- _ekf->dtIMU = 0.01f;
- _filter_start_time = _last_sensor_timestamp;
-
- /* now skip this loop and get data on the next one, which will also re-init the filter */
- continue;
- }
-
- /**
- * PART ONE: COLLECT ALL DATA
- **/
- pollData();
-
- /*
- * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
- */
- if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
- continue;
- }
-
- /**
- * PART TWO: EXECUTE THE FILTER
- *
- * We run the filter only once all data has been fetched
- **/
-
- if (_baro_init && _sensor_combined.timestamp &&
- _sensor_combined.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID &&
- _sensor_combined.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
-
- // maintain filtered baro and gps altitudes to calculate weather offset
- // baro sample rate is ~70Hz and measurement bandwidth is high
- // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
- // maintain heavily filtered values for both baro and gps altitude
- // Assume the filtered output should be identical for both sensors
-
- if (_gpsIsGood) {
- _baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
- }
-
-// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
-// _last_debug_print = hrt_absolute_time();
-// perf_print_counter(_perf_baro);
-// perf_reset(_perf_baro);
-// PX4_INFO("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
-// (double)_baro_gps_offset,
-// (double)_baro_alt_filt,
-// (double)_gps_alt_filt,
-// (double)_global_pos.alt,
-// (double)_local_pos.z);
-// }
-
- /* Initialize the filter first */
- if (!_ekf->statesInitialised) {
- // North, East Down position (m)
- float initVelNED[3] = {0.0f, 0.0f, 0.0f};
-
- _ekf->posNE[0] = 0.0f;
- _ekf->posNE[1] = 0.0f;
-
- _local_pos.ref_alt = 0.0f;
- _baro_gps_offset = 0.0f;
- _baro_alt_filt = _baro.altitude;
-
- _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
-
- _filter_ref_offset = -_baro.altitude;
-
- } else {
-
- if (!_gps_initialized && _gpsIsGood) {
- initializeGPS();
- continue;
- }
-
- // Check if on ground - status is used by covariance prediction
- _ekf->setOnGround(_vehicle_land_detected.landed);
-
- // We're apparently initialized in this case now
- // check (and reset the filter as needed)
- int check = check_filter_state();
-
- if (check) {
- // Let the system re-initialize itself
- continue;
- }
-
- // Run EKF data fusion steps
- updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
-
- // Run separate terrain estimator
- _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance);
- _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att);
-
- // Publish attitude estimations
- publishAttitude();
-
- // Publish Local Position estimations
- publishLocalPosition();
-
- // Publish Global Position, it will have a large uncertainty
- // set if only altitude is known
- publishGlobalPosition();
-
- // Publish wind estimates
- if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
- publishWindEstimate();
- }
- }
- }
-
- }
-
- perf_end(_loop_perf);
- }
-
- _task_running = false;
-
- _estimator_task = -1;
-}
-
-void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
- bool gps_valid, double lat, double lon, float gps_alt, float baro_alt)
-{
- // Reference altitude
- if (PX4_ISFINITE(_ekf->states[9])) {
- _filter_ref_offset = _ekf->states[9];
-
- } else if (PX4_ISFINITE(-_ekf->hgtMea)) {
- _filter_ref_offset = -_ekf->hgtMea;
-
- } else {
- _filter_ref_offset = -_baro.altitude;
- }
-
- // init filtered gps and baro altitudes
- _baro_alt_filt = baro_alt;
-
- if (gps_valid) {
- _gps_alt_filt = gps_alt;
-
- // Initialize projection
- _local_pos.ref_lat = lat;
- _local_pos.ref_lon = lon;
- _local_pos.ref_alt = gps_alt;
- _local_pos.ref_timestamp = timestamp;
-
- map_projection_init(&_pos_ref, lat, lon);
- }
-}
-
-void AttitudePositionEstimatorEKF::initializeGPS()
-{
- // GPS is in scaled integers, convert
- double lat = _gps.lat / 1.0e7;
- double lon = _gps.lon / 1.0e7;
- float gps_alt = _gps.alt / 1e3f;
-
- // Set up height correctly
- orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-
- _ekf->baroHgt = _baro.altitude;
- _ekf->hgtMea = _ekf->baroHgt;
-
- // Set up position variables correctly
- _ekf->GPSstatus = _gps.fix_type;
-
- _ekf->gpsLat = math::radians(lat);
- _ekf->gpsLon = math::radians(lon) - M_PI;
- _ekf->gpsHgt = gps_alt;
-
- // Look up mag declination based on current position
- float declination = math::radians(get_mag_declination(lat, lon));
-
- float initVelNED[3];
- initVelNED[0] = _gps.vel_n_m_s;
- initVelNED[1] = _gps.vel_e_m_s;
- initVelNED[2] = _gps.vel_d_m_s;
-
- _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
-
- initReferencePosition(_gps.timestamp, _gpsIsGood, lat, lon, gps_alt, _baro.altitude);
-
-#if 0
- PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
- (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
- PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
- (double)_filter_ref_offset);
- PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
- (double)math::degrees(declination));
-#endif
-
- _gps_initialized = true;
-}
-
-void AttitudePositionEstimatorEKF::publishAttitude()
-{
- // Output results
-
- _att.timestamp = _last_sensor_timestamp;
- _att.q[0] = _ekf->states[0];
- _att.q[1] = _ekf->states[1];
- _att.q[2] = _ekf->states[2];
- _att.q[3] = _ekf->states[3];
-
- _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
- _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
- _att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
-
- /* lazily publish the attitude only once available */
- if (_att_pub != nullptr) {
- /* publish the attitude */
- orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
-
- } else {
- /* advertise and publish */
- _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
- }
-}
-
-void AttitudePositionEstimatorEKF::publishLocalPosition()
-{
- _local_pos.timestamp = _last_sensor_timestamp;
- _local_pos.x = _ekf->states[7];
- _local_pos.y = _ekf->states[8];
-
- // XXX need to announce change of Z reference somehow elegantly
- _local_pos.z = _ekf->states[9] - _filter_ref_offset;
- //_local_pos.z_stable = _ekf->states[9];
-
- _local_pos.vx = _ekf->states[4];
- _local_pos.vy = _ekf->states[5];
- _local_pos.vz = _ekf->states[6];
-
- // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
- _local_pos.z_deriv = _ekf->states[6];
-
- _local_pos.xy_valid = _gps_initialized && _gpsIsGood;
- _local_pos.z_valid = true;
- _local_pos.v_xy_valid = _gps_initialized && _gpsIsGood;
- _local_pos.v_z_valid = true;
- _local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here
-
- // TODO provide calculated values for these
- _local_pos.eph = 0.0f;
- _local_pos.epv = 0.0f;
- _local_pos.evh = 0.0f;
- _local_pos.evv = 0.0f;
-
- _local_pos.z_global = false;
- matrix::Eulerf euler = matrix::Quatf(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
- _local_pos.yaw = euler.psi();
-
- if (!PX4_ISFINITE(_local_pos.x) ||
- !PX4_ISFINITE(_local_pos.y) ||
- !PX4_ISFINITE(_local_pos.z) ||
- !PX4_ISFINITE(_local_pos.vx) ||
- !PX4_ISFINITE(_local_pos.vy) ||
- !PX4_ISFINITE(_local_pos.vz)) {
- // bad data, abort publication
- return;
- }
-
- /* lazily publish the local position only once available */
- if (_local_pos_pub != nullptr) {
- /* publish the attitude setpoint */
- orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
-
- } else {
- /* advertise and publish */
- _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
- }
-}
-
-void AttitudePositionEstimatorEKF::publishGlobalPosition()
-{
- _global_pos.timestamp = _local_pos.timestamp;
-
- if (_local_pos.xy_global) {
- double est_lat, est_lon;
- map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
- _global_pos.lat = est_lat;
- _global_pos.lon = est_lon;
-
- } else {
- _global_pos.lat = 0.0;
- _global_pos.lon = 0.0;
- }
-
- if (_local_pos.v_xy_valid) {
- _global_pos.vel_n = _local_pos.vx;
- _global_pos.vel_e = _local_pos.vy;
-
- } else {
- _global_pos.vel_n = 0.0f;
- _global_pos.vel_e = 0.0f;
- }
-
- /* local pos alt is negative, change sign and add alt offsets */
- _global_pos.alt = (-_local_pos.z) - _filter_ref_offset - _baro_gps_offset;
-
- if (_local_pos.v_z_valid) {
- _global_pos.vel_d = _local_pos.vz;
- } else {
- _global_pos.vel_d = 0.0f;
- }
-
- // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
- _global_pos.pos_d_deriv = _global_pos.vel_d;
-
- /* terrain altitude */
- if (_terrain_estimator->is_valid()) {
- _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();
- _global_pos.terrain_alt_valid = true;
-
- } else {
- _global_pos.terrain_alt_valid = false;
- }
-
- /* baro altitude */
- _global_pos.pressure_alt = _ekf->baroHgt;
-
- _global_pos.yaw = _local_pos.yaw;
-
- const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
-
- if (!_local_pos.xy_global ||
- !_local_pos.v_xy_valid ||
- _gps.timestamp == 0 ||
- (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
-
- _global_pos.eph = EPH_LARGE_VALUE;
- _global_pos.epv = EPV_LARGE_VALUE;
-
- } else {
- _global_pos.eph = _gps.eph;
- _global_pos.epv = _gps.epv;
- }
-
- if (!PX4_ISFINITE(_global_pos.lat) ||
- !PX4_ISFINITE(_global_pos.lon) ||
- !PX4_ISFINITE(_global_pos.alt) ||
- !PX4_ISFINITE(_global_pos.vel_n) ||
- !PX4_ISFINITE(_global_pos.vel_e) ||
- !PX4_ISFINITE(_global_pos.vel_d)) {
- // bad data, abort publication
- return;
- }
-
- // TODO provide calculated values for these
- _global_pos.evh = 0.0f;
- _global_pos.evv = 0.0f;
-
- /* lazily publish the global position only once available */
- if (_global_pos_pub != nullptr) {
- /* publish the global position */
- orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
-
- } else {
- /* advertise and publish */
- _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
- }
-}
-
-void AttitudePositionEstimatorEKF::publishWindEstimate()
-{
- _wind.timestamp = _global_pos.timestamp;
- _wind.windspeed_north = _ekf->states[14];
- _wind.windspeed_east = _ekf->states[15];
-
- // XXX we need to do something smart about the variance here
- // but we default to the estimated variance for now
- _wind.variance_north = _ekf->P[14][14];
- _wind.variance_east = _ekf->P[15][15];
-
- /* lazily publish the wind estimate only once available */
- if (_wind_pub != nullptr) {
- /* publish the wind estimate */
- orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
-
- } else {
- /* advertise and publish */
- _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
- }
-
-}
-
-void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag,
- const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed)
-{
- // Run the strapdown INS equations every IMU update
- _ekf->UpdateStrapdownEquationsNED();
-
- // store the predicted states for subsequent use by measurement fusion
- _ekf->StoreStates(getMillis());
-
- // sum delta angles and time used by covariance prediction
- _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
- _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
- _covariancePredictionDt += _ekf->dtIMU;
-
- // only fuse every few steps
- if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) {
- _prediction_steps++;
- return;
-
- } else {
- _prediction_steps = 0;
- _prediction_last = hrt_absolute_time();
- }
-
- // perform a covariance prediction if the total delta angle has exceeded the limit
- // or the time limit will be exceeded at the next IMU update
- if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU))
- || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
- _ekf->CovariancePrediction(_covariancePredictionDt);
- _ekf->summedDelAng.zero();
- _ekf->summedDelVel.zero();
- _covariancePredictionDt = 0.0f;
- }
-
- // Fuse GPS Measurements
- if (fuseGPS && _gps_initialized) {
- // Convert GPS measurements to Pos NE, hgt and Vel NED
-
- // set fusion flags
- _ekf->fuseVelData = _gps.vel_ned_valid;
- _ekf->fusePosData = true;
-
- // recall states stored at time of measurement after adjusting for delays
- _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
- _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
-
- // run the fusion step
- _ekf->FuseVelposNED();
-
- } else if (!_gps_initialized) {
-
- // force static mode
- _ekf->staticMode = true;
-
- // Convert GPS measurements to Pos NE, hgt and Vel NED
- _ekf->velNED[0] = 0.0f;
- _ekf->velNED[1] = 0.0f;
- _ekf->velNED[2] = 0.0f;
-
- _ekf->posNE[0] = 0.0f;
- _ekf->posNE[1] = 0.0f;
-
- // set fusion flags
- _ekf->fuseVelData = true;
- _ekf->fusePosData = true;
-
- // recall states stored at time of measurement after adjusting for delays
- _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
- _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
-
- // run the fusion step
- _ekf->FuseVelposNED();
-
- } else {
- _ekf->fuseVelData = false;
- _ekf->fusePosData = false;
- }
-
- if (fuseBaro) {
- // Could use a blend of GPS and baro alt data if desired
- _ekf->hgtMea = _ekf->baroHgt;
- _ekf->fuseHgtData = true;
-
- // recall states stored at time of measurement after adjusting for delays
- _ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms));
-
- // run the fusion step
- _ekf->FuseVelposNED();
-
- } else {
- _ekf->fuseHgtData = false;
- }
-
- // Fuse Magnetometer Measurements
- if (fuseMag) {
- _ekf->fuseMagData = true;
- _ekf->RecallStates(_ekf->statesAtMagMeasTime,
- (getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
-
- _ekf->magstate.obsIndex = 0;
- _ekf->FuseMagnetometer();
- _ekf->FuseMagnetometer();
- _ekf->FuseMagnetometer();
-
- } else {
- _ekf->fuseMagData = false;
- }
-
- // Fuse Airspeed Measurements
- if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
- _ekf->fuseVtasData = true;
- _ekf->RecallStates(_ekf->statesAtVtasMeasTime,
- (getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
- _ekf->FuseAirspeed();
-
- } else {
- _ekf->fuseVtasData = false;
- }
-
- // Fuse Rangefinder Measurements
- if (fuseRangeSensor) {
- if (_ekf->Tnb.z.z > 0.9f) {
- // _ekf->rngMea is set in sensor readout already
- _ekf->fuseRngData = true;
- _ekf->fuseOptFlowData = false;
- _ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f));
- _ekf->OpticalFlowEKF();
- _ekf->fuseRngData = false;
- }
- }
-}
-
-int AttitudePositionEstimatorEKF::start()
-{
- ASSERT(_estimator_task == -1);
-
- /* start the task */
- _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 20,
- 4600,
- (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
- nullptr);
-
- if (_estimator_task < 0) {
- warn("task start failed");
- return -errno;
- }
-
- return OK;
-}
-
-void AttitudePositionEstimatorEKF::print_status()
-{
- math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
- math::Matrix<3, 3> R = q.to_dcm();
- math::Vector<3> euler = R.to_euler();
-
- PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
- (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
-
- // State vector:
- // 0-3: quaternions (q0, q1, q2, q3)
- // 4-6: Velocity - m/sec (North, East, Down)
- // 7-9: Position - m (North, East, Down)
- // 10-12: Delta Angle bias - rad (X,Y,Z)
- // 13: Delta Velocity Bias - m/s (Z)
- // 14-15: Wind Vector - m/sec (North,East)
- // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
- // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
-
- PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis());
- PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt);
- PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt);
- PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset,
- (double)_baro_gps_offset);
- PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
- (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
- PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
- (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
- (double)_ekf->correctedDelAng.z);
-
- PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
- (double)_ekf->states[2], (double)_ekf->states[3]);
- PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
- (double)_ekf->states[6]);
- PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8],
- (double)_ekf->states[9]);
- PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11],
- (double)_ekf->states[12]);
-
- if (EKF_STATE_ESTIMATES == 23) {
- PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]);
- PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
- PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
- (double)_ekf->states[18]);
- PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
- (double)_ekf->states[21]);
- PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]);
-
- } else {
- PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]);
- PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
- PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
- (double)_ekf->states[18]);
- PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
- (double)_ekf->states[21]);
- }
-
- PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
- (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
- (_vehicle_land_detected.landed) ? "ON_GROUND" : "AIRBORNE",
- (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
- (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
- (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
- (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
- (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
- (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
- (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
- (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
-}
-
-void AttitudePositionEstimatorEKF::pollData()
-{
- //Update arming status
- bool armedUpdate;
- orb_check(_armedSub, &armedUpdate);
-
- if (armedUpdate) {
- orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
- }
-
- orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
-
- /* set time fields */
- IMUusec = _sensor_combined.timestamp;
- float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
-
- /* guard against too large deltaT's */
- if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) {
-
- if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) {
- deltaT = _ekf->dtIMUfilt;
-
- } else {
- deltaT = 0.01f;
- }
- }
-
- /* fill in last data set */
- _ekf->dtIMU = deltaT;
-
- _ekf->angRate.x = _sensor_combined.gyro_rad[0];
- _ekf->angRate.y = _sensor_combined.gyro_rad[1];
- _ekf->angRate.z = _sensor_combined.gyro_rad[2];
-
- float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
- _ekf->dAngIMU = _ekf->angRate * gyro_dt;
-
- perf_count(_perf_gyro);
-
- if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
-
- _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
- _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
- _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
-
- float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
- _ekf->dVelIMU = _ekf->accel * accel_dt;
-
- _last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
- }
-
- Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
- _sensor_combined.magnetometer_ga[2]);
-
- if (mag.length() > 0.1f && _last_mag != _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative) {
- _ekf->magData.x = mag.x;
- _ekf->magData.y = mag.y;
- _ekf->magData.z = mag.z;
- _newDataMag = true;
- _last_mag = _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative;
- perf_count(_perf_mag);
- }
-
- _last_sensor_timestamp = _sensor_combined.timestamp;
-
- // XXX this is for assessing the filter performance
- // leave this in as long as larger improvements are still being made.
-#if 0
-
- float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1.e6f;
- float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1.e6f;
-
- static unsigned dtoverflow5 = 0;
- static unsigned dtoverflow10 = 0;
- static hrt_abstime lastprint = 0;
-
- if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) {
- PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
- (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
- dtoverflow5, dtoverflow10);
-
- PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
- (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
- (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
-
- PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
- (double)(_sensor_combined.gyro_rad[0]), (double)(_sensor_combined.gyro_rad[2]),
- (double)(_sensor_combined.accelerometer_m_s2[0]),
- (double)(_sensor_combined.accelerometer_m_s2[1]),
- (double)(_sensor_combined.accelerometer_m_s2[2]));
-
- PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
- (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
-
- lastprint = hrt_absolute_time();
- }
-
- if (deltaT > 0.005f) {
- dtoverflow5++;
- }
-
- if (deltaT > 0.01f) {
- dtoverflow10++;
- }
-
-#endif
-
- _data_good = true;
-
- //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
-
- //Update AirSpeed
- orb_check(_airspeed_sub, &_newAdsData);
-
- if (_newAdsData) {
- orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
- perf_count(_perf_airspeed);
-
- _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
- }
-
- bool gps_update;
- orb_check(_gps_sub, &gps_update);
-
- if (gps_update) {
- orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
- perf_count(_perf_gps);
-
- //We are more strict for our first fix
- float requiredAccuracy = _parameters.pos_stddev_threshold;
-
- if (_gpsIsGood) {
- requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
- }
-
- //Check if the GPS fix is good enough for us to use
- if ((_gps.fix_type >= 3) && (_gps.eph < requiredAccuracy) && (_gps.epv < requiredAccuracy)) {
- _gpsIsGood = true;
-
- } else {
- _gpsIsGood = false;
- }
-
- if (_gpsIsGood) {
-
- //Calculate time since last good GPS fix
- const float dtLastGoodGPS = static_cast<float>(_gps.timestamp - _previousGPSTimestamp) / 1e6f;
-
- //Stop dead-reckoning mode
- if (_global_pos.dead_reckoning) {
- mavlink_log_info(&_mavlink_log_pub, "[ekf] stop dead-reckoning");
- }
-
- _global_pos.dead_reckoning = false;
-
- //Fetch new GPS data
- _ekf->GPSstatus = _gps.fix_type;
- _ekf->velNED[0] = _gps.vel_n_m_s;
- _ekf->velNED[1] = _gps.vel_e_m_s;
- _ekf->velNED[2] = _gps.vel_d_m_s;
-
- _ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
- _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
- _ekf->gpsHgt = _gps.alt / 1e3f;
-
- if (_previousGPSTimestamp != 0) {
- //Calculate average time between GPS updates
- _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
-
- // update LPF
- float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
-
- if (PX4_ISFINITE(filter_step)) {
- _gps_alt_filt += filter_step;
- }
- }
-
- //check if we had a GPS outage for a long time
- if (_gps_initialized) {
-
- //Convert from global frame to local frame
- map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]);
-
- if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
- _ekf->ResetPosition();
- _ekf->ResetVelocity();
- }
- }
-
- //PX4_INFO("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
-
- // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
- // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
- // } else {
- // _ekf->vneSigma = _parameters.velne_noise;
- // }
-
- // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
- // _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
- // } else {
- // _ekf->posNeSigma = _parameters.posne_noise;
- // }
-
- // PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
-
- _previousGPSTimestamp = _gps.timestamp;
-
- }
- }
-
- // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
- // then something is very wrong with the GPS (possibly a hardware failure or comlink error)
- const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
-
- if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
-
- if (_global_pos.dead_reckoning) {
- mavlink_log_info(&_mavlink_log_pub, "[ekf] gave up dead-reckoning after long timeout");
- }
-
- _gpsIsGood = false;
- _global_pos.dead_reckoning = false;
- }
-
- //If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds)
- else if (dtLastGoodGPS >= 0.5f) {
- if (_armed.armed) {
- if (!_global_pos.dead_reckoning) {
- mavlink_log_info(&_mavlink_log_pub, "[ekf] dead-reckoning enabled");
- }
-
- _global_pos.dead_reckoning = true;
-
- } else {
- _global_pos.dead_reckoning = false;
- }
- }
-
- //Update barometer
- orb_check(_baro_sub, &_newHgtData);
-
- if (_newHgtData) {
- static hrt_abstime baro_last = 0;
-
- orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-
- // init lowpass filters for baro and gps altitude
- float baro_elapsed;
-
- if (baro_last == 0) {
- baro_elapsed = 0.0f;
-
- } else {
- baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
- }
-
- baro_last = _baro.timestamp;
-
- if (!_baro_init) {
- _baro_init = true;
- _baro_alt_filt = _baro.altitude;
- }
-
- _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
-
- _ekf->baroHgt = _baro.altitude;
- float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
-
- if (PX4_ISFINITE(filter_step)) {
- _baro_alt_filt += filter_step;
- }
-
- perf_count(_perf_baro);
- }
-
- //Update range data
- orb_check(_distance_sub, &_newRangeData);
-
- if (_newRangeData) {
- orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
-
- if ((_distance.current_distance > _distance.min_distance)
- && (_distance.current_distance < _distance.max_distance)) {
- _ekf->rngMea = _distance.current_distance;
- _distance_last_valid = _distance.timestamp;
-
- } else {
- _newRangeData = false;
- }
- }
-}
-
-int AttitudePositionEstimatorEKF::trip_nan()
-{
-
- int ret = 0;
-
- // If system is not armed, inject a NaN value into the filter
- if (_armed.armed) {
- PX4_INFO("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
- ret = 1;
-
- } else {
-
- float nan_val = 0.0f / 0.0f;
-
- PX4_INFO("system not armed, tripping state vector with NaN");
- _ekf->states[5] = nan_val;
- usleep(100000);
-
- PX4_INFO("tripping covariance #1 with NaN");
- _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates
- usleep(100000);
-
- PX4_INFO("tripping covariance #2 with NaN");
- _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
- usleep(100000);
-
- PX4_INFO("tripping covariance #3 with NaN");
- _ekf->P[3][3] = nan_val; // covariance matrix
- usleep(100000);
-
- PX4_INFO("tripping Kalman gains with NaN");
- _ekf->Kfusion[0] = nan_val; // Kalman gains
- usleep(100000);
-
- PX4_INFO("tripping stored states[0] with NaN");
- _ekf->storedStates[0][0] = nan_val;
- usleep(100000);
-
- PX4_INFO("tripping states[9] with NaN");
- _ekf->states[9] = nan_val;
- usleep(100000);
-
- PX4_INFO("DONE - FILTER STATE:");
- print_status();
- }
-
- return ret;
-}
-
-int ekf_att_pos_estimator_main(int argc, char *argv[])
-{
- if (argc < 2) {
- PX4_ERR("usage: ekf_att_pos_estimator {start|stop|status|logging}");
- return 1;
- }
-
- if (!strcmp(argv[1], "start")) {
-
- if (estimator::g_estimator != nullptr) {
- PX4_ERR("already running");
- return 1;
- }
-
- estimator::g_estimator = new AttitudePositionEstimatorEKF();
-
- if (estimator::g_estimator == nullptr) {
- PX4_ERR("alloc failed");
- return 1;
- }
-
- if (OK != estimator::g_estimator->start()) {
- delete estimator::g_estimator;
- estimator::g_estimator = nullptr;
- PX4_ERR("start failed");
- return 1;
- }
-
- /* avoid memory fragmentation by not exiting start handler until the task has fully started */
- while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) {
- usleep(50000);
- PX4_INFO(".");
- }
-
- return 0;
- }
-
- if (estimator::g_estimator == nullptr) {
- PX4_ERR("not running");
- return 1;
- }
-
- if (!strcmp(argv[1], "stop")) {
-
- delete estimator::g_estimator;
- estimator::g_estimator = nullptr;
- return 0;
- }
-
- if (!strcmp(argv[1], "status")) {
- estimator::g_estimator->print_status();
-
- return 0;
- }
-
- if (!strcmp(argv[1], "trip")) {
- int ret = estimator::g_estimator->trip_nan();
-
- return ret;
- }
-
- if (!strcmp(argv[1], "logging")) {
- int ret = estimator::g_estimator->enable_logging(true);
-
- return ret;
- }
-
- if (!strcmp(argv[1], "debug")) {
- int debug = strtoul(argv[2], nullptr, 10);
- int ret = estimator::g_estimator->set_debuglevel(debug);
-
- return ret;
- }
-
- PX4_ERR("unrecognized command");
- return 1;
-}
diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
deleted file mode 100644
index ce01135c6d..0000000000
--- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+++ /dev/null
@@ -1,307 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ekf_att_pos_estimator_params.c
- *
- * Parameters defined by the attitude and position estimator task
- *
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-/*
- * Estimator parameters, accessible via MAVLink
- *
- */
-
-/**
- * Velocity estimate delay
- *
- * The delay in milliseconds of the velocity estimate from GPS.
- *
- * @unit ms
- * @min 0
- * @max 1000
- * @group Position Estimator
- */
-PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
-
-/**
- * Position estimate delay
- *
- * The delay in milliseconds of the position estimate from GPS.
- *
- * @unit ms
- * @min 0
- * @max 1000
- * @group Position Estimator
- */
-PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
-
-/**
- * Height estimate delay
- *
- * The delay in milliseconds of the height estimate from the barometer.
- *
- * @unit ms
- * @min 0
- * @max 1000
- * @group Position Estimator
- */
-PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
-
-/**
- * Mag estimate delay
- *
- * The delay in milliseconds of the magnetic field estimate from
- * the magnetometer.
- *
- * @unit ms
- * @min 0
- * @max 1000
- * @group Position Estimator
- */
-PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
-
-/**
- * True airspeeed estimate delay
- *
- * The delay in milliseconds of the airspeed estimate.
- *
- * @unit ms
- * @min 0
- * @max 1000
- * @group Position Estimator
- */
-PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
-
-/**
- * GPS vs. barometric altitude update weight
- *
- * RE-CHECK this.
- * @min 0.0
- * @max 1.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
-
-/**
- * Airspeed measurement noise.
- *
- * Increasing this value will make the filter trust this sensor
- * less and trust other sensors more.
- *
- * @min 0.5
- * @max 5.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
-
-/**
- * Velocity measurement noise in north-east (horizontal) direction.
- *
- * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
- *
- * @min 0.05
- * @max 5.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
-
-/**
- * Velocity noise in down (vertical) direction
- *
- * Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7
- *
- * @min 0.2
- * @max 3.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f);
-
-/**
- * Position noise in north-east (horizontal) direction
- *
- * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
- *
- * @min 0.1
- * @max 10.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
-
-/**
- * Position noise in down (vertical) direction
- *
- * Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0
- *
- * @min 0.5
- * @max 3.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f);
-
-/**
- * Magnetometer measurement noise
- *
- * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
- *
- * @min 0.01
- * @max 1.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
-
-/**
- * Gyro process noise
- *
- * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
- * This noise controls how much the filter trusts the gyro measurements.
- * Increasing it makes the filter trust the gyro less and other sensors more.
- *
- * @min 0.001
- * @max 0.05
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
-
-/**
- * Accelerometer process noise
- *
- * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
- * Increasing this value makes the filter trust the accelerometer less
- * and other sensors more.
- *
- * @min 0.05
- * @max 1.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.125f);
-
-/**
- * Gyro bias estimate process noise
- *
- * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
- * Increasing this value will make the gyro bias converge faster but noisier.
- *
- * @min 0.00000005
- * @max 0.00001
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
-
-/**
- * Accelerometer bias estimate process noise
- *
- * Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f.
- * Increasing this value makes the bias estimation faster and noisier.
- *
- * @min 0.00001
- * @max 0.001
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f);
-
-/**
- * Magnetometer earth frame offsets process noise
- *
- * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
- * Increasing this value makes the magnetometer earth bias estimate converge
- * faster but also noisier.
- *
- * @min 0.0001
- * @max 0.01
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
-
-/**
- * Magnetometer body frame offsets process noise
- *
- * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
- * Increasing this value makes the magnetometer body bias estimate converge faster
- * but also noisier.
- *
- * @min 0.0001
- * @max 0.01
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
-
-/**
- * Magnetometer X bias
- *
- * The magnetometer bias. This bias is learnt by the filter over time and
- * persists between boots.
- *
- * @min -0.6
- * @max 0.6
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
-
-/**
- * Magnetometer Y bias
- *
- * The magnetometer bias. This bias is learnt by the filter over time and
- * persists between boots.
- *
- * @min -0.6
- * @max 0.6
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
-
-/**
- * Magnetometer Z bias
- *
- * The magnetometer bias. This bias is learnt by the filter over time and
- * persists between boots.
- *
- * @min -0.6
- * @max 0.6
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
-
-/**
- * Threshold for filter initialization.
- *
- * If the standard deviation of the GPS position estimate is below this threshold
- * in meters, the filter will initialize.
- *
- * @min 0.3
- * @max 10.0
- * @group Position Estimator
- */
-PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
diff --git a/src/examples/ekf_att_pos_estimator/estimator_22states.cpp b/src/examples/ekf_att_pos_estimator/estimator_22states.cpp
deleted file mode 100644
index c9dcc0322e..0000000000
--- a/src/examples/ekf_att_pos_estimator/estimator_22states.cpp
+++ /dev/null
@@ -1,3330 +0,0 @@
-/****************************************************************************
-* Copyright (c) 2014, Paul Riseborough All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* Redistributions of source code must retain the above copyright notice, this
-* list of conditions and the following disclaimer.
-*
-* Redistributions in binary form must reproduce the above copyright notice,
-* this list of conditions and the following disclaimer in the documentation
-* and/or other materials provided with the distribution.
-*
-* Neither the name of the {organization} nor the names of its contributors
-* may be used to endorse or promote products derived from this software without
-* specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-****************************************************************************/
-
-/**
- * @file estimator_22states.cpp
- *
- * Implementation of the attitude and position estimator.
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-#include "estimator_22states.h"
-
-#include <px4_defines.h>
-#include <mathlib/mathlib.h>
-#include <cmath>
-
-using std::cos;
-using std::sin;
-
-#define MIN_AIRSPEED_MEAS 5.0f
-
-constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
-
-AttPosEKF::AttPosEKF() :
- covTimeStepMax(0.0f),
- covDelAngMax(0.0f),
- rngFinderPitch(0.0f),
- yawVarScale(0.0f),
- windVelSigma(0.0f),
- dAngBiasSigma(0.0f),
- dVelBiasSigma(0.0f),
- magEarthSigma(0.0f),
- magBodySigma(0.0f),
- gndHgtSigma(0.0f),
- vneSigma(0.0f),
- vdSigma(0.0f),
- posNeSigma(0.0f),
- posDSigma(0.0f),
- magMeasurementSigma(0.0f),
- airspeedMeasurementSigma(0.0f),
- gyroProcessNoise(0.0f),
- accelProcessNoise(0.0f),
- EAS2TAS(1.0f),
- magstate{},
- resetMagState{},
- KH{},
- KHP{},
- P{},
- Kfusion{},
- states{},
- resetStates{},
- storedStates{},
- statetimeStamp{},
- lastVelPosFusion(millis()),
- statesAtVelTime{},
- statesAtPosTime{},
- statesAtHgtTime{},
- statesAtMagMeasTime{},
- statesAtVtasMeasTime{},
- statesAtRngTime{},
- statesAtFlowTime{},
- accNavMag(),
- dtIMU(0.005f),
- dtIMUfilt(0.005f),
- dtVelPos(0.01f),
- dtVelPosFilt(0.01f),
- dtHgtFilt(0.01f),
- dtGpsFilt(0.1f),
- fusionModeGPS(0),
- innovVelPos{},
- varInnovVelPos{},
- velNED{},
- posNE{},
- hgtMea(0.0f),
- baroHgtOffset(0.0f),
- rngMea(0.0f),
- innovMag{},
- varInnovMag{},
- innovVtas(0.0f),
- innovRng(0.0f),
- innovOptFlow{},
- varInnovOptFlow{},
- varInnovVtas(0.0f),
- varInnovRng(0.0f),
- VtasMeas(0.0f),
- magDeclination(0.0f),
- latRef(0.0),
- lonRef(-M_PI),
- hgtRef(0.0f),
- refSet(false),
- covSkipCount(0),
- lastFixTime_ms(0),
- globalTimeStamp_ms(0),
- gpsLat(0.0),
- gpsLon(-M_PI),
- gpsHgt(0.0f),
- GPSstatus(0),
- baroHgt(0.0f),
- statesInitialised(false),
- fuseVelData(false),
- fusePosData(false),
- fuseHgtData(false),
- fuseMagData(false),
- fuseVtasData(false),
- fuseRngData(false),
- fuseOptFlowData(false),
-
- inhibitWindStates(true),
- inhibitMagStates(true),
- inhibitGndState(true),
- inhibitScaleState(true),
-
- staticMode(true),
- useGPS(false),
- useAirspeed(true),
- useCompass(true),
- useRangeFinder(true),
- useOpticalFlow(false),
-
- ekfDiverged(false),
- lastReset(0),
- current_ekf_state{},
- last_ekf_error{},
- numericalProtection(true),
- storeIndex(0),
- storedOmega{},
- Popt{},
- flowStates{},
- prevPosN(0.0f),
- prevPosE(0.0f),
- auxFlowObsInnov{},
- auxFlowObsInnovVar{},
- fScaleFactorVar(0.0f),
- R_LOS(0.0f),
- auxFlowTestRatio{},
- auxRngTestRatio(0.0f),
- flowInnovGate(0.0f),
- auxFlowInnovGate(0.0f),
- rngInnovGate(0.0f),
- minFlowRng(0.0f),
- moCompR_LOS(0.0f),
-
- _isFixedWing(false),
- _onGround(true),
- _accNavMagHorizontal(0.0f)
-{
-
- memset(&last_ekf_error, 0, sizeof(last_ekf_error));
- memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
- ZeroVariables();
- InitialiseParameters();
-}
-
-AttPosEKF::~AttPosEKF()
-{
- //dtor
-}
-
-void AttPosEKF::InitialiseParameters()
-{
- covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
- covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
- rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
- EAS2TAS = 1.0f;
-
- yawVarScale = 1.0f;
- windVelSigma = 0.1f;
- dAngBiasSigma = 1.0e-6;
- dVelBiasSigma = 0.0002f;
- magEarthSigma = 0.0003f;
- magBodySigma = 0.0003f;
-
- vneSigma = 0.2f;
- vdSigma = 0.3f;
- posNeSigma = 2.0f;
- posDSigma = 2.0f;
-
- magMeasurementSigma = 0.05;
- airspeedMeasurementSigma = 1.4f;
- gyroProcessNoise = 1.4544411e-2f;
- accelProcessNoise = 0.5f;
-
- gndHgtSigma = 0.1f; // terrain gradient 1-sigma
- R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
- flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
- auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
- rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
- minFlowRng = 0.3f; //minimum range between ground and flow sensor
- moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
-}
-
-
-void AttPosEKF::UpdateStrapdownEquationsNED()
-{
- Vector3f delVelNav;
- float q00;
- float q11;
- float q22;
- float q33;
- float q01;
- float q02;
- float q03;
- float q12;
- float q13;
- float q23;
- float rotationMag;
- float qUpdated[4];
- float quatMag;
- float deltaQuat[4];
- const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS);
-
- // Remove sensor bias errors
- correctedDelAng.x = dAngIMU.x - states[10];
- correctedDelAng.y = dAngIMU.y - states[11];
- correctedDelAng.z = dAngIMU.z - states[12];
-
- Vector3f dVelIMURel;
-
- dVelIMURel.x = dVelIMU.x;
- dVelIMURel.y = dVelIMU.y;
- dVelIMURel.z = dVelIMU.z - states[13];
-
- delAngTotal.x += correctedDelAng.x;
- delAngTotal.y += correctedDelAng.y;
- delAngTotal.z += correctedDelAng.z;
-
- // Apply corrections for earths rotation rate and coning errors
- // * and + operators have been overloaded
- correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
- prevDelAng = correctedDelAng;
-
- // Convert the rotation vector to its equivalent quaternion
- rotationMag = correctedDelAng.length();
- if (rotationMag < 1e-12f)
- {
- deltaQuat[0] = 1.0;
- deltaQuat[1] = 0.0;
- deltaQuat[2] = 0.0;
- deltaQuat[3] = 0.0;
- }
- else
- {
- // We are using double here as we are unsure how small
- // the angle differences are and if we get into numeric
- // issues with float. The runtime impact is not measurable
- // for these quantities.
- deltaQuat[0] = cos(0.5*(double)rotationMag);
- float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
- deltaQuat[1] = correctedDelAng.x*rotScaler;
- deltaQuat[2] = correctedDelAng.y*rotScaler;
- deltaQuat[3] = correctedDelAng.z*rotScaler;
- }
-
- // Update the quaternions by rotating from the previous attitude through
- // the delta angle rotation quaternion
- qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
- qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
- qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
- qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
-
- // Normalise the quaternions and update the quaternion states
- quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
- if (quatMag > 1e-16f)
- {
- float quatMagInv = 1.0f/quatMag;
- states[0] = quatMagInv*qUpdated[0];
- states[1] = quatMagInv*qUpdated[1];
- states[2] = quatMagInv*qUpdated[2];
- states[3] = quatMagInv*qUpdated[3];
- }
-
- // Calculate the body to nav cosine matrix
- q00 = sq(states[0]);
- q11 = sq(states[1]);
- q22 = sq(states[2]);
- q33 = sq(states[3]);
- q01 = states[0]*states[1];
- q02 = states[0]*states[2];
- q03 = states[0]*states[3];
- q12 = states[1]*states[2];
- q13 = states[1]*states[3];
- q23 = states[2]*states[3];
-
- Tbn.x.x = q00 + q11 - q22 - q33;
- Tbn.y.y = q00 - q11 + q22 - q33;
- Tbn.z.z = q00 - q11 - q22 + q33;
- Tbn.x.y = 2*(q12 - q03);
- Tbn.x.z = 2*(q13 + q02);
- Tbn.y.x = 2*(q12 + q03);
- Tbn.y.z = 2*(q23 - q01);
- Tbn.z.x = 2*(q13 - q02);
- Tbn.z.y = 2*(q23 + q01);
-
- Tnb = Tbn.transpose();
-
- // transform body delta velocities to delta velocities in the nav frame
- // * and + operators have been overloaded
- //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
- delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
- delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
- delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
-
- // calculate the magnitude of the nav acceleration (required for GPS
- // variance estimation)
- accNavMag = delVelNav.length()/dtIMU;
-
- //First order low-pass filtered magnitude of horizontal nav acceleration
- Vector3f derivativeNav = (delVelNav / dtIMU);
- float derivativeVelNavMagnitude = sqrtf(sq(derivativeNav.x) + sq(derivativeNav.y));
- _accNavMagHorizontal = _accNavMagHorizontal * 0.95f + derivativeVelNavMagnitude * 0.05f;
-
- // If calculating position save previous velocity
- float lastVelocity[3];
- lastVelocity[0] = states[4];
- lastVelocity[1] = states[5];
- lastVelocity[2] = states[6];
-
- // Sum delta velocities to get velocity
- states[4] = states[4] + delVelNav.x;
- states[5] = states[5] + delVelNav.y;
- states[6] = states[6] + delVelNav.z;
-
- // If calculating postions, do a trapezoidal integration for position
- states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
- states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
- states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
-
- // Constrain states (to protect against filter divergence)
- ConstrainStates();
-
- // update filtered IMU time step length
- dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
-}
-
-void AttPosEKF::CovariancePrediction(float dt)
-{
- // scalars
- float daxCov;
- float dayCov;
- float dazCov;
- float dvxCov;
- float dvyCov;
- float dvzCov;
- float dvx;
- float dvy;
- float dvz;
- float dax;
- float day;
- float daz;
- float q0;
- float q1;
- float q2;
- float q3;
- float dax_b;
- float day_b;
- float daz_b;
- float dvz_b;
-
- // arrays
- float processNoise[EKF_STATE_ESTIMATES];
- float SF[15];
- float SG[8];
- float SQ[11];
- float SPP[8] = {0};
- float nextP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES];
-
- // calculate covariance prediction process noise
- for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f;
- for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f;
- // scale gyro bias noise when on ground to allow for faster bias estimation
- float gyroBiasScale = (_onGround) ? 2.0f : 1.0f;
-
- for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale;
- processNoise[13] = dVelBiasSigma;
- if (!inhibitWindStates) {
- for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
- } else {
- for (uint8_t i=14; i<=15; i++) processNoise[i] = 0;
- }
- if (!inhibitMagStates) {
- for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
- for (uint8_t i=19; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = dt * magBodySigma;
- } else {
- for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = 0;
- }
-
- // square all sigmas
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = sq(processNoise[i]);
-
- // set variables used to calculate covariance growth
- dvx = summedDelVel.x;
- dvy = summedDelVel.y;
- dvz = summedDelVel.z;
- dax = summedDelAng.x;
- day = summedDelAng.y;
- daz = summedDelAng.z;
- q0 = states[0];
- q1 = states[1];
- q2 = states[2];
- q3 = states[3];
- dax_b = states[10];
- day_b = states[11];
- daz_b = states[12];
- dvz_b = states[13];
- gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f);
- daxCov = sq(dt*gyroProcessNoise);
- dayCov = sq(dt*gyroProcessNoise);
- dazCov = sq(dt*gyroProcessNoise);
- if (_onGround) dazCov = dazCov * sq(yawVarScale);
- accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f);
- dvxCov = sq(dt*accelProcessNoise);
- dvyCov = sq(dt*accelProcessNoise);
- dvzCov = sq(dt*accelProcessNoise);
-
- // Predicted covariance calculation
- SF[0] = dvz - dvz_b;
- SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2;
- SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
- SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
- SF[4] = day/2 - day_b/2;
- SF[5] = daz/2 - daz_b/2;
- SF[6] = dax/2 - dax_b/2;
- SF[7] = dax_b/2 - dax/2;
- SF[8] = daz_b/2 - daz/2;
- SF[9] = day_b/2 - day/2;
- SF[10] = 2*q0*SF[0];
- SF[11] = q1/2;
- SF[12] = q2/2;
- SF[13] = q3/2;
- SF[14] = 2*dvy*q1;
-
- SG[0] = q0/2;
- SG[1] = sq(q3);
- SG[2] = sq(q2);
- SG[3] = sq(q1);
- SG[4] = sq(q0);
- SG[5] = 2*q2*q3;
- SG[6] = 2*q1*q3;
- SG[7] = 2*q1*q2;
-
- SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
- SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
- SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
- SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
- SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
- SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
- SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
- SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
- SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
- SQ[9] = sq(SG[0]);
- SQ[10] = sq(q1);
-
- SPP[0] = SF[10] + SF[14] - 2*dvx*q2;
- SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
- SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
- SPP[3] = 2*q0*q1 - 2*q2*q3;
- SPP[4] = 2*q0*q2 + 2*q1*q3;
- SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
- SPP[6] = SF[13];
- SPP[7] = SF[12];
-
- nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
- nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2;
- nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2;
- nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2;
- nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
- nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
- nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
- nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]);
- nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]);
- nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]);
- nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6];
- nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6];
- nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6];
- nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6];
- nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6];
- nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6];
- nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6];
- nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6];
- nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6];
- nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6];
- nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6];
- nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6];
- nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2);
- nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2;
- nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2;
- nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2;
- nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
- nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
- nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
- nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2);
- nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2);
- nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2);
- nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2;
- nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2;
- nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2;
- nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2;
- nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2;
- nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2;
- nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2;
- nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2;
- nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2;
- nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2;
- nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2;
- nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2;
- nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2);
- nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2;
- nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2;
- nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2;
- nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
- nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
- nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
- nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2);
- nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2);
- nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2);
- nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2;
- nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2;
- nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2;
- nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2;
- nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2;
- nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2;
- nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2;
- nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2;
- nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2;
- nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2;
- nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2;
- nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2;
- nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2);
- nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2;
- nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2;
- nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2;
- nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
- nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
- nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
- nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2);
- nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2);
- nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2);
- nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2;
- nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2;
- nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2;
- nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2;
- nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2;
- nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2;
- nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2;
- nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2;
- nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2;
- nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2;
- nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2;
- nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2;
- nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]);
- nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2;
- nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2;
- nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2;
- nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
- nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
- nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
- nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]);
- nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]);
- nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]);
- nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4];
- nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4];
- nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4];
- nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4];
- nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4];
- nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4];
- nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4];
- nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4];
- nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4];
- nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4];
- nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4];
- nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4];
- nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]);
- nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2;
- nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2;
- nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2;
- nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
- nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
- nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
- nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]);
- nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]);
- nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]);
- nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3];
- nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3];
- nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3];
- nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3];
- nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3];
- nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3];
- nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3];
- nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3];
- nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3];
- nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3];
- nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3];
- nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3];
- nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
- nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
- nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
- nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
- nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
- nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
- nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
- nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]);
- nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]);
- nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]);
- nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5];
- nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5];
- nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5];
- nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5];
- nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5];
- nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5];
- nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5];
- nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5];
- nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5];
- nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5];
- nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5];
- nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5];
- nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt);
- nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
- nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
- nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
- nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt);
- nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt);
- nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt);
- nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
- nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
- nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
- nextP[7][10] = P[7][10] + P[4][10]*dt;
- nextP[7][11] = P[7][11] + P[4][11]*dt;
- nextP[7][12] = P[7][12] + P[4][12]*dt;
- nextP[7][13] = P[7][13] + P[4][13]*dt;
- nextP[7][14] = P[7][14] + P[4][14]*dt;
- nextP[7][15] = P[7][15] + P[4][15]*dt;
- nextP[7][16] = P[7][16] + P[4][16]*dt;
- nextP[7][17] = P[7][17] + P[4][17]*dt;
- nextP[7][18] = P[7][18] + P[4][18]*dt;
- nextP[7][19] = P[7][19] + P[4][19]*dt;
- nextP[7][20] = P[7][20] + P[4][20]*dt;
- nextP[7][21] = P[7][21] + P[4][21]*dt;
- nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt);
- nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
- nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
- nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
- nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt);
- nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt);
- nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt);
- nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
- nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
- nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
- nextP[8][10] = P[8][10] + P[5][10]*dt;
- nextP[8][11] = P[8][11] + P[5][11]*dt;
- nextP[8][12] = P[8][12] + P[5][12]*dt;
- nextP[8][13] = P[8][13] + P[5][13]*dt;
- nextP[8][14] = P[8][14] + P[5][14]*dt;
- nextP[8][15] = P[8][15] + P[5][15]*dt;
- nextP[8][16] = P[8][16] + P[5][16]*dt;
- nextP[8][17] = P[8][17] + P[5][17]*dt;
- nextP[8][18] = P[8][18] + P[5][18]*dt;
- nextP[8][19] = P[8][19] + P[5][19]*dt;
- nextP[8][20] = P[8][20] + P[5][20]*dt;
- nextP[8][21] = P[8][21] + P[5][21]*dt;
- nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt);
- nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
- nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
- nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
- nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt);
- nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt);
- nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt);
- nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
- nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
- nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
- nextP[9][10] = P[9][10] + P[6][10]*dt;
- nextP[9][11] = P[9][11] + P[6][11]*dt;
- nextP[9][12] = P[9][12] + P[6][12]*dt;
- nextP[9][13] = P[9][13] + P[6][13]*dt;
- nextP[9][14] = P[9][14] + P[6][14]*dt;
- nextP[9][15] = P[9][15] + P[6][15]*dt;
- nextP[9][16] = P[9][16] + P[6][16]*dt;
- nextP[9][17] = P[9][17] + P[6][17]*dt;
- nextP[9][18] = P[9][18] + P[6][18]*dt;
- nextP[9][19] = P[9][19] + P[6][19]*dt;
- nextP[9][20] = P[9][20] + P[6][20]*dt;
- nextP[9][21] = P[9][21] + P[6][21]*dt;
- nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6];
- nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2;
- nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2;
- nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2;
- nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4];
- nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3];
- nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5];
- nextP[10][7] = P[10][7] + P[10][4]*dt;
- nextP[10][8] = P[10][8] + P[10][5]*dt;
- nextP[10][9] = P[10][9] + P[10][6]*dt;
- nextP[10][10] = P[10][10];
- nextP[10][11] = P[10][11];
- nextP[10][12] = P[10][12];
- nextP[10][13] = P[10][13];
- nextP[10][14] = P[10][14];
- nextP[10][15] = P[10][15];
- nextP[10][16] = P[10][16];
- nextP[10][17] = P[10][17];
- nextP[10][18] = P[10][18];
- nextP[10][19] = P[10][19];
- nextP[10][20] = P[10][20];
- nextP[10][21] = P[10][21];
- nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6];
- nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2;
- nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2;
- nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2;
- nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4];
- nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3];
- nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5];
- nextP[11][7] = P[11][7] + P[11][4]*dt;
- nextP[11][8] = P[11][8] + P[11][5]*dt;
- nextP[11][9] = P[11][9] + P[11][6]*dt;
- nextP[11][10] = P[11][10];
- nextP[11][11] = P[11][11];
- nextP[11][12] = P[11][12];
- nextP[11][13] = P[11][13];
- nextP[11][14] = P[11][14];
- nextP[11][15] = P[11][15];
- nextP[11][16] = P[11][16];
- nextP[11][17] = P[11][17];
- nextP[11][18] = P[11][18];
- nextP[11][19] = P[11][19];
- nextP[11][20] = P[11][20];
- nextP[11][21] = P[11][21];
- nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6];
- nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2;
- nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2;
- nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2;
- nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4];
- nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3];
- nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5];
- nextP[12][7] = P[12][7] + P[12][4]*dt;
- nextP[12][8] = P[12][8] + P[12][5]*dt;
- nextP[12][9] = P[12][9] + P[12][6]*dt;
- nextP[12][10] = P[12][10];
- nextP[12][11] = P[12][11];
- nextP[12][12] = P[12][12];
- nextP[12][13] = P[12][13];
- nextP[12][14] = P[12][14];
- nextP[12][15] = P[12][15];
- nextP[12][16] = P[12][16];
- nextP[12][17] = P[12][17];
- nextP[12][18] = P[12][18];
- nextP[12][19] = P[12][19];
- nextP[12][20] = P[12][20];
- nextP[12][21] = P[12][21];
- nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6];
- nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2;
- nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2;
- nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2;
- nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4];
- nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3];
- nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5];
- nextP[13][7] = P[13][7] + P[13][4]*dt;
- nextP[13][8] = P[13][8] + P[13][5]*dt;
- nextP[13][9] = P[13][9] + P[13][6]*dt;
- nextP[13][10] = P[13][10];
- nextP[13][11] = P[13][11];
- nextP[13][12] = P[13][12];
- nextP[13][13] = P[13][13];
- nextP[13][14] = P[13][14];
- nextP[13][15] = P[13][15];
- nextP[13][16] = P[13][16];
- nextP[13][17] = P[13][17];
- nextP[13][18] = P[13][18];
- nextP[13][19] = P[13][19];
- nextP[13][20] = P[13][20];
- nextP[13][21] = P[13][21];
- nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6];
- nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2;
- nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2;
- nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2;
- nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4];
- nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3];
- nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5];
- nextP[14][7] = P[14][7] + P[14][4]*dt;
- nextP[14][8] = P[14][8] + P[14][5]*dt;
- nextP[14][9] = P[14][9] + P[14][6]*dt;
- nextP[14][10] = P[14][10];
- nextP[14][11] = P[14][11];
- nextP[14][12] = P[14][12];
- nextP[14][13] = P[14][13];
- nextP[14][14] = P[14][14];
- nextP[14][15] = P[14][15];
- nextP[14][16] = P[14][16];
- nextP[14][17] = P[14][17];
- nextP[14][18] = P[14][18];
- nextP[14][19] = P[14][19];
- nextP[14][20] = P[14][20];
- nextP[14][21] = P[14][21];
- nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6];
- nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2;
- nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2;
- nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2;
- nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4];
- nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3];
- nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5];
- nextP[15][7] = P[15][7] + P[15][4]*dt;
- nextP[15][8] = P[15][8] + P[15][5]*dt;
- nextP[15][9] = P[15][9] + P[15][6]*dt;
- nextP[15][10] = P[15][10];
- nextP[15][11] = P[15][11];
- nextP[15][12] = P[15][12];
- nextP[15][13] = P[15][13];
- nextP[15][14] = P[15][14];
- nextP[15][15] = P[15][15];
- nextP[15][16] = P[15][16];
- nextP[15][17] = P[15][17];
- nextP[15][18] = P[15][18];
- nextP[15][19] = P[15][19];
- nextP[15][20] = P[15][20];
- nextP[15][21] = P[15][21];
- nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6];
- nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2;
- nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2;
- nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2;
- nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4];
- nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3];
- nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5];
- nextP[16][7] = P[16][7] + P[16][4]*dt;
- nextP[16][8] = P[16][8] + P[16][5]*dt;
- nextP[16][9] = P[16][9] + P[16][6]*dt;
- nextP[16][10] = P[16][10];
- nextP[16][11] = P[16][11];
- nextP[16][12] = P[16][12];
- nextP[16][13] = P[16][13];
- nextP[16][14] = P[16][14];
- nextP[16][15] = P[16][15];
- nextP[16][16] = P[16][16];
- nextP[16][17] = P[16][17];
- nextP[16][18] = P[16][18];
- nextP[16][19] = P[16][19];
- nextP[16][20] = P[16][20];
- nextP[16][21] = P[16][21];
- nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6];
- nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2;
- nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2;
- nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2;
- nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4];
- nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3];
- nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5];
- nextP[17][7] = P[17][7] + P[17][4]*dt;
- nextP[17][8] = P[17][8] + P[17][5]*dt;
- nextP[17][9] = P[17][9] + P[17][6]*dt;
- nextP[17][10] = P[17][10];
- nextP[17][11] = P[17][11];
- nextP[17][12] = P[17][12];
- nextP[17][13] = P[17][13];
- nextP[17][14] = P[17][14];
- nextP[17][15] = P[17][15];
- nextP[17][16] = P[17][16];
- nextP[17][17] = P[17][17];
- nextP[17][18] = P[17][18];
- nextP[17][19] = P[17][19];
- nextP[17][20] = P[17][20];
- nextP[17][21] = P[17][21];
- nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6];
- nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2;
- nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2;
- nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2;
- nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4];
- nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3];
- nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5];
- nextP[18][7] = P[18][7] + P[18][4]*dt;
- nextP[18][8] = P[18][8] + P[18][5]*dt;
- nextP[18][9] = P[18][9] + P[18][6]*dt;
- nextP[18][10] = P[18][10];
- nextP[18][11] = P[18][11];
- nextP[18][12] = P[18][12];
- nextP[18][13] = P[18][13];
- nextP[18][14] = P[18][14];
- nextP[18][15] = P[18][15];
- nextP[18][16] = P[18][16];
- nextP[18][17] = P[18][17];
- nextP[18][18] = P[18][18];
- nextP[18][19] = P[18][19];
- nextP[18][20] = P[18][20];
- nextP[18][21] = P[18][21];
- nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6];
- nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2;
- nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2;
- nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2;
- nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4];
- nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3];
- nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5];
- nextP[19][7] = P[19][7] + P[19][4]*dt;
- nextP[19][8] = P[19][8] + P[19][5]*dt;
- nextP[19][9] = P[19][9] + P[19][6]*dt;
- nextP[19][10] = P[19][10];
- nextP[19][11] = P[19][11];
- nextP[19][12] = P[19][12];
- nextP[19][13] = P[19][13];
- nextP[19][14] = P[19][14];
- nextP[19][15] = P[19][15];
- nextP[19][16] = P[19][16];
- nextP[19][17] = P[19][17];
- nextP[19][18] = P[19][18];
- nextP[19][19] = P[19][19];
- nextP[19][20] = P[19][20];
- nextP[19][21] = P[19][21];
- nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6];
- nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2;
- nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2;
- nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2;
- nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4];
- nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3];
- nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5];
- nextP[20][7] = P[20][7] + P[20][4]*dt;
- nextP[20][8] = P[20][8] + P[20][5]*dt;
- nextP[20][9] = P[20][9] + P[20][6]*dt;
- nextP[20][10] = P[20][10];
- nextP[20][11] = P[20][11];
- nextP[20][12] = P[20][12];
- nextP[20][13] = P[20][13];
- nextP[20][14] = P[20][14];
- nextP[20][15] = P[20][15];
- nextP[20][16] = P[20][16];
- nextP[20][17] = P[20][17];
- nextP[20][18] = P[20][18];
- nextP[20][19] = P[20][19];
- nextP[20][20] = P[20][20];
- nextP[20][21] = P[20][21];
- nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6];
- nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2;
- nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2;
- nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2;
- nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4];
- nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3];
- nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5];
- nextP[21][7] = P[21][7] + P[21][4]*dt;
- nextP[21][8] = P[21][8] + P[21][5]*dt;
- nextP[21][9] = P[21][9] + P[21][6]*dt;
- nextP[21][10] = P[21][10];
- nextP[21][11] = P[21][11];
- nextP[21][12] = P[21][12];
- nextP[21][13] = P[21][13];
- nextP[21][14] = P[21][14];
- nextP[21][15] = P[21][15];
- nextP[21][16] = P[21][16];
- nextP[21][17] = P[21][17];
- nextP[21][18] = P[21][18];
- nextP[21][19] = P[21][19];
- nextP[21][20] = P[21][20];
- nextP[21][21] = P[21][21];
-
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- nextP[i][i] = nextP[i][i] + processNoise[i];
- }
-
- // If the total position variance exceds 1E6 (1000m), then stop covariance
- // growth by setting the predicted to the previous values
- // This prevent an ill conditioned matrix from occurring for long periods
- // without GPS
- if ((P[7][7] + P[8][8]) > 1E6f)
- {
- for (uint8_t i=7; i<=8; i++)
- {
- for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- nextP[i][j] = P[i][j];
- nextP[j][i] = P[j][i];
- }
- }
- }
-
- // Copy covariance
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- P[i][i] = nextP[i][i];
- }
-
- // force symmetry for observable states
- for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < i; j++)
- {
- P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
- P[j][i] = P[i][j];
- }
- }
-
- ConstrainVariances();
-}
-
-void AttPosEKF::updateDtGpsFilt(float dt)
-{
- dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
-}
-
-void AttPosEKF::updateDtHgtFilt(float dt)
-{
- dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
-}
-
-void AttPosEKF::updateDtVelPosFilt(float dt)
-{
- dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
-}
-
-void AttPosEKF::FuseVelposNED()
-{
-
- // declare variables used by fault isolation logic
- uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
- uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
- uint32_t hgtRetryTime = 500; // height measurement retry time
- uint32_t horizRetryTime;
-
- // declare variables used to check measurement errors
- float velInnov[3] = {0.0f,0.0f,0.0f};
- float posInnov[2] = {0.0f,0.0f};
- float hgtInnov = 0.0f;
-
- // declare variables used to control access to arrays
- bool fuseData[6] = {false,false,false,false,false,false};
- uint8_t stateIndex;
- uint8_t obsIndex;
- uint8_t indexLimit = 21;
-
- // declare variables used by state and covariance update calculations
- float velErr;
- float posErr;
- float R_OBS[6];
- float observation[6];
- float SK;
- float quatMag;
-
- // Perform sequential fusion of GPS measurements. This assumes that the
- // errors in the different velocity and position components are
- // uncorrelated which is not true, however in the absence of covariance
- // data from the GPS receiver it is the only assumption we can make
- // so we might as well take advantage of the computational efficiencies
- // associated with sequential fusion
- if (fuseVelData || fusePosData || fuseHgtData)
- {
- uint64_t tNow = getMicros();
- updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
- lastVelPosFusion = tNow;
-
- // scaler according to the number of repetitions of the
- // same measurement in one fusion step
- float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
-
- // scaler according to the number of repetitions of the
- // same measurement in one fusion step
- float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
-
- // set the GPS data timeout depending on whether airspeed data is present
- if (useAirspeed) {
- horizRetryTime = gpsRetryTime;
- } else {
- horizRetryTime = gpsRetryTimeNoTAS;
- }
-
- // Form the observation vector
- for (uint8_t i=0; i <=2; i++) observation[i] = velNED[i];
- for (uint8_t i=3; i <=4; i++) observation[i] = posNE[i-3];
- observation[5] = -(hgtMea);
-
- // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
- velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
- posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
- R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
- R_OBS[1] = R_OBS[0];
- R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
- R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
- R_OBS[4] = R_OBS[3];
- R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
-
- // calculate innovations and check GPS data validity using an innovation consistency check
- if (fuseVelData)
- {
- // test velocity measurements
- uint8_t imax = 2;
- if (fusionModeGPS == 1) imax = 1;
- for (uint8_t i = 0; i<=imax; i++)
- {
- velInnov[i] = statesAtVelTime[i+4] - velNED[i];
- stateIndex = 4 + i;
- varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
- }
- // apply a 5-sigma threshold
- current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
- current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
- if (current_ekf_state.velHealth || staticMode) {
- current_ekf_state.velHealth = true;
- current_ekf_state.velFailTime = millis();
- } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
- current_ekf_state.velHealth = true;
- ResetVelocity();
-
- // do not fuse bad data
- fuseVelData = false;
- }
- else
- {
- current_ekf_state.velHealth = false;
- }
- }
-
- if (fusePosData)
- {
- // test horizontal position measurements
- posInnov[0] = statesAtPosTime[7] - posNE[0];
- posInnov[1] = statesAtPosTime[8] - posNE[1];
- varInnovVelPos[3] = P[7][7] + R_OBS[3];
- varInnovVelPos[4] = P[8][8] + R_OBS[4];
- // apply a 10-sigma threshold
- current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
- current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
- if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
- {
- current_ekf_state.posHealth = true;
- current_ekf_state.posFailTime = millis();
-
- if (current_ekf_state.posTimeout) {
- ResetPosition();
-
- // do not fuse position data on this time
- // step
- fusePosData = false;
- }
- }
- else
- {
- current_ekf_state.posHealth = false;
- }
- }
-
- // test height measurements
- if (fuseHgtData)
- {
- hgtInnov = statesAtHgtTime[9] + hgtMea;
- varInnovVelPos[5] = P[9][9] + R_OBS[5];
- // apply a 10-sigma threshold
- current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
- current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
- if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
- {
- current_ekf_state.hgtHealth = true;
- current_ekf_state.hgtFailTime = millis();
-
- // if we just reset from a timeout, do not fuse
- // the height data, but reset height and stored states
- if (current_ekf_state.hgtTimeout) {
- ResetHeight();
- fuseHgtData = false;
- }
- }
- else
- {
- current_ekf_state.hgtHealth = false;
- }
- }
- // Set range for sequential fusion of velocity and position measurements depending
- // on which data is available and its health
- if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
- {
- fuseData[0] = true;
- fuseData[1] = true;
- fuseData[2] = true;
- }
- if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
- {
- fuseData[0] = true;
- fuseData[1] = true;
- }
- if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
- {
- fuseData[3] = true;
- fuseData[4] = true;
- }
- if (fuseHgtData && current_ekf_state.hgtHealth)
- {
- fuseData[5] = true;
- }
- // Fuse measurements sequentially
- for (obsIndex=0; obsIndex<=5; obsIndex++)
- {
- if (fuseData[obsIndex])
- {
- stateIndex = 4 + obsIndex;
- // Calculate the measurement innovation, using states from a
- // different time coordinate if fusing height data
- if (obsIndex <= 2)
- {
- innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
- }
- else if (obsIndex == 3 || obsIndex == 4)
- {
- innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
- }
- else if (obsIndex == 5)
- {
- innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
- }
- // Calculate the Kalman Gain
- // Calculate innovation variances - also used for data logging
- varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
- SK = 1.0/(double)varInnovVelPos[obsIndex];
- for (uint8_t i= 0; i<=indexLimit; i++)
- {
- Kfusion[i] = P[i][stateIndex]*SK;
- }
-
- // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased)
- if (obsIndex != 5) {
- Kfusion[13] = 0;
- }
- // Don't update wind states if inhibited
- if (inhibitWindStates) {
- Kfusion[14] = 0;
- Kfusion[15] = 0;
- }
- // Don't update magnetic field states if inhibited
- if (inhibitMagStates) {
- for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++)
- {
- Kfusion[i] = 0;
- }
- }
-
- // Calculate state corrections and re-normalise the quaternions
- for (uint8_t i = 0; i<=indexLimit; i++)
- {
- states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
- }
- quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
- if (quatMag > 1e-12f) // divide by 0 protection
- {
- for (uint8_t i = 0; i<=3; i++)
- {
- states[i] = states[i] / quatMag;
- }
- }
- // Update the covariance - take advantage of direct observation of a
- // single state at index = stateIndex to reduce computations
- // Optimised implementation of standard equation P = (I - K*H)*P;
- for (uint8_t i= 0; i<=indexLimit; i++)
- {
- for (uint8_t j= 0; j<=indexLimit; j++)
- {
- KHP[i][j] = Kfusion[i] * P[stateIndex][j];
- }
- }
- for (uint8_t i= 0; i<=indexLimit; i++)
- {
- for (uint8_t j= 0; j<=indexLimit; j++)
- {
- P[i][j] = P[i][j] - KHP[i][j];
- }
- }
- }
- }
- }
-
- ForceSymmetry();
- ConstrainVariances();
-
-}
-
-void AttPosEKF::FuseMagnetometer()
-{
-
- float &q0 = magstate.q0;
- float &q1 = magstate.q1;
- float &q2 = magstate.q2;
- float &q3 = magstate.q3;
- float &magN = magstate.magN;
- float &magE = magstate.magE;
- float &magD = magstate.magD;
- float &magXbias = magstate.magXbias;
- float &magYbias = magstate.magYbias;
- float &magZbias = magstate.magZbias;
- unsigned &obsIndex = magstate.obsIndex;
- Mat3f &DCM = magstate.DCM;
- float *MagPred = &magstate.MagPred[0];
- float &R_MAG = magstate.R_MAG;
- float *SH_MAG = &magstate.SH_MAG[0];
-
- float SK_MX[6];
- float SK_MY[5];
- float SK_MZ[6];
- float H_MAG[EKF_STATE_ESTIMATES];
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- H_MAG[i] = 0.0f;
- }
-
- // Perform sequential fusion of Magnetometer measurements.
- // This assumes that the errors in the different components are
- // uncorrelated which is not true, however in the absence of covariance
- // data fit is the only assumption we can make
- // so we might as well take advantage of the computational efficiencies
- // associated with sequential fusion
- if (useCompass && fuseMagData && (obsIndex < 3))
- {
- // Calculate observation jacobians and Kalman gains
- if (obsIndex == 0)
- {
- // Copy required states to local variable names
- q0 = statesAtMagMeasTime[0];
- q1 = statesAtMagMeasTime[1];
- q2 = statesAtMagMeasTime[2];
- q3 = statesAtMagMeasTime[3];
- magN = statesAtMagMeasTime[16];
- magE = statesAtMagMeasTime[17];
- magD = statesAtMagMeasTime[18];
- magXbias = statesAtMagMeasTime[19];
- magYbias = statesAtMagMeasTime[20];
- magZbias = statesAtMagMeasTime[21];
-
- // rotate predicted earth components into body axes and calculate
- // predicted measurments
- DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3;
- DCM.x.y = 2*(q1*q2 + q0*q3);
- DCM.x.z = 2*(q1*q3-q0*q2);
- DCM.y.x = 2*(q1*q2 - q0*q3);
- DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3;
- DCM.y.z = 2*(q2*q3 + q0*q1);
- DCM.z.x = 2*(q1*q3 + q0*q2);
- DCM.z.y = 2*(q2*q3 - q0*q1);
- DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
- MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias;
- MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias;
- MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias;
-
- // scale magnetometer observation error with total angular rate
- R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU);
-
- // Calculate observation jacobians
- SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
- SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
- SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
- SH_MAG[3] = sq(q3);
- SH_MAG[4] = sq(q2);
- SH_MAG[5] = sq(q1);
- SH_MAG[6] = sq(q0);
- SH_MAG[7] = 2*magN*q0;
- SH_MAG[8] = 2*magE*q3;
-
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0;
- H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
- H_MAG[1] = SH_MAG[0];
- H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
- H_MAG[3] = SH_MAG[2];
- H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
- H_MAG[17] = 2*q0*q3 + 2*q1*q2;
- H_MAG[18] = 2*q1*q3 - 2*q0*q2;
- H_MAG[19] = 1.0f;
-
- // Calculate Kalman gain
- float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
- if (temp >= R_MAG) {
- SK_MX[0] = 1.0f / temp;
- } else {
- // the calculation is badly conditioned, so we cannot perform fusion on this step
- // we increase the state variances and try again next time
- P[19][19] += 0.1f*R_MAG;
- obsIndex = 1;
- return;
- }
- SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
- SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
- SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
- SK_MX[4] = 2*q0*q2 - 2*q1*q3;
- SK_MX[5] = 2*q0*q3 + 2*q1*q2;
- Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]);
- Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]);
- Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]);
- Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]);
- Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]);
- Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]);
- Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]);
- Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]);
- Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]);
- Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]);
- Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]);
- Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]);
- Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
- // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
- Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
- // Estimation of selected states is inhibited by setting their Kalman gains to zero
- if (!inhibitWindStates) {
- Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
- Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
- } else {
- Kfusion[14] = 0;
- Kfusion[15] = 0;
- }
- if (!inhibitMagStates) {
- Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
- Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
- Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
- Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
- Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
- Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
- } else {
- for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) {
- Kfusion[i] = 0;
- }
- }
- varInnovMag[0] = 1.0f/SK_MX[0];
- innovMag[0] = MagPred[0] - magData.x;
- }
- else if (obsIndex == 1) // we are now fusing the Y measurement
- {
- // Calculate observation jacobians
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0;
- H_MAG[0] = SH_MAG[2];
- H_MAG[1] = SH_MAG[1];
- H_MAG[2] = SH_MAG[0];
- H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
- H_MAG[16] = 2*q1*q2 - 2*q0*q3;
- H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
- H_MAG[18] = 2*q0*q1 + 2*q2*q3;
- H_MAG[20] = 1;
-
- // Calculate Kalman gain
- float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
- if (temp >= R_MAG) {
- SK_MY[0] = 1.0f / temp;
- } else {
- // the calculation is badly conditioned, so we cannot perform fusion on this step
- // we increase the state variances and try again next time
- P[20][20] += 0.1f*R_MAG;
- obsIndex = 2;
- return;
- }
- SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
- SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
- SK_MY[3] = 2*q0*q3 - 2*q1*q2;
- SK_MY[4] = 2*q0*q1 + 2*q2*q3;
- Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
- Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
- Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
- Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
- Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
- Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
- Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
- Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
- Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
- Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
- Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
- Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
- Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
- // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
- Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
- // Estimation of selected states is inhibited by setting their Kalman gains to zero
- if (!inhibitWindStates) {
- Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
- Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
- } else {
- Kfusion[14] = 0;
- Kfusion[15] = 0;
- }
- if (!inhibitMagStates) {
- Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
- Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
- Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
- Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
- Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
- Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
- } else {
- Kfusion[16] = 0;
- Kfusion[17] = 0;
- Kfusion[18] = 0;
- Kfusion[19] = 0;
- Kfusion[20] = 0;
- Kfusion[21] = 0;
- }
- varInnovMag[1] = 1.0f/SK_MY[0];
- innovMag[1] = MagPred[1] - magData.y;
- }
- else if (obsIndex == 2) // we are now fusing the Z measurement
- {
- // Calculate observation jacobians
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0;
- H_MAG[0] = SH_MAG[1];
- H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
- H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
- H_MAG[3] = SH_MAG[0];
- H_MAG[16] = 2*q0*q2 + 2*q1*q3;
- H_MAG[17] = 2*q2*q3 - 2*q0*q1;
- H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
- H_MAG[21] = 1;
-
- // Calculate Kalman gain
- float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
- if (temp >= R_MAG) {
- SK_MZ[0] = 1.0f / temp;
- } else {
- // the calculation is badly conditioned, so we cannot perform fusion on this step
- // we increase the state variances and try again next time
- P[21][21] += 0.1f*R_MAG;
- obsIndex = 3;
- return;
- }
- SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
- SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
- SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
- SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
- SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
- Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
- Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
- Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
- Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
- Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
- Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
- Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
- Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
- Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
- Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
- Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
- Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
- Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
- // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
- Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
- // Estimation of selected states is inhibited by setting their Kalman gains to zero
- if (!inhibitWindStates) {
- Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
- Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
- } else {
- Kfusion[14] = 0;
- Kfusion[15] = 0;
- }
- if (!inhibitMagStates) {
- Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
- Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
- Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
- Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
- Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
- Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
- } else {
- Kfusion[16] = 0;
- Kfusion[17] = 0;
- Kfusion[18] = 0;
- Kfusion[19] = 0;
- Kfusion[20] = 0;
- Kfusion[21] = 0;
- }
- varInnovMag[2] = 1.0f/SK_MZ[0];
- innovMag[2] = MagPred[2] - magData.z;
-
- }
-
- // Check the innovation for consistency and don't fuse if > 5Sigma
- if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
- {
- // correct the state vector
- for (uint8_t j= 0; j < EKF_STATE_ESTIMATES; j++)
- {
- states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
- }
- // normalise the quaternion states
- float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
- if (quatMag > 1e-12f)
- {
- for (uint8_t j= 0; j<=3; j++)
- {
- float quatMagInv = 1.0f/quatMag;
- states[j] = states[j] * quatMagInv;
- }
- }
- // correct the covariance P = (I - K*H)*P
- // take advantage of the empty columns in KH to reduce the
- // number of operations
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j <= 3; j++)
- {
- KH[i][j] = Kfusion[i] * H_MAG[j];
- }
- for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f;
- if (!_onGround)
- {
- for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++)
- {
- KH[i][j] = Kfusion[i] * H_MAG[j];
- }
- }
- else
- {
- for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++)
- {
- KH[i][j] = 0.0f;
- }
- }
- }
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- KHP[i][j] = 0.0f;
- for (uint8_t k = 0; k <= 3; k++)
- {
- KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
- }
- if (!_onGround)
- {
- for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++)
- {
- KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
- }
- }
- }
- }
- }
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- P[i][j] = P[i][j] - KHP[i][j];
- }
- }
- }
- obsIndex = obsIndex + 1;
-
- ForceSymmetry();
- ConstrainVariances();
-}
-
-void AttPosEKF::FuseAirspeed()
-{
- float vn;
- float ve;
- float vd;
- float vwn;
- float vwe;
- float R_TAS = sq(airspeedMeasurementSigma);
- float SH_TAS[3];
- float SK_TAS;
- float VtasPred;
-
- // Copy required states to local variable names
- vn = statesAtVtasMeasTime[4];
- ve = statesAtVtasMeasTime[5];
- vd = statesAtVtasMeasTime[6];
- vwn = statesAtVtasMeasTime[14];
- vwe = statesAtVtasMeasTime[15];
-
- // Need to check that it is flying before fusing airspeed data
- // Calculate the predicted airspeed
- VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
- // Perform fusion of True Airspeed measurement
- if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
- {
- // Calculate observation jacobians
- SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
- SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
- SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
-
- float H_TAS[EKF_STATE_ESTIMATES];
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_TAS[i] = 0.0f;
- H_TAS[4] = SH_TAS[2];
- H_TAS[5] = SH_TAS[1];
- H_TAS[6] = vd*SH_TAS[0];
- H_TAS[14] = -SH_TAS[2];
- H_TAS[15] = -SH_TAS[1];
-
- // Calculate Kalman gains
- float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
- if (temp >= R_TAS) {
- SK_TAS = 1.0f / temp;
- } else {
- // the calculation is badly conditioned, so we cannot perform fusion on this step
- // we increase the wind state variances and try again next time
- P[14][14] += 0.05f*R_TAS;
- P[15][15] += 0.05f*R_TAS;
- return;
- }
- Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
- Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
- Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
- Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
- Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
- Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
- Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
- Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
- Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
- Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
- Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
- Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
- Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
- // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
- Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
- // Estimation of selected states is inhibited by setting their Kalman gains to zero
- if (!inhibitWindStates) {
- Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
- Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
- } else {
- Kfusion[14] = 0;
- Kfusion[15] = 0;
- }
- if (!inhibitMagStates) {
- Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
- Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
- Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
- Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
- Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
- Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
- } else {
- for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) {
- Kfusion[i] = 0;
- }
- }
- varInnovVtas = 1.0f/SK_TAS;
-
- // Calculate the measurement innovation
- innovVtas = VtasPred - VtasMeas;
- // Check the innovation for consistency and don't fuse if > 5Sigma
- if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
- {
- // correct the state vector
- for (uint8_t j=0; j < EKF_STATE_ESTIMATES; j++)
- {
- states[j] = states[j] - Kfusion[j] * innovVtas;
- }
- // normalise the quaternion states
- float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
- if (quatMag > 1e-12f)
- {
- for (uint8_t j= 0; j <= 3; j++)
- {
- float quatMagInv = 1.0f/quatMag;
- states[j] = states[j] * quatMagInv;
- }
- }
- // correct the covariance P = (I - K*H)*P
- // take advantage of the empty columns in H to reduce the
- // number of operations
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0;
- for (uint8_t j = 4; j <= 6; j++)
- {
- KH[i][j] = Kfusion[i] * H_TAS[j];
- }
- for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0;
- for (uint8_t j = 14; j <= 15; j++)
- {
- KH[i][j] = Kfusion[i] * H_TAS[j];
- }
- for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) KH[i][j] = 0.0;
- }
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- KHP[i][j] = 0.0;
- for (uint8_t k = 4; k <= 6; k++)
- {
- KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
- }
- for (uint8_t k = 14; k <= 15; k++)
- {
- KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
- }
- }
- }
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- P[i][j] = P[i][j] - KHP[i][j];
- }
- }
- }
- }
-
- ForceSymmetry();
- ConstrainVariances();
-}
-
-void AttPosEKF::zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last)
-{
- uint8_t row;
- uint8_t col;
- for (row=first; row<=last; row++)
- {
- for (col=0; col<EKF_STATE_ESTIMATES; col++)
- {
- covMat[row][col] = 0.0;
- }
- }
-}
-
-void AttPosEKF::FuseOptFlow()
-{
- static float SH_LOS[13];
- static float SK_LOS[9];
- static float q0 = 0.0f;
- static float q1 = 0.0f;
- static float q2 = 0.0f;
- static float q3 = 1.0f;
- static float vn = 0.0f;
- static float ve = 0.0f;
- static float vd = 0.0f;
- static float pd = 0.0f;
- static float ptd = 0.0f;
- static float losPred[2];
-
- // Transformation matrix from nav to body axes
- float H_LOS[2][EKF_STATE_ESTIMATES];
- float K_LOS[2][EKF_STATE_ESTIMATES];
- Vector3f velNED_local;
- Vector3f relVelSensor;
-
- // Perform sequential fusion of optical flow measurements only with valid tilt and height
- flowStates[1] = math::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
- float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
- bool validTilt = Tnb.z.z > 0.71f;
- if (validTilt)
- {
- // Sequential fusion of XY components.
-
- // Calculate observation jacobians and Kalman gains
- if (fuseOptFlowData)
- {
- // Copy required states to local variable names
- q0 = statesAtFlowTime[0];
- q1 = statesAtFlowTime[1];
- q2 = statesAtFlowTime[2];
- q3 = statesAtFlowTime[3];
- vn = statesAtFlowTime[4];
- ve = statesAtFlowTime[5];
- vd = statesAtFlowTime[6];
- pd = statesAtFlowTime[9];
- ptd = flowStates[1];
- velNED_local.x = vn;
- velNED_local.y = ve;
- velNED_local.z = vd;
-
- // calculate range from ground plain to centre of sensor fov assuming flat earth
- float range = heightAboveGndEst/Tnb_flow.z.z;
-
- // calculate relative velocity in sensor frame
- relVelSensor = Tnb_flow*velNED_local;
-
- // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
- losPred[0] = relVelSensor.y/range;
- losPred[1] = -relVelSensor.x/range;
-
- // Calculate common expressions for observation jacobians
- SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
- SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
- SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
- SH_LOS[3] = 1/(pd - ptd);
- SH_LOS[4] = 1/sq(pd - ptd);
-
- // Calculate common expressions for Kalman gains
- SK_LOS[0] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[0] * moCompR_LOS)) + (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))));
- SK_LOS[1] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[1] * moCompR_LOS))+ (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))));
- SK_LOS[2] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn);
- SK_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn);
- SK_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn);
- SK_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn);
- SK_LOS[6] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
- SK_LOS[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
- SK_LOS[8] = SH_LOS[3];
-
- // Calculate common intermediate terms
- float tempVar[9];
- tempVar[0] = SH_LOS[0]*SK_LOS[6]*SK_LOS[8];
- tempVar[1] = SH_LOS[0]*SH_LOS[2]*SH_LOS[4];
- tempVar[2] = 2.0f*SH_LOS[2]*SK_LOS[8];
- tempVar[3] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q1 + 2.0f*q2*q3);
- tempVar[4] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q3 - 2.0f*q1*q2);
- tempVar[5] = (SK_LOS[5] - q2*tempVar[2]);
- tempVar[6] = (SK_LOS[2] - q3*tempVar[2]);
- tempVar[7] = (SK_LOS[3] - q1*tempVar[2]);
- tempVar[8] = (SK_LOS[4] + q0*tempVar[2]);
-
- // calculate observation jacobians for X LOS rate
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_LOS[0][i] = 0;
- H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3];
- H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn);
- H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn);
- H_LOS[0][3] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3];
- H_LOS[0][4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2);
- H_LOS[0][5] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
- H_LOS[0][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3);
- H_LOS[0][9] = tempVar[1];
-
- // calculate Kalman gains for X LOS rate
- K_LOS[0][0] = -(P[0][0]*tempVar[8] + P[0][1]*tempVar[7] - P[0][3]*tempVar[6] + P[0][2]*tempVar[5] - P[0][4]*tempVar[4] + P[0][6]*tempVar[3] - P[0][9]*tempVar[1] + P[0][5]*tempVar[0])/(R_LOS + tempVar[8]*(P[0][0]*tempVar[8] + P[1][0]*tempVar[7] + P[2][0]*tempVar[5] - P[3][0]*tempVar[6] - P[4][0]*tempVar[4] + P[6][0]*tempVar[3] - P[9][0]*tempVar[1] + P[5][0]*tempVar[0]) + tempVar[7]*(P[0][1]*tempVar[8] + P[1][1]*tempVar[7] + P[2][1]*tempVar[5] - P[3][1]*tempVar[6] - P[4][1]*tempVar[4] + P[6][1]*tempVar[3] - P[9][1]*tempVar[1] + P[5][1]*tempVar[0]) + tempVar[5]*(P[0][2]*tempVar[8] + P[1][2]*tempVar[7] + P[2][2]*tempVar[5] - P[3][2]*tempVar[6] - P[4][2]*tempVar[4] + P[6][2]*tempVar[3] - P[9][2]*tempVar[1] + P[5][2]*tempVar[0]) - tempVar[6]*(P[0][3]*tempVar[8] + P[1][3]*tempVar[7] + P[2][3]*tempVar[5] - P[3][3]*tempVar[6] - P[4][3]*tempVar[4] + P[6][3]*tempVar[3] - P[9][3]*tempVar[1] + P[5][3]*tempVar[0]) - tempVar[4]*(P[0][4]*tempVar[8] + P[1][4]*tempVar[7] + P[2][4]*tempVar[5] - P[3][4]*tempVar[6] - P[4][4]*tempVar[4] + P[6][4]*tempVar[3] - P[9][4]*tempVar[1] + P[5][4]*tempVar[0]) + tempVar[3]*(P[0][6]*tempVar[8] + P[1][6]*tempVar[7] + P[2][6]*tempVar[5] - P[3][6]*tempVar[6] - P[4][6]*tempVar[4] + P[6][6]*tempVar[3] - P[9][6]*tempVar[1] + P[5][6]*tempVar[0]) - tempVar[1]*(P[0][9]*tempVar[8] + P[1][9]*tempVar[7] + P[2][9]*tempVar[5] - P[3][9]*tempVar[6] - P[4][9]*tempVar[4] + P[6][9]*tempVar[3] - P[9][9]*tempVar[1] + P[5][9]*tempVar[0]) + tempVar[0]*(P[0][5]*tempVar[8] + P[1][5]*tempVar[7] + P[2][5]*tempVar[5] - P[3][5]*tempVar[6] - P[4][5]*tempVar[4] + P[6][5]*tempVar[3] - P[9][5]*tempVar[1] + P[5][5]*tempVar[0]));
- K_LOS[0][1] = -SK_LOS[1]*(P[1][0]*tempVar[8] + P[1][1]*tempVar[7] - P[1][3]*tempVar[6] + P[1][2]*tempVar[5] - P[1][4]*tempVar[4] + P[1][6]*tempVar[3] - P[1][9]*tempVar[1] + P[1][5]*tempVar[0]);
- K_LOS[0][2] = -SK_LOS[1]*(P[2][0]*tempVar[8] + P[2][1]*tempVar[7] - P[2][3]*tempVar[6] + P[2][2]*tempVar[5] - P[2][4]*tempVar[4] + P[2][6]*tempVar[3] - P[2][9]*tempVar[1] + P[2][5]*tempVar[0]);
- K_LOS[0][3] = -SK_LOS[1]*(P[3][0]*tempVar[8] + P[3][1]*tempVar[7] - P[3][3]*tempVar[6] + P[3][2]*tempVar[5] - P[3][4]*tempVar[4] + P[3][6]*tempVar[3] - P[3][9]*tempVar[1] + P[3][5]*tempVar[0]);
- K_LOS[0][4] = -SK_LOS[1]*(P[4][0]*tempVar[8] + P[4][1]*tempVar[7] - P[4][3]*tempVar[6] + P[4][2]*tempVar[5] - P[4][4]*tempVar[4] + P[4][6]*tempVar[3] - P[4][9]*tempVar[1] + P[4][5]*tempVar[0]);
- K_LOS[0][5] = -SK_LOS[1]*(P[5][0]*tempVar[8] + P[5][1]*tempVar[7] - P[5][3]*tempVar[6] + P[5][2]*tempVar[5] - P[5][4]*tempVar[4] + P[5][6]*tempVar[3] - P[5][9]*tempVar[1] + P[5][5]*tempVar[0]);
- K_LOS[0][6] = -SK_LOS[1]*(P[6][0]*tempVar[8] + P[6][1]*tempVar[7] - P[6][3]*tempVar[6] + P[6][2]*tempVar[5] - P[6][4]*tempVar[4] + P[6][6]*tempVar[3] - P[6][9]*tempVar[1] + P[6][5]*tempVar[0]);
- K_LOS[0][7] = -SK_LOS[1]*(P[7][0]*tempVar[8] + P[7][1]*tempVar[7] - P[7][3]*tempVar[6] + P[7][2]*tempVar[5] - P[7][4]*tempVar[4] + P[7][6]*tempVar[3] - P[7][9]*tempVar[1] + P[7][5]*tempVar[0]);
- K_LOS[0][8] = -SK_LOS[1]*(P[8][0]*tempVar[8] + P[8][1]*tempVar[7] - P[8][3]*tempVar[6] + P[8][2]*tempVar[5] - P[8][4]*tempVar[4] + P[8][6]*tempVar[3] - P[8][9]*tempVar[1] + P[8][5]*tempVar[0]);
- K_LOS[0][9] = -SK_LOS[1]*(P[9][0]*tempVar[8] + P[9][1]*tempVar[7] - P[9][3]*tempVar[6] + P[9][2]*tempVar[5] - P[9][4]*tempVar[4] + P[9][6]*tempVar[3] - P[9][9]*tempVar[1] + P[9][5]*tempVar[0]);
- K_LOS[0][10] = -SK_LOS[1]*(P[10][0]*tempVar[8] + P[10][1]*tempVar[7] - P[10][3]*tempVar[6] + P[10][2]*tempVar[5] - P[10][4]*tempVar[4] + P[10][6]*tempVar[3] - P[10][9]*tempVar[1] + P[10][5]*tempVar[0]);
- K_LOS[0][11] = -SK_LOS[1]*(P[11][0]*tempVar[8] + P[11][1]*tempVar[7] - P[11][3]*tempVar[6] + P[11][2]*tempVar[5] - P[11][4]*tempVar[4] + P[11][6]*tempVar[3] - P[11][9]*tempVar[1] + P[11][5]*tempVar[0]);
- K_LOS[0][12] = -SK_LOS[1]*(P[12][0]*tempVar[8] + P[12][1]*tempVar[7] - P[12][3]*tempVar[6] + P[12][2]*tempVar[5] - P[12][4]*tempVar[4] + P[12][6]*tempVar[3] - P[12][9]*tempVar[1] + P[12][5]*tempVar[0]);
- // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate
- K_LOS[0][13] = 0.0f;//-SK_LOS[1]*(P[13][0]*tempVar[8] + P[13][1]*tempVar[7] - P[13][3]*tempVar[6] + P[13][2]*tempVar[5] - P[13][4]*tempVar[4] + P[13][6]*tempVar[3] - P[13][9]*tempVar[1] + P[13][5]*tempVar[0]);
- if (inhibitWindStates) {
- K_LOS[0][14] = -SK_LOS[1]*(P[14][0]*tempVar[8] + P[14][1]*tempVar[7] - P[14][3]*tempVar[6] + P[14][2]*tempVar[5] - P[14][4]*tempVar[4] + P[14][6]*tempVar[3] - P[14][9]*tempVar[1] + P[14][5]*tempVar[0]);
- K_LOS[0][15] = -SK_LOS[1]*(P[15][0]*tempVar[8] + P[15][1]*tempVar[7] - P[15][3]*tempVar[6] + P[15][2]*tempVar[5] - P[15][4]*tempVar[4] + P[15][6]*tempVar[3] - P[15][9]*tempVar[1] + P[15][5]*tempVar[0]);
- } else {
- K_LOS[0][14] = 0.0f;
- K_LOS[0][15] = 0.0f;
- }
- if (inhibitMagStates) {
- K_LOS[0][16] = -SK_LOS[1]*(P[16][0]*tempVar[8] + P[16][1]*tempVar[7] - P[16][3]*tempVar[6] + P[16][2]*tempVar[5] - P[16][4]*tempVar[4] + P[16][6]*tempVar[3] - P[16][9]*tempVar[1] + P[16][5]*tempVar[0]);
- K_LOS[0][17] = -SK_LOS[1]*(P[17][0]*tempVar[8] + P[17][1]*tempVar[7] - P[17][3]*tempVar[6] + P[17][2]*tempVar[5] - P[17][4]*tempVar[4] + P[17][6]*tempVar[3] - P[17][9]*tempVar[1] + P[17][5]*tempVar[0]);
- K_LOS[0][18] = -SK_LOS[1]*(P[18][0]*tempVar[8] + P[18][1]*tempVar[7] - P[18][3]*tempVar[6] + P[18][2]*tempVar[5] - P[18][4]*tempVar[4] + P[18][6]*tempVar[3] - P[18][9]*tempVar[1] + P[18][5]*tempVar[0]);
- K_LOS[0][19] = -SK_LOS[1]*(P[19][0]*tempVar[8] + P[19][1]*tempVar[7] - P[19][3]*tempVar[6] + P[19][2]*tempVar[5] - P[19][4]*tempVar[4] + P[19][6]*tempVar[3] - P[19][9]*tempVar[1] + P[19][5]*tempVar[0]);
- K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]);
- K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]);
- } else {
- for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) {
- K_LOS[0][i] = 0.0f;
- }
- }
-
- // calculate innovation variance and innovation for X axis observation
- varInnovOptFlow[0] = 1.0f/SK_LOS[0];
- innovOptFlow[0] = losPred[0] - flowRadXYcomp[0];
-
- // calculate intermediate common variables
- tempVar[0] = 2.0f*SH_LOS[1]*SK_LOS[8];
- tempVar[1] = (SK_LOS[2] + q0*tempVar[0]);
- tempVar[2] = (SK_LOS[5] - q1*tempVar[0]);
- tempVar[3] = (SK_LOS[3] + q2*tempVar[0]);
- tempVar[4] = (SK_LOS[4] + q3*tempVar[0]);
- tempVar[5] = SH_LOS[0]*SK_LOS[8]*(2*q0*q3 + 2*q1*q2);
- tempVar[6] = SH_LOS[0]*SK_LOS[8]*(2*q0*q2 - 2*q1*q3);
- tempVar[7] = SH_LOS[0]*SH_LOS[1]*SH_LOS[4];
- tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8];
-
- // Calculate observation jacobians for Y LOS rate
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_LOS[1][i] = 0;
- H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3];
- H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3];
- H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3];
- H_LOS[1][3] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3];
- H_LOS[1][4] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3));
- H_LOS[1][5] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2);
- H_LOS[1][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3);
- H_LOS[1][9] = -tempVar[7];
-
- // Calculate Kalman gains for Y LOS rate
- K_LOS[1][0] = SK_LOS[0]*(P[0][0]*tempVar[1] + P[0][1]*tempVar[2] - P[0][2]*tempVar[3] + P[0][3]*tempVar[4] + P[0][5]*tempVar[5] - P[0][6]*tempVar[6] - P[0][9]*tempVar[7] + P[0][4]*tempVar[8]);
- K_LOS[1][1] = SK_LOS[0]*(P[1][0]*tempVar[1] + P[1][1]*tempVar[2] - P[1][2]*tempVar[3] + P[1][3]*tempVar[4] + P[1][5]*tempVar[5] - P[1][6]*tempVar[6] - P[1][9]*tempVar[7] + P[1][4]*tempVar[8]);
- K_LOS[1][2] = SK_LOS[0]*(P[2][0]*tempVar[1] + P[2][1]*tempVar[2] - P[2][2]*tempVar[3] + P[2][3]*tempVar[4] + P[2][5]*tempVar[5] - P[2][6]*tempVar[6] - P[2][9]*tempVar[7] + P[2][4]*tempVar[8]);
- K_LOS[1][3] = SK_LOS[0]*(P[3][0]*tempVar[1] + P[3][1]*tempVar[2] - P[3][2]*tempVar[3] + P[3][3]*tempVar[4] + P[3][5]*tempVar[5] - P[3][6]*tempVar[6] - P[3][9]*tempVar[7] + P[3][4]*tempVar[8]);
- K_LOS[1][4] = SK_LOS[0]*(P[4][0]*tempVar[1] + P[4][1]*tempVar[2] - P[4][2]*tempVar[3] + P[4][3]*tempVar[4] + P[4][5]*tempVar[5] - P[4][6]*tempVar[6] - P[4][9]*tempVar[7] + P[4][4]*tempVar[8]);
- K_LOS[1][5] = SK_LOS[0]*(P[5][0]*tempVar[1] + P[5][1]*tempVar[2] - P[5][2]*tempVar[3] + P[5][3]*tempVar[4] + P[5][5]*tempVar[5] - P[5][6]*tempVar[6] - P[5][9]*tempVar[7] + P[5][4]*tempVar[8]);
- K_LOS[1][6] = SK_LOS[0]*(P[6][0]*tempVar[1] + P[6][1]*tempVar[2] - P[6][2]*tempVar[3] + P[6][3]*tempVar[4] + P[6][5]*tempVar[5] - P[6][6]*tempVar[6] - P[6][9]*tempVar[7] + P[6][4]*tempVar[8]);
- K_LOS[1][7] = SK_LOS[0]*(P[7][0]*tempVar[1] + P[7][1]*tempVar[2] - P[7][2]*tempVar[3] + P[7][3]*tempVar[4] + P[7][5]*tempVar[5] - P[7][6]*tempVar[6] - P[7][9]*tempVar[7] + P[7][4]*tempVar[8]);
- K_LOS[1][8] = SK_LOS[0]*(P[8][0]*tempVar[1] + P[8][1]*tempVar[2] - P[8][2]*tempVar[3] + P[8][3]*tempVar[4] + P[8][5]*tempVar[5] - P[8][6]*tempVar[6] - P[8][9]*tempVar[7] + P[8][4]*tempVar[8]);
- K_LOS[1][9] = SK_LOS[0]*(P[9][0]*tempVar[1] + P[9][1]*tempVar[2] - P[9][2]*tempVar[3] + P[9][3]*tempVar[4] + P[9][5]*tempVar[5] - P[9][6]*tempVar[6] - P[9][9]*tempVar[7] + P[9][4]*tempVar[8]);
- K_LOS[1][10] = SK_LOS[0]*(P[10][0]*tempVar[1] + P[10][1]*tempVar[2] - P[10][2]*tempVar[3] + P[10][3]*tempVar[4] + P[10][5]*tempVar[5] - P[10][6]*tempVar[6] - P[10][9]*tempVar[7] + P[10][4]*tempVar[8]);
- K_LOS[1][11] = SK_LOS[0]*(P[11][0]*tempVar[1] + P[11][1]*tempVar[2] - P[11][2]*tempVar[3] + P[11][3]*tempVar[4] + P[11][5]*tempVar[5] - P[11][6]*tempVar[6] - P[11][9]*tempVar[7] + P[11][4]*tempVar[8]);
- K_LOS[1][12] = SK_LOS[0]*(P[12][0]*tempVar[1] + P[12][1]*tempVar[2] - P[12][2]*tempVar[3] + P[12][3]*tempVar[4] + P[12][5]*tempVar[5] - P[12][6]*tempVar[6] - P[12][9]*tempVar[7] + P[12][4]*tempVar[8]);
- // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate
- K_LOS[1][13] = 0.0f;//SK_LOS[0]*(P[13][0]*tempVar[1] + P[13][1]*tempVar[2] - P[13][2]*tempVar[3] + P[13][3]*tempVar[4] + P[13][5]*tempVar[5] - P[13][6]*tempVar[6] - P[13][9]*tempVar[7] + P[13][4]*tempVar[8]);
- if (inhibitWindStates) {
- K_LOS[1][14] = SK_LOS[0]*(P[14][0]*tempVar[1] + P[14][1]*tempVar[2] - P[14][2]*tempVar[3] + P[14][3]*tempVar[4] + P[14][5]*tempVar[5] - P[14][6]*tempVar[6] - P[14][9]*tempVar[7] + P[14][4]*tempVar[8]);
- K_LOS[1][15] = SK_LOS[0]*(P[15][0]*tempVar[1] + P[15][1]*tempVar[2] - P[15][2]*tempVar[3] + P[15][3]*tempVar[4] + P[15][5]*tempVar[5] - P[15][6]*tempVar[6] - P[15][9]*tempVar[7] + P[15][4]*tempVar[8]);
- } else {
- K_LOS[1][14] = 0.0f;
- K_LOS[1][15] = 0.0f;
- }
- if (inhibitMagStates) {
- K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]);
- K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]);
- K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]);
- K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]);
- K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
- K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
- } else {
- for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) {
- K_LOS[1][i] = 0.0f;
- }
- }
-
- // calculate variance and innovation for Y observation
- varInnovOptFlow[1] = 1.0f/SK_LOS[1];
- innovOptFlow[1] = losPred[1] - flowRadXYcomp[1];
-
- // loop through the X and Y observations and fuse them sequentially
- for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
- // Check the innovation for consistency and don't fuse if > 5Sigma
- if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) {
- // correct the state vector
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex];
- }
- // normalise the quaternion states
- float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
- if (quatMag > 1e-12f)
- {
- for (uint8_t j= 0; j<=3; j++)
- {
- float quatMagInv = 1.0f/quatMag;
- states[j] = states[j] * quatMagInv;
- }
- }
- // correct the covariance P = (I - K*H)*P
- // take advantage of the empty columns in KH to reduce the
- // number of operations
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j <= 6; j++)
- {
- KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j];
- }
- for (uint8_t j = 7; j <= 8; j++)
- {
- KH[i][j] = 0.0f;
- }
- KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9];
- for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++)
- {
- KH[i][j] = 0.0f;
- }
- }
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- KHP[i][j] = 0.0f;
- for (uint8_t k = 0; k <= 6; k++)
- {
- KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
- }
- KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
- }
- }
- for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
- {
- P[i][j] = P[i][j] - KHP[i][j];
- }
- }
- }
- }
- }
- ForceSymmetry();
- ConstrainVariances();
- }
-}
-
-void AttPosEKF::OpticalFlowEKF()
-{
- // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
- // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
- if (!inhibitGndState) {
- float distanceTravelledSq;
- if (fuseRngData) {
- distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
- prevPosN = statesAtRngTime[7];
- prevPosE = statesAtRngTime[8];
- } else if (fuseOptFlowData) {
- distanceTravelledSq = sq(statesAtFlowTime[7] - prevPosN) + sq(statesAtFlowTime[8] - prevPosE);
- prevPosN = statesAtFlowTime[7];
- prevPosE = statesAtFlowTime[8];
- } else {
- return;
- }
- distanceTravelledSq = math::min(distanceTravelledSq, 100.0f);
- Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
- }
-
- // fuse range finder data
- if (fuseRngData) {
- float range; // range from camera to centre of image
- float q0; // quaternion at optical flow measurement time
- float q1; // quaternion at optical flow measurement time
- float q2; // quaternion at optical flow measurement time
- float q3; // quaternion at optical flow measurement time
- float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
-
- // Copy required states to local variable names
- q0 = statesAtRngTime[0];
- q1 = statesAtRngTime[1];
- q2 = statesAtRngTime[2];
- q3 = statesAtRngTime[3];
-
- // calculate Kalman gains
- float SK_RNG[3];
- SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
- SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
- SK_RNG[2] = 1/SK_RNG[0];
- float K_RNG[2];
- if (!inhibitScaleState) {
- K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
- } else {
- K_RNG[0] = 0.0f;
- }
- if (!inhibitGndState) {
- K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
- } else {
- K_RNG[1] = 0.0f;
- }
-
- // Calculate the innovation variance for data logging
- varInnovRng = 1.0f/SK_RNG[1];
-
- // constrain terrain height to be below the vehicle
- flowStates[1] = math::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
-
- // estimate range to centre of image
- range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
-
- // Calculate the measurement innovation
- innovRng = range - rngMea;
-
- // calculate the innovation consistency test ratio
- auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
-
- // Check the innovation for consistency and don't fuse if out of bounds
- if (auxRngTestRatio < 1.0f)
- {
- // correct the state
- for (uint8_t i = 0; i < 2 ; i++) {
- flowStates[i] -= K_RNG[i] * innovRng;
- }
- // constrain the states
- flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
- flowStates[1] = math::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
-
- // correct the covariance matrix
- float nextPopt[2][2];
- nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
- nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
- nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
- nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
- // prevent the state variances from becoming negative and maintain symmetry
- Popt[0][0] = math::max(nextPopt[0][0],0.0f);
- Popt[1][1] = math::max(nextPopt[1][1],0.0f);
- Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
- Popt[1][0] = Popt[0][1];
- }
- }
-
- if (fuseOptFlowData) {
- Vector3f vel; // velocity of sensor relative to ground in NED axes
- Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
- float losPred[2]; // predicted optical flow angular rate measurements
- float range; // range from camera to centre of image
- float q0; // quaternion at optical flow measurement time
- float q1; // quaternion at optical flow measurement time
- float q2; // quaternion at optical flow measurement time
- float q3; // quaternion at optical flow measurement time
- float HP[2];
- float SH_OPT[6];
- float SK_OPT[3];
- float K_OPT[2][2];
- float H_OPT[2][2];
- float nextPopt[2][2];
-
- // propagate scale factor state noise
- if (!inhibitScaleState) {
- Popt[0][0] += 1e-8f;
- } else {
- Popt[0][0] = 0.0f;
- }
-
- // Copy required states to local variable names
- q0 = statesAtFlowTime[0];
- q1 = statesAtFlowTime[1];
- q2 = statesAtFlowTime[2];
- q3 = statesAtFlowTime[3];
- vel.x = statesAtFlowTime[4];
- vel.y = statesAtFlowTime[5];
- vel.z = statesAtFlowTime[6];
-
- // constrain terrain height to be below the vehicle
- flowStates[1] = math::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
-
- // estimate range to centre of image
- range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
-
- // calculate relative velocity in sensor frame
- relVelSensor = Tnb_flow * vel;
-
- // divide velocity by range, subtract body rates and apply scale factor to
- // get predicted sensed angular optical rates relative to X and Y sensor axes
- losPred[0] = flowStates[0]*( relVelSensor.y / range) - omegaAcrossFlowTime[0];
- losPred[1] = flowStates[0]*(-relVelSensor.x / range) - omegaAcrossFlowTime[1];
-
- // calculate innovations
- auxFlowObsInnov[0] = losPred[0] - flowRadXY[0];
- auxFlowObsInnov[1] = losPred[1] - flowRadXY[1];
-
- // calculate Kalman gains
- SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
- SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3);
- SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3);
- SH_OPT[3] = statesAtFlowTime[9] - flowStates[1];
- SH_OPT[4] = 1.0f/sq(SH_OPT[3]);
- SH_OPT[5] = 1.0f/SH_OPT[3];
- float SH015 = SH_OPT[0]*SH_OPT[1]*SH_OPT[5];
- float SH025 = SH_OPT[0]*SH_OPT[2]*SH_OPT[5];
- float SH014 = SH_OPT[0]*SH_OPT[1]*SH_OPT[4];
- float SH024 = SH_OPT[0]*SH_OPT[2]*SH_OPT[4];
- SK_OPT[0] = 1.0f/(R_LOS + SH015*(Popt[0][0]*SH015 + Popt[1][0]*flowStates[0]*SH014) + flowStates[0]*SH014*(Popt[0][1]*SH015 + Popt[1][1]*flowStates[0]*SH014));
- SK_OPT[1] = 1.0f/(R_LOS + SH025*(Popt[0][0]*SH025 + Popt[1][0]*flowStates[0]*SH024) + flowStates[0]*SH024*(Popt[0][1]*SH025 + Popt[1][1]*flowStates[0]*SH024));
- SK_OPT[2] = SH_OPT[0];
- if (!inhibitScaleState) {
- K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]);
- K_OPT[0][1] = SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]);
- } else {
- K_OPT[0][0] = 0.0f;
- K_OPT[0][1] = 0.0f;
- }
- if (!inhibitGndState) {
- K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]);
- K_OPT[1][1] = SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]);
- } else {
- K_OPT[1][0] = 0.0f;
- K_OPT[1][1] = 0.0f;
- }
-
- // calculate innovation variances
- auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1];
- auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0];
-
- // calculate observations jacobians
- H_OPT[0][0] = -SH025;
- H_OPT[0][1] = -flowStates[0]*SH024;
- H_OPT[1][0] = SH015;
- H_OPT[1][1] = flowStates[0]*SH014;
-
- // Check the innovation for consistency and don't fuse if > threshold
- for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
-
- // calculate the innovation consistency test ratio
- auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(auxFlowInnovGate) * auxFlowObsInnovVar[obsIndex]);
- if (auxFlowTestRatio[obsIndex] < 1.0f) {
- // correct the state
- for (uint8_t i = 0; i < 2 ; i++) {
- flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex];
- }
- // constrain the states
- flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
- flowStates[1] = math::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
-
- // correct the covariance matrix
- for (uint8_t i = 0; i < 2 ; i++) {
- HP[i] = 0.0f;
- for (uint8_t j = 0; j < 2 ; j++) {
- HP[i] += H_OPT[obsIndex][j] * P[j][i];
- }
- }
- for (uint8_t i = 0; i < 2 ; i++) {
- for (uint8_t j = 0; j < 2 ; j++) {
- nextPopt[i][j] = P[i][j] - K_OPT[i][obsIndex] * HP[j];
- }
- }
-
- // prevent the state variances from becoming negative and maintain symmetry
- Popt[0][0] = math::max(nextPopt[0][0],0.0f);
- Popt[1][1] = math::max(nextPopt[1][1],0.0f);
- Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
- Popt[1][0] = Popt[0][1];
- }
- }
- }
-
-}
-
-void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last)
-{
- uint8_t row;
- uint8_t col;
- for (col=first; col<=last; col++)
- {
- for (row=0; row < EKF_STATE_ESTIMATES; row++)
- {
- covMat[row][col] = 0.0;
- }
- }
-}
-
-// Store states in a history array along with time stamp
-void AttPosEKF::StoreStates(uint64_t timestamp_ms)
-{
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- storedStates[i][storeIndex] = states[i];
- }
-
- storedOmega[0][storeIndex] = angRate.x;
- storedOmega[1][storeIndex] = angRate.y;
- storedOmega[2][storeIndex] = angRate.z;
- statetimeStamp[storeIndex] = timestamp_ms;
-
- // increment to next storage index
- storeIndex++;
- if (storeIndex >= EKF_DATA_BUFFER_SIZE) {
- storeIndex = 0;
- }
-}
-
-void AttPosEKF::ResetStoredStates()
-{
- // reset all stored states
- memset(&storedStates[0][0], 0, sizeof(storedStates));
- memset(&storedOmega[0][0], 0, sizeof(storedOmega));
- memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
-
- // reset store index to first
- storeIndex = 0;
-
- //Reset stored state to current state
- StoreStates(millis());
-}
-
-// Output the state vector stored at the time that best matches that specified by msec
-int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
-{
- int ret = 0;
-
- int64_t bestTimeDelta = 200;
- size_t bestStoreIndex = 0;
- for (size_t storeIndexLocal = 0; storeIndexLocal < EKF_DATA_BUFFER_SIZE; storeIndexLocal++)
- {
- // Work around a GCC compiler bug - we know 64bit support on ARM is
- // sketchy in GCC.
- uint64_t timeDelta;
-
- if (msec > statetimeStamp[storeIndexLocal]) {
- timeDelta = msec - statetimeStamp[storeIndexLocal];
- } else {
- timeDelta = statetimeStamp[storeIndexLocal] - msec;
- }
-
- if (timeDelta < (uint64_t)bestTimeDelta)
- {
- bestStoreIndex = storeIndexLocal;
- bestTimeDelta = timeDelta;
- }
- }
- if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
- {
- for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) {
- if (PX4_ISFINITE(storedStates[i][bestStoreIndex])) {
- statesForFusion[i] = storedStates[i][bestStoreIndex];
- } else if (PX4_ISFINITE(states[i])) {
- statesForFusion[i] = states[i];
- } else {
- // There is not much we can do here, except reporting the error we just
- // found.
- ret++;
- }
- }
- }
- else // otherwise output current state
- {
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- if (PX4_ISFINITE(states[i])) {
- statesForFusion[i] = states[i];
- } else {
- ret++;
- }
- }
- }
-
- return ret;
-}
-
-void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec)
-{
- // work back in time and calculate average angular rate over the time interval
- for (size_t i=0; i < 3; i++) {
- omegaForFusion[i] = 0.0f;
- }
- uint8_t sumIndex = 0;
- int64_t timeDelta;
- for (size_t storeIndexLocal = 0; storeIndexLocal < EKF_DATA_BUFFER_SIZE; storeIndexLocal++)
- {
- // calculate the average of all samples younger than msec
- timeDelta = statetimeStamp[storeIndexLocal] - msec;
- if (timeDelta > 0)
- {
- for (size_t i=0; i < 3; i++) {
- omegaForFusion[i] += storedOmega[i][storeIndexLocal];
- }
- sumIndex += 1;
- }
- }
- if (sumIndex >= 1) {
- for (size_t i=0; i < 3; i++) {
- omegaForFusion[i] = omegaForFusion[i] / float(sumIndex);
- }
- } else {
- omegaForFusion[0] = angRate.x;
- omegaForFusion[1] = angRate.y;
- omegaForFusion[2] = angRate.z;
- }
-}
-
-#if 0
-void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
-{
- // Calculate the nav to body cosine matrix
- float q00 = sq(quat[0]);
- float q11 = sq(quat[1]);
- float q22 = sq(quat[2]);
- float q33 = sq(quat[3]);
- float q01 = quat[0]*quat[1];
- float q02 = quat[0]*quat[2];
- float q03 = quat[0]*quat[3];
- float q12 = quat[1]*quat[2];
- float q13 = quat[1]*quat[3];
- float q23 = quat[2]*quat[3];
-
- Tnb.x.x = q00 + q11 - q22 - q33;
- Tnb.y.y = q00 - q11 + q22 - q33;
- Tnb.z.z = q00 - q11 - q22 + q33;
- Tnb.y.x = 2*(q12 - q03);
- Tnb.z.x = 2*(q13 + q02);
- Tnb.x.y = 2*(q12 + q03);
- Tnb.z.y = 2*(q23 - q01);
- Tnb.x.z = 2*(q13 - q02);
- Tnb.y.z = 2*(q23 + q01);
-}
-#endif
-
-void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
-{
- // Calculate the body to nav cosine matrix
- float q00 = sq(quat[0]);
- float q11 = sq(quat[1]);
- float q22 = sq(quat[2]);
- float q33 = sq(quat[3]);
- float q01 = quat[0]*quat[1];
- float q02 = quat[0]*quat[2];
- float q03 = quat[0]*quat[3];
- float q12 = quat[1]*quat[2];
- float q13 = quat[1]*quat[3];
- float q23 = quat[2]*quat[3];
-
- Tbn_ret.x.x = q00 + q11 - q22 - q33;
- Tbn_ret.y.y = q00 - q11 + q22 - q33;
- Tbn_ret.z.z = q00 - q11 - q22 + q33;
- Tbn_ret.x.y = 2*(q12 - q03);
- Tbn_ret.x.z = 2*(q13 + q02);
- Tbn_ret.y.x = 2*(q12 + q03);
- Tbn_ret.y.z = 2*(q23 - q01);
- Tbn_ret.z.x = 2*(q13 - q02);
- Tbn_ret.z.y = 2*(q23 + q01);
-}
-
-void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
-{
- float u1 = cos(0.5f*eul[0]);
- float u2 = cos(0.5f*eul[1]);
- float u3 = cos(0.5f*eul[2]);
- float u4 = sin(0.5f*eul[0]);
- float u5 = sin(0.5f*eul[1]);
- float u6 = sin(0.5f*eul[2]);
- quat[0] = u1*u2*u3+u4*u5*u6;
- quat[1] = u4*u2*u3-u1*u5*u6;
- quat[2] = u1*u5*u3+u4*u2*u6;
- quat[3] = u1*u2*u6-u4*u5*u3;
-}
-
-void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
-{
- y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
- y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
- y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
-}
-
-void AttPosEKF::setOnGround(const bool isLanded)
-{
- _onGround = isLanded;
-
- if (staticMode) {
- staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
- }
- // don't update wind states if there is no airspeed measurement
- if (_onGround || !useAirspeed) {
- inhibitWindStates = true;
- } else {
- inhibitWindStates = false;
- }
-
- //Check if we are accelerating forward, only then is the mag offset is observable
- bool isMovingForward = _accNavMagHorizontal > 0.5f;
-
- // don't update magnetic field states if on ground or not using compass
- inhibitMagStates = (!useCompass || _onGround) || (!_isFixedWing && !isMovingForward);
-
- // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS
- if ((_onGround || !useGPS) && !useRangeFinder) {
- inhibitGndState = true;
- } else {
- inhibitGndState = false;
- }
-
- // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable
- if ((_onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) {
- inhibitGndState = true;
- } else {
- inhibitGndState = false;
- }
-
- // Don't update focal length offset state if there is no range finder or optical flow sensor
- // we need both sensors to do this estimation
- if (!useRangeFinder || !useOpticalFlow) {
- inhibitScaleState = true;
- } else {
- inhibitScaleState = false;
- }
-}
-
-void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
-{
- //Define Earth rotation vector in the NED navigation frame
- omega.x = earthRate*cosf(latitude);
- omega.y = 0.0f;
- omega.z = -earthRate*sinf(latitude);
-}
-
-void AttPosEKF::CovarianceInit()
-{
- // Calculate the initial covariance matrix P
- P[0][0] = 0.25f * sq(1.0f*deg2rad);
- P[1][1] = 0.25f * sq(1.0f*deg2rad);
- P[2][2] = 0.25f * sq(1.0f*deg2rad);
- P[3][3] = 0.25f * sq(10.0f*deg2rad);
-
- //velocities
- P[4][4] = sq(0.7f);
- P[5][5] = P[4][4];
- P[6][6] = sq(0.7f);
-
- //positions
- P[7][7] = sq(15.0f);
- P[8][8] = P[7][7];
- P[9][9] = sq(5.0f);
-
- //delta angle biases
- P[10][10] = sq(0.1f*deg2rad*dtIMU);
- P[11][11] = P[10][10];
- P[12][12] = P[10][10];
-
- //Z delta velocity bias
- P[13][13] = sq(0.2f*dtIMU);
-
- //Wind velocities
- P[14][14] = 0.01f;
- P[15][15] = P[14][14];
-
- //Earth magnetic field
- P[16][16] = sq(0.02f);
- P[17][17] = P[16][16];
- P[18][18] = P[16][16];
-
- //Body magnetic field
- P[19][19] = sq(0.02f);
- P[20][20] = P[19][19];
- P[21][21] = P[19][19];
-
- //Optical flow
- fScaleFactorVar = 0.001f; // focal length scale factor variance
- Popt[0][0] = 0.001f;
-}
-
-float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
-{
- float ret;
- if (val > max_val) {
- ret = max_val;
- ekf_debug("> max: %8.4f, val: %8.4f", (double)max_val, (double)val);
- } else if (val < min_val) {
- ret = min_val;
- ekf_debug("< min: %8.4f, val: %8.4f", (double)min_val, (double)val);
- } else {
- ret = val;
- }
-
- if (!PX4_ISFINITE(val)) {
- ekf_debug("constrain: non-finite!");
- }
-
- return ret;
-}
-
-void AttPosEKF::ConstrainVariances()
-{
- if (!numericalProtection) {
- return;
- }
-
- // State vector:
- // 0-3: quaternions (q0, q1, q2, q3)
- // 4-6: Velocity - m/sec (North, East, Down)
- // 7-9: Position - m (North, East, Down)
- // 10-12: Delta Angle bias - rad (X,Y,Z)
- // 13: Delta Velocity bias - m/s (Z)
- // 14-15: Wind Vector - m/sec (North,East)
- // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
- // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
-
- // Constrain quaternion variances
- for (size_t i = 0; i <= 3; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
- }
-
- // Constrain velocity variances
- for (size_t i = 4; i <= 6; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
- }
-
- // Constrain position variances
- for (size_t i = 7; i <= 9; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
- }
-
- // Constrain delta angle bias variances
- for (size_t i = 10; i <= 12; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU));
- }
-
- // Constrain delta velocity bias variance
- P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU));
-
- // Wind velocity variances
- for (size_t i = 14; i <= 15; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
- }
-
- // Earth magnetic field variances
- for (size_t i = 16; i <= 18; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
- }
-
- // Body magnetic field variances
- for (size_t i = 19; i <= 21; i++) {
- P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
- }
-
-}
-
-void AttPosEKF::ConstrainStates()
-{
- if (!numericalProtection) {
- return;
- }
-
- // State vector:
- // 0-3: quaternions (q0, q1, q2, q3)
- // 4-6: Velocity - m/sec (North, East, Down)
- // 7-9: Position - m (North, East, Down)
- // 10-12: Delta Angle bias - rad (X,Y,Z)
- // 13: Delta Velocity bias - m/s (Z)
- // 14-15: Wind Vector - m/sec (North,East)
- // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
- // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
-
- // Constrain dtIMUfilt
- if (!PX4_ISFINITE(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) {
- dtIMUfilt = dtIMU;
- }
-
- // Constrain quaternion
- for (size_t i = 0; i <= 3; i++) {
- states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
- }
-
- // Constrain velocities to what GPS can do for us
- for (size_t i = 4; i <= 6; i++) {
- states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
- }
-
- // Constrain position to a reasonable vehicle range (in meters)
- for (size_t i = 7; i <= 8; i++) {
- states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
- }
-
- // Constrain altitude
- // NOT FOR FLIGHT : Upper value of 0.0 is a temporary fix to get around lack of range finder data during development testing
- states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f);
-
- // Angle bias limit - set to 8 degrees / sec
- for (size_t i = 10; i <= 12; i++) {
- states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt);
- }
-
- // Constrain delta velocity bias
- states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt);
-
- // Wind velocity limits - assume 120 m/s max velocity
- for (size_t i = 14; i <= 15; i++) {
- states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
- }
-
- // Earth magnetic field limits (in Gauss)
- for (size_t i = 16; i <= 18; i++) {
- states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
- }
-
- // Body magnetic field variances (in Gauss).
- // the max offset should be in this range.
- for (size_t i = 19; i <= 21; i++) {
- states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
- }
-
-}
-
-void AttPosEKF::ForceSymmetry()
-{
- if (!numericalProtection) {
- return;
- }
-
- // Force symmetry on the covariance matrix to prevent ill-conditioning
- // of the matrix which would cause the filter to blow-up
- for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++)
- {
- for (uint8_t j = 0; j < i; j++)
- {
- P[i][j] = 0.5f * (P[i][j] + P[j][i]);
- P[j][i] = P[i][j];
-
- if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) ||
- (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) {
- current_ekf_state.covariancesExcessive = true;
- current_ekf_state.error |= true;
- InitializeDynamic(velNED, magDeclination);
- return;
- }
-
- float symmetric = 0.5f * (P[i][j] + P[j][i]);
- P[i][j] = symmetric;
- P[j][i] = symmetric;
- }
- }
-}
-
-bool AttPosEKF::GyroOffsetsDiverged()
-{
- // Detect divergence by looking for rapid changes of the gyro offset
- Vector3f current_bias;
- current_bias.x = states[10];
- current_bias.y = states[11];
- current_bias.z = states[12];
-
- Vector3f delta = current_bias - lastGyroOffset;
- float delta_len = delta.length();
- float delta_len_scaled = 0.0f;
-
- // Protect against division by zero
- if (delta_len > 0.0f) {
- float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f);
- delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt;
- }
-
- bool diverged = (delta_len_scaled > 1.0f);
- lastGyroOffset = current_bias;
- current_ekf_state.error |= diverged;
- current_ekf_state.gyroOffsetsExcessive = diverged;
-
- return diverged;
-}
-
-bool AttPosEKF::VelNEDDiverged()
-{
- Vector3f current_vel;
- current_vel.x = states[4];
- current_vel.y = states[5];
- current_vel.z = states[6];
-
- Vector3f gps_vel;
- gps_vel.x = velNED[0];
- gps_vel.y = velNED[1];
- gps_vel.z = velNED[2];
-
- Vector3f delta = current_vel - gps_vel;
- float delta_len = delta.length();
-
- bool excessive = (delta_len > 30.0f);
-
- current_ekf_state.error |= excessive;
- current_ekf_state.velOffsetExcessive = excessive;
-
- return excessive;
-}
-
-bool AttPosEKF::FilterHealthy()
-{
- if (!statesInitialised) {
- return false;
- }
-
- // XXX Check state vector for NaNs and ill-conditioning
-
- // Check if any of the major inputs timed out
- if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
- return false;
- }
-
- // Nothing fired, return ok.
- return true;
-}
-
-void AttPosEKF::ResetPosition()
-{
- if (staticMode) {
- states[7] = 0;
- states[8] = 0;
- } else if (GPSstatus >= GPS_FIX_3D) {
-
- // reset the states from the GPS measurements
- states[7] = posNE[0];
- states[8] = posNE[1];
-
- // stored horizontal position states to prevent subsequent GPS measurements from being rejected
- for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
- storedStates[7][i] = states[7];
- storedStates[8][i] = states[8];
- }
- }
-
- //reset position covariance
- P[7][7] = sq(15.0f);
- P[8][8] = P[7][7];
-}
-
-void AttPosEKF::ResetHeight()
-{
- // write to the state vector
- states[9] = -hgtMea;
-
- // stored horizontal position states to prevent subsequent Barometer measurements from being rejected
- for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
- storedStates[9][i] = states[9];
- }
-
- //reset altitude covariance
- P[9][9] = sq(5.0f);
- P[6][6] = sq(0.7f);
-}
-
-void AttPosEKF::ResetVelocity()
-{
- if (staticMode) {
- states[4] = 0.0f;
- states[5] = 0.0f;
- states[6] = 0.0f;
- }
- else if (GPSstatus >= GPS_FIX_3D) {
- //Do not use Z velocity, we trust the Barometer history more
-
- states[4] = velNED[0]; // north velocity from last reading
- states[5] = velNED[1]; // east velocity from last reading
-
- // stored horizontal position states to prevent subsequent GPS measurements from being rejected
- for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
- storedStates[4][i] = states[4];
- storedStates[5][i] = states[5];
- }
- }
-
- //reset velocities covariance
- P[4][4] = sq(0.7f);
- P[5][5] = P[4][4];
-}
-
-bool AttPosEKF::StatesNaN() {
- bool err = false;
-
- // check all integrators
- if (!PX4_ISFINITE(summedDelAng.x) || !PX4_ISFINITE(summedDelAng.y) || !PX4_ISFINITE(summedDelAng.z)) {
- current_ekf_state.angNaN = true;
- ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
- err = true;
- goto out;
- } // delta angles
-
- if (!PX4_ISFINITE(correctedDelAng.x) || !PX4_ISFINITE(correctedDelAng.y) || !PX4_ISFINITE(correctedDelAng.z)) {
- current_ekf_state.angNaN = true;
- ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
- err = true;
- goto out;
- } // delta angles
-
- if (!PX4_ISFINITE(summedDelVel.x) || !PX4_ISFINITE(summedDelVel.y) || !PX4_ISFINITE(summedDelVel.z)) {
- current_ekf_state.summedDelVelNaN = true;
- ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
- err = true;
- goto out;
- } // delta velocities
-
- // check all states and covariance matrices
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
- if (!PX4_ISFINITE(KH[i][j])) {
-
- current_ekf_state.KHNaN = true;
- err = true;
- ekf_debug("KH NaN");
- goto out;
- } // intermediate result used for covariance updates
-
- if (!PX4_ISFINITE(KHP[i][j])) {
-
- current_ekf_state.KHPNaN = true;
- err = true;
- ekf_debug("KHP NaN");
- goto out;
- } // intermediate result used for covariance updates
-
- if (!PX4_ISFINITE(P[i][j])) {
-
- current_ekf_state.covarianceNaN = true;
- err = true;
- ekf_debug("P NaN");
- } // covariance matrix
- }
-
- if (!PX4_ISFINITE(Kfusion[i])) {
-
- current_ekf_state.kalmanGainsNaN = true;
- ekf_debug("Kfusion NaN");
- err = true;
- goto out;
- } // Kalman gains
-
- if (!PX4_ISFINITE(states[i])) {
-
- current_ekf_state.statesNaN = true;
- ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
- err = true;
- goto out;
- } // state matrix
- }
-
-out:
- if (err) {
- current_ekf_state.error |= true;
- }
-
- return err;
-
-}
-
-int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
-{
-
- // Store the old filter state
- bool currStaticMode = staticMode;
-
- // Limit reset rate to 5 Hz to allow the filter
- // to settle
- if (millis() - lastReset < 200) {
- return 0;
- }
-
- if (ekfDiverged) {
- ekfDiverged = false;
- }
-
- int ret = 0;
-
- // Reset the filter if the states went NaN
- if (StatesNaN()) {
- ekf_debug("re-initializing dynamic");
-
- // Reset and fill error report
- InitializeDynamic(velNED, magDeclination);
-
- ret = 1;
- }
-
- // Reset the filter if the IMU data is too old
- if (dtIMU > 0.3f) {
-
- current_ekf_state.imuTimeout = true;
-
- // Fill error report
- GetFilterState(&last_ekf_error);
-
- ResetVelocity();
- ResetPosition();
- ResetHeight();
- ResetStoredStates();
-
- // Timeout cleared with this reset
- current_ekf_state.imuTimeout = false;
-
- // that's all we can do here, return
- ret = 2;
- }
-
- // Check if we switched between states
- if (currStaticMode != staticMode) {
- // Fill error report, but not setting error flag
- GetFilterState(&last_ekf_error);
-
- ResetVelocity();
- ResetPosition();
- ResetHeight();
- ResetStoredStates();
-
- ret = 0;
- }
-
- // Reset the filter if gyro offsets are excessive
- if (GyroOffsetsDiverged()) {
-
- // Reset and fill error report
- InitializeDynamic(velNED, magDeclination);
-
- // that's all we can do here, return
- ret = 4;
- }
-
- // Reset the filter if it diverges too far from GPS
- if (VelNEDDiverged()) {
-
- // Reset and fill error report
- InitializeDynamic(velNED, magDeclination);
-
- // that's all we can do here, return
- ret = 5;
- }
-
- // The excessive covariance detection already
- // reset the filter. Just need to report here.
- if (last_ekf_error.covariancesExcessive) {
- ret = 6;
- }
-
- if (ret) {
- ekfDiverged = true;
- lastReset = millis();
-
- // This reads the last error and clears it
- GetLastErrorState(last_error);
- }
-
- return ret;
-}
-
-void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
-{
- float initialRoll, initialPitch;
- float cosRoll, sinRoll, cosPitch, sinPitch;
- float magX, magY;
- float initialHdg, cosHeading, sinHeading;
-
- initialRoll = atan2f(-ay, -az);
- initialPitch = atan2f(ax, -az);
-
- cosRoll = cosf(initialRoll);
- sinRoll = sinf(initialRoll);
- cosPitch = cosf(initialPitch);
- sinPitch = sinf(initialPitch);
-
- magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
-
- magY = my * cosRoll - mz * sinRoll;
-
- initialHdg = atan2f(-magY, magX);
- /* true heading is the mag heading minus declination */
- initialHdg += declination;
-
- cosRoll = cosf(initialRoll * 0.5f);
- sinRoll = sinf(initialRoll * 0.5f);
-
- cosPitch = cosf(initialPitch * 0.5f);
- sinPitch = sinf(initialPitch * 0.5f);
-
- cosHeading = cosf(initialHdg * 0.5f);
- sinHeading = sinf(initialHdg * 0.5f);
-
- initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
- initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
- initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
- initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
-
- /* normalize */
- float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]);
-
- initQuat[0] /= norm;
- initQuat[1] /= norm;
- initQuat[2] /= norm;
- initQuat[3] /= norm;
-}
-
-void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
-{
- if (current_ekf_state.error) {
- GetFilterState(&last_ekf_error);
- }
-
- ZeroVariables();
-
- // Reset error states
- current_ekf_state.error = false;
- current_ekf_state.angNaN = false;
- current_ekf_state.summedDelVelNaN = false;
- current_ekf_state.KHNaN = false;
- current_ekf_state.KHPNaN = false;
- current_ekf_state.PNaN = false;
- current_ekf_state.covarianceNaN = false;
- current_ekf_state.kalmanGainsNaN = false;
- current_ekf_state.statesNaN = false;
-
- current_ekf_state.velHealth = true;
- current_ekf_state.posHealth = true;
- current_ekf_state.hgtHealth = true;
-
- current_ekf_state.velTimeout = false;
- current_ekf_state.posTimeout = false;
- current_ekf_state.hgtTimeout = false;
-
- fuseVelData = false;
- fusePosData = false;
- fuseHgtData = false;
- fuseMagData = false;
- fuseVtasData = false;
-
- // Fill variables with valid data
- velNED[0] = initvelNED[0];
- velNED[1] = initvelNED[1];
- velNED[2] = initvelNED[2];
- magDeclination = declination;
-
- // Calculate initial filter quaternion states from raw measurements
- float initQuat[4];
- Vector3f initMagXYZ;
- initMagXYZ = magData - magBias;
- AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat);
-
- // Calculate initial Tbn matrix and rotate Mag measurements into NED
- // to set initial NED magnetic field states
- quat2Tbn(Tbn, initQuat);
- Tnb = Tbn.transpose();
- Vector3f initMagNED;
- initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
- initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
- initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
-
- magstate.q0 = initQuat[0];
- magstate.q1 = initQuat[1];
- magstate.q2 = initQuat[2];
- magstate.q3 = initQuat[3];
- magstate.magN = initMagNED.x;
- magstate.magE = initMagNED.y;
- magstate.magD = initMagNED.z;
- magstate.magXbias = magBias.x;
- magstate.magYbias = magBias.y;
- magstate.magZbias = magBias.z;
- magstate.R_MAG = sq(magMeasurementSigma);
- magstate.DCM = Tbn;
-
- // write to state vector
- for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
- for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
- // positions:
- states[7] = posNE[0];
- states[8] = posNE[1];
- states[9] = -hgtMea;
- for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
- states[16] = initMagNED.x; // Magnetic Field North
- states[17] = initMagNED.y; // Magnetic Field East
- states[18] = initMagNED.z; // Magnetic Field Down
- states[19] = magBias.x; // Magnetic Field Bias X
- states[20] = magBias.y; // Magnetic Field Bias Y
- states[21] = magBias.z; // Magnetic Field Bias Z
-
- ResetVelocity();
- ResetPosition();
- ResetHeight();
- ResetStoredStates();
-
- // initialise focal length scale factor estimator states
- flowStates[0] = 1.0f;
-
- statesInitialised = true;
-
- // initialise the covariance matrix
- CovarianceInit();
-
- //Define Earth rotation vector in the NED navigation frame
- calcEarthRateNED(earthRateNED, latRef);
-}
-
-void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
-{
- // store initial lat,long and height
- latRef = referenceLat;
- lonRef = referenceLon;
- hgtRef = referenceHgt;
- refSet = true;
-
- // we are at reference position, so measurement must be zero
- posNE[0] = 0.0f;
- posNE[1] = 0.0f;
-
- // we are at an unknown, possibly non-zero altitude - so altitude
- // is not reset (hgtMea)
-
- // the baro offset must be this difference now
- baroHgtOffset = baroHgt - referenceHgt;
-
- InitializeDynamic(initvelNED, declination);
-}
-
-void AttPosEKF::ZeroVariables()
-{
-
- // Initialize on-init initialized variables
- dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
- dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
- dtGpsFilt = 1.0f / 5.0f;
- dtHgtFilt = 1.0f / 100.0f;
- storeIndex = 0;
-
- lastVelPosFusion = millis();
-
- // Do the data structure init
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
- KH[i][j] = 0.0f; // intermediate result used for covariance updates
- KHP[i][j] = 0.0f; // intermediate result used for covariance updates
- P[i][j] = 0.0f; // covariance matrix
- }
-
- Kfusion[i] = 0.0f; // Kalman gains
- states[i] = 0.0f; // state matrix
- }
-
- // initialise the variables for the focal length scale factor to unity
- flowStates[0] = 1.0f;
-
- correctedDelAng.zero();
- summedDelAng.zero();
- summedDelVel.zero();
- prevDelAng.zero();
- dAngIMU.zero();
- dVelIMU.zero();
- lastGyroOffset.zero();
-
- // initialise states used by OpticalFlowEKF
- flowStates[0] = 1.0f;
- flowStates[1] = 0.0f;
-
- for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; i++) {
-
- for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
- storedStates[j][i] = 0.0f;
- }
-
- statetimeStamp[i] = 0;
- }
-
- memset(&magstate, 0, sizeof(magstate));
- magstate.q0 = 1.0f;
- magstate.DCM.identity();
-
- memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
-}
-
-void AttPosEKF::GetFilterState(struct ekf_status_report *err)
-{
-
- // Copy states
- for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
- current_ekf_state.states[i] = states[i];
- }
- current_ekf_state.n_states = EKF_STATE_ESTIMATES;
- current_ekf_state.onGround = _onGround;
- current_ekf_state.staticMode = staticMode;
- current_ekf_state.useCompass = useCompass;
- current_ekf_state.useAirspeed = useAirspeed;
-
- memcpy(err, ¤t_ekf_state, sizeof(*err));
-}
-
-void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
-{
- memcpy(last_error, &last_ekf_error, sizeof(*last_error));
- memset(&last_ekf_error, 0, sizeof(last_ekf_error));
-}
-
-void AttPosEKF::setIsFixedWing(const bool fixedWing)
-{
- _isFixedWing = fixedWing;
-}
-
-void AttPosEKF::get_covariance(float c[EKF_STATE_ESTIMATES])
-{
- for (unsigned int i = 0; i < EKF_STATE_ESTIMATES; i++) {
- c[i] = P[i][i];
- }
-}
diff --git a/src/examples/ekf_att_pos_estimator/estimator_22states.h b/src/examples/ekf_att_pos_estimator/estimator_22states.h
deleted file mode 100644
index c0a51afc0a..0000000000
--- a/src/examples/ekf_att_pos_estimator/estimator_22states.h
+++ /dev/null
@@ -1,424 +0,0 @@
-/****************************************************************************
-* Copyright (c) 2014, Paul Riseborough All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* Redistributions of source code must retain the above copyright notice, this
-* list of conditions and the following disclaimer.
-*
-* Redistributions in binary form must reproduce the above copyright notice,
-* this list of conditions and the following disclaimer in the documentation
-* and/or other materials provided with the distribution.
-*
-* Neither the name of the {organization} nor the names of its contributors
-* may be used to endorse or promote products derived from this software without
-* specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-****************************************************************************/
-
-/**
- * @file estimator_22states.h
- *
- * Definition of the attitude and position estimator.
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-#pragma once
-
-#include "estimator_utilities.h"
-#include <cstddef>
-
-constexpr size_t EKF_STATE_ESTIMATES = 22;
-constexpr size_t EKF_DATA_BUFFER_SIZE = 50;
-
-class AttPosEKF {
-
-public:
-
- AttPosEKF();
- ~AttPosEKF();
-
-
-
- /* ##############################################
- *
- * M A I N F I L T E R P A R A M E T E R S
- *
- * ########################################### */
-
- /*
- * parameters are defined here and initialised in
- * the InitialiseParameters()
- */
-
- float covTimeStepMax; // maximum time allowed between covariance predictions
- float covDelAngMax; // maximum delta angle between covariance predictions
- float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-
- float yawVarScale;
- float windVelSigma;
- float dAngBiasSigma;
- float dVelBiasSigma;
- float magEarthSigma;
- float magBodySigma;
- float gndHgtSigma;
-
- float vneSigma;
- float vdSigma;
- float posNeSigma;
- float posDSigma;
- float magMeasurementSigma;
- float airspeedMeasurementSigma;
-
- float gyroProcessNoise;
- float accelProcessNoise;
-
- float EAS2TAS; // ratio f true to equivalent airspeed
-
- struct mag_state_struct {
- unsigned obsIndex;
- float MagPred[3];
- float SH_MAG[9];
- float q0;
- float q1;
- float q2;
- float q3;
- float magN;
- float magE;
- float magD;
- float magXbias;
- float magYbias;
- float magZbias;
- float R_MAG;
- Mat3f DCM;
- };
-
- struct mag_state_struct magstate;
- struct mag_state_struct resetMagState;
-
-
-
-
- // Global variables
- float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
- float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
- float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix
- float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains
- float states[EKF_STATE_ESTIMATES]; // state matrix
- float resetStates[EKF_STATE_ESTIMATES];
- float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps
- uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored
-
- // Times
- uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
-
- float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
- float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
- float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement
- float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time
- float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
- float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
- float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time
- float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
-
- Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
- Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
- Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
- Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
- Vector3f prevDelAng; ///< previous delta angle, used for coning correction
- float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
- Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
- Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
- Vector3f lastGyroOffset; // Last gyro offset
- Vector3f delAngTotal;
-
- Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
- Mat3f Tnb; // transformation amtrix from NED to body coordinates
-
- Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
- Vector3f dVelIMU;
- Vector3f dAngIMU;
- float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
- float dtIMUfilt; // average time between IMU measurements (sec)
- float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
- float dtVelPosFilt; // average time between position / velocity fusion steps
- float dtHgtFilt; // average time between height measurement updates
- float dtGpsFilt; // average time between gps measurement updates
- uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
- float innovVelPos[6]; // innovation output
- float varInnovVelPos[6]; // innovation variance output
-
- float velNED[3]; // North, East, Down velocity obs (m/s)
- float posNE[2]; // North, East position obs (m)
- float hgtMea; // measured height (m)
- float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
- float rngMea; // Ground distance
-
- float innovMag[3]; // innovation output
- float varInnovMag[3]; // innovation variance output
- Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
- float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
- float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
- float innovVtas; // innovation output
- float innovRng; ///< Range finder innovation
- float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
- float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
- float varInnovVtas; // innovation variance output
- float varInnovRng; // range finder innovation variance
- float VtasMeas; // true airspeed measurement (m/s)
- float magDeclination; ///< magnetic declination
- double latRef; // WGS-84 latitude of reference point (rad)
- double lonRef; // WGS-84 longitude of reference point (rad)
- float hgtRef; // WGS-84 height of reference point (m)
- bool refSet; ///< flag to indicate if the reference position has been set
- Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
- unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
- uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
- uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
-
- // GPS input data variables
- double gpsLat;
- double gpsLon;
- float gpsHgt;
- uint8_t GPSstatus;
-
- // Baro input
- float baroHgt;
-
- bool statesInitialised;
-
- bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
- bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
- bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
- bool fuseMagData; // boolean true when magnetometer data is to be fused
- bool fuseVtasData; // boolean true when airspeed data is to be fused
- bool fuseRngData; ///< true when range data is fused
- bool fuseOptFlowData; // true when optical flow data is fused
-
- bool inhibitWindStates; // true when wind states and covariances are to remain constant
- bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
- bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
- bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
-
- bool staticMode; ///< boolean true if no position feedback is fused
- bool useGPS; // boolean true if GPS data is being used
- bool useAirspeed; ///< boolean true if airspeed data is being used
- bool useCompass; ///< boolean true if magnetometer data is being used
- bool useRangeFinder; ///< true when rangefinder is being used
- bool useOpticalFlow; // true when optical flow data is being used
-
- bool ekfDiverged;
- uint64_t lastReset;
-
- struct ekf_status_report current_ekf_state;
- struct ekf_status_report last_ekf_error;
-
- bool numericalProtection;
-
- unsigned storeIndex;
-
- // Optical Flow error estimation
- float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
-
- // Two state EKF used to estimate focal length scale factor and terrain position
- float Popt[2][2]; // state covariance matrix
- float flowStates[2]; // flow states [scale factor, terrain position]
- float prevPosN; // north position at last measurement
- float prevPosE; // east position at last measurement
- float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
- float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
- float fScaleFactorVar; // optical flow sensor focal length scale factor variance
- Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
- float R_LOS; // Optical flow observation noise variance (rad/sec)^2
- float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
- float auxRngTestRatio; // ratio of range observation innovations to fault threshold
- float flowInnovGate; // number of standard deviations used for the innovation consistency check
- float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
- float rngInnovGate; // number of standard deviations used for the innovation consistency check
- float minFlowRng; // minimum range over which to fuse optical flow measurements
- float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
-
- void updateDtGpsFilt(float dt);
-
- void updateDtHgtFilt(float dt);
-
- void UpdateStrapdownEquationsNED();
-
- void CovariancePrediction(float dt);
-
- void FuseVelposNED();
-
- void FuseMagnetometer();
-
- void FuseAirspeed();
-
- void FuseOptFlow();
-
- /**
- * @brief
- * Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
- * This fiter requires optical flow rates that are not motion compensated
- * Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
- **/
- void OpticalFlowEKF();
-
- void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
-
- void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
-
- void quatNorm(float (&quatOut)[4], const float quatIn[4]);
-
- // store staes along with system time stamp in msces
- void StoreStates(uint64_t timestamp_ms);
-
- /**
- * Recall the state vector.
- *
- * Recalls the vector stored at closest time to the one specified by msec
- * @return zero on success, integer indicating the number of invalid states on failure.
- * Does only copy valid states, if the statesForFusion vector was initialized
- * correctly by the caller, the result can be safely used, but is a mixture
- * time-wise where valid states were updated and invalid remained at the old
- * value.
- */
- int RecallStates(float *statesForFusion, uint64_t msec);
-
- void RecallOmega(float *omegaForFusion, uint64_t msec);
-
- void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
-
- void calcEarthRateNED(Vector3f &omega, float latitude);
-
- static void eul2quat(float (&quat)[4], const float (&eul)[3]);
-
- static void quat2eul(float (&eul)[3], const float (&quat)[4]);
-
- //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
-
- static inline float sq(float valIn) {return valIn * valIn;}
-
- /**
- * @brief
- * Tell the EKF if the vehicle has landed
- **/
- void setOnGround(const bool isLanded);
-
- void CovarianceInit();
-
- void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
-
- float ConstrainFloat(float val, float min, float maxf);
-
- void ConstrainVariances();
-
- void ConstrainStates();
-
- void ForceSymmetry();
-
- /**
- * @brief
- * Check the filter inputs and bound its operational state
- *
- * This check will reset the filter states if required
- * due to a failure of consistency or timeout checks.
- * it should be run after the measurement data has been
- * updated, but before any of the fusion steps are
- * executed.
- */
- int CheckAndBound(struct ekf_status_report *last_error);
-
- /**
- * @brief
- * Reset the filter position states
- *
- * This resets the position to the last GPS measurement
- * or to zero in case of static position.
- */
- void ResetPosition();
-
- /**
- * @brief
- * Reset the velocity state.
- */
- void ResetVelocity();
-
- void GetFilterState(struct ekf_status_report *state);
-
- void GetLastErrorState(struct ekf_status_report *last_error);
-
- bool StatesNaN();
-
- void InitializeDynamic(float (&initvelNED)[3], float declination);
-
- /**
- * @brief
- * Tells the EKF wheter the vehicle is a fixed wing frame or not.
- * This changes the way the EKF fuses position or attitude calulations
- * by making some assumptions on movement.
- * @param fixedWing
- * true if the vehicle moves like a Fixed Wing, false otherwise
- **/
- void setIsFixedWing(const bool fixedWing);
-
- /**
- * @brief
- * Reset internal filter states and clear all variables to zero value
- */
- void ZeroVariables();
-
- void get_covariance(float c[28]);
-
- float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
-
-protected:
-
- /**
- * @brief
- * Initializes algorithm parameters to safe default values
- **/
- void InitialiseParameters();
-
- void updateDtVelPosFilt(float dt);
-
- bool FilterHealthy();
-
- bool GyroOffsetsDiverged();
-
- bool VelNEDDiverged();
-
- /**
- * @brief
- * Reset the height state.
- *
- * This resets the height state with the last altitude measurements
- */
- void ResetHeight();
-
- void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
-
- void ResetStoredStates();
-
-private:
- bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type
- bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
- float _accNavMagHorizontal; ///< First-order low-pass filtered rate of change maneuver velocity
-};
-
-uint32_t millis();
-
-uint64_t getMicros();
-
diff --git a/src/examples/ekf_att_pos_estimator/estimator_utilities.cpp b/src/examples/ekf_att_pos_estimator/estimator_utilities.cpp
deleted file mode 100644
index a8c09f11f5..0000000000
--- a/src/examples/ekf_att_pos_estimator/estimator_utilities.cpp
+++ /dev/null
@@ -1,235 +0,0 @@
-/****************************************************************************
-* Copyright (c) 2014, Paul Riseborough All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* Redistributions of source code must retain the above copyright notice, this
-* list of conditions and the following disclaimer.
-*
-* Redistributions in binary form must reproduce the above copyright notice,
-* this list of conditions and the following disclaimer in the documentation
-* and/or other materials provided with the distribution.
-*
-* Neither the name of the {organization} nor the names of its contributors
-* may be used to endorse or promote products derived from this software without
-* specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-****************************************************************************/
-
-/**
- * @file estimator_utilities.cpp
- *
- * Estimator support utilities.
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-#include "estimator_utilities.h"
-
-// Define EKF_DEBUG here to enable the debug print calls
-// if the macro is not set, these will be completely
-// optimized out by the compiler.
-//#define EKF_DEBUG
-
-#ifdef EKF_DEBUG
-#include <stdio.h>
-#include <stdarg.h>
-
-static void
-ekf_debug_print(const char *fmt, va_list args)
-{
- fprintf(stderr, "%s: ", "[ekf]");
- vfprintf(stderr, fmt, args);
-
- fprintf(stderr, "\n");
-}
-
-void
-ekf_debug(const char *fmt, ...)
-{
- va_list args;
-
- va_start(args, fmt);
- ekf_debug_print(fmt, args);
- va_end(args);
-}
-
-#else
-
-void ekf_debug(const char *fmt, ...) { while(0){} }
-#endif
-
-/* we don't want to pull in the standard lib just to swap two floats */
-void swap_var(float &d1, float &d2);
-
-float Vector3f::length() const
-{
- return sqrtf(x*x + y*y + z*z);
-}
-
-void Vector3f::zero()
-{
- x = 0.0f;
- y = 0.0f;
- z = 0.0f;
-}
-
-Mat3f::Mat3f() :
- x{1.0f, 0.0f, 0.0f},
- y{0.0f, 1.0f, 0.0f},
- z{0.0f, 0.0f, 1.0f}
-{
-}
-
-void Mat3f::identity() {
- x.x = 1.0f;
- x.y = 0.0f;
- x.z = 0.0f;
-
- y.x = 0.0f;
- y.y = 1.0f;
- y.z = 0.0f;
-
- z.x = 0.0f;
- z.y = 0.0f;
- z.z = 1.0f;
-}
-
-Mat3f Mat3f::transpose() const
-{
- Mat3f ret = *this;
- swap_var(ret.x.y, ret.y.x);
- swap_var(ret.x.z, ret.z.x);
- swap_var(ret.y.z, ret.z.y);
- return ret;
-}
-
-void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
-{
- velNEDr[0] = gpsGndSpd*cosf(gpsCourse);
- velNEDr[1] = gpsGndSpd*sinf(gpsCourse);
- velNEDr[2] = gpsVelD;
-}
-
-void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
-{
- posNEDr[0] = earthRadius * (lat - latReference);
- posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference);
- posNEDr[2] = -(hgt - hgtReference);
-}
-
-void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
-{
- lat = latRef + (double)posNEDi[0] * earthRadiusInv;
- lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef);
- hgt = hgtRef - posNEDi[2];
-}
-
-// overload + operator to provide a vector addition
-Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
-{
- Vector3f vecOut;
- vecOut.x = vecIn1.x + vecIn2.x;
- vecOut.y = vecIn1.y + vecIn2.y;
- vecOut.z = vecIn1.z + vecIn2.z;
- return vecOut;
-}
-
-// overload - operator to provide a vector subtraction
-Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2)
-{
- Vector3f vecOut;
- vecOut.x = vecIn1.x - vecIn2.x;
- vecOut.y = vecIn1.y - vecIn2.y;
- vecOut.z = vecIn1.z - vecIn2.z;
- return vecOut;
-}
-
-// overload * operator to provide a matrix vector product
-Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn)
-{
- Vector3f vecOut;
- vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
- vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
- vecOut.z = matIn.z.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
- return vecOut;
-}
-
-// overload * operator to provide a matrix product
-Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2)
-{
- Mat3f matOut;
- matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
- matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
- matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
-
- matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
- matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
- matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
-
- matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
- matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
- matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
-
- return matOut;
-}
-
-// overload % operator to provide a vector cross product
-Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2)
-{
- Vector3f vecOut;
- vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
- vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
- vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
- return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(const Vector3f &vecIn1, const float sclIn1)
-{
- Vector3f vecOut;
- vecOut.x = vecIn1.x * sclIn1;
- vecOut.y = vecIn1.y * sclIn1;
- vecOut.z = vecIn1.z * sclIn1;
- return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(float sclIn1, const Vector3f &vecIn1)
-{
- Vector3f vecOut;
- vecOut.x = vecIn1.x * sclIn1;
- vecOut.y = vecIn1.y * sclIn1;
- vecOut.z = vecIn1.z * sclIn1;
- return vecOut;
-}
-
-// overload / operator to provide a vector scalar division
-Vector3f operator/(const Vector3f &vec, const float scalar)
-{
- Vector3f vecOut;
- vecOut.x = vec.x / scalar;
- vecOut.y = vec.y / scalar;
- vecOut.z = vec.z / scalar;
- return vecOut;
-}
-
-void swap_var(float &d1, float &d2)
-{
- float tmp = d1;
- d1 = d2;
- d2 = tmp;
-}
diff --git a/src/examples/ekf_att_pos_estimator/estimator_utilities.h b/src/examples/ekf_att_pos_estimator/estimator_utilities.h
deleted file mode 100644
index 788fb85ec9..0000000000
--- a/src/examples/ekf_att_pos_estimator/estimator_utilities.h
+++ /dev/null
@@ -1,135 +0,0 @@
-/****************************************************************************
-* Copyright (c) 2014, Paul Riseborough All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* Redistributions of source code must retain the above copyright notice, this
-* list of conditions and the following disclaimer.
-*
-* Redistributions in binary form must reproduce the above copyright notice,
-* this list of conditions and the following disclaimer in the documentation
-* and/or other materials provided with the distribution.
-*
-* Neither the name of the {organization} nor the names of its contributors
-* may be used to endorse or promote products derived from this software without
-* specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-****************************************************************************/
-
-/**
- * @file estimator_utilities.h
- *
- * Estimator support utilities.
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-#pragma once
-
-#include <math.h>
-#include <stdint.h>
-
-#define GRAVITY_MSS 9.80665f
-#define deg2rad 0.017453292f
-#define rad2deg 57.295780f
-#define earthRate 0.000072921f
-#define earthRadius 6378145.0
-#define earthRadiusInv 1.5678540e-7
-
-class Vector3f
-{
-public:
- float x;
- float y;
- float z;
-
- Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) :
- x(a),
- y(b),
- z(c)
- {}
-
- float length() const;
- void zero();
-};
-
-class Mat3f
-{
-private:
-public:
- Vector3f x;
- Vector3f y;
- Vector3f z;
-
- Mat3f();
-
- void identity();
- Mat3f transpose() const;
-};
-
-Vector3f operator*(const float sclIn1, const Vector3f &vecIn1);
-Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2);
-Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2);
-Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn);
-Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2);
-Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2);
-Vector3f operator*(const Vector3f &vecIn1, const float sclIn1);
-Vector3f operator/(const Vector3f &vec, const float scalar);
-
-enum GPS_FIX {
- GPS_FIX_NOFIX = 0,
- GPS_FIX_2D = 2,
- GPS_FIX_3D = 3
-};
-
-struct ekf_status_report {
- bool error;
- bool velHealth;
- bool posHealth;
- bool hgtHealth;
- bool velTimeout;
- bool posTimeout;
- bool hgtTimeout;
- bool imuTimeout;
- bool onGround;
- bool staticMode;
- bool useCompass;
- bool useAirspeed;
- uint32_t velFailTime;
- uint32_t posFailTime;
- uint32_t hgtFailTime;
- float states[32];
- unsigned n_states;
- bool angNaN;
- bool summedDelVelNaN;
- bool KHNaN;
- bool KHPNaN;
- bool PNaN;
- bool covarianceNaN;
- bool kalmanGainsNaN;
- bool statesNaN;
- bool gyroOffsetsExcessive;
- bool covariancesExcessive;
- bool velOffsetExcessive;
-};
-
-void ekf_debug(const char *fmt, ...);
-
-void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
-
-void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference);
-
-void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
--
GitLab