consume PX4 ecl submodule and preserve all history

.github/workflows/ekf_functional_change_indicator.yml

@@ -0,0 +1,20 @@
+name: EKF Change Indicator
+
+on: pull_request
+
+jobs:
+  unit_tests:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-base-focal:2021-05-04
+    steps:
+    - uses: actions/checkout@v2.3.1
+    - name: checkout newest version of branch
+      run: |
+        git fetch origin pull/${{github.event.pull_request.number}}/head:${{github.head_ref}}
+        git checkout ${GITHUB_HEAD_REF}
+    - name: main test
+      run: cd ${GITHUB_WORKSPACE}/src/lib/ecl; make test
+      working-directory: src/lib/ecl
+    - name: Check if there is a functional change
+      run: git diff --exit-code
+      working-directory: src/lib/ecl/test/change_indication

.github/workflows/ekf_standalone_build_tests.yml

@@ -0,0 +1,56 @@
+
+name: EKF Build Tests
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - '*'
+
+jobs:
+  Linux-GCC:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-base-focal:2021-05-04
+    steps:
+    - uses: actions/checkout@v1
+    - name: main build
+      run: make
+      working-directory: src/lib/ecl
+    - name: clean build
+      run: make clean
+      working-directory: src/lib/ecl
+    - name: main test
+      run: make test
+      working-directory: src/lib/ecl
+  Linux-Clang:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-clang:2021-05-04
+    env:
+      CC: clang
+      CXX: clang++
+    steps:
+      - uses: actions/checkout@v1
+      - name: main build
+        run: make
+        working-directory: src/lib/ecl
+      - name: clean build
+        run: make clean
+        working-directory: src/lib/ecl
+      - name: main test
+        run: make test
+        working-directory: src/lib/ecl
+  Mac-OS:
+    runs-on: macos-latest
+    steps:
+      - uses: actions/checkout@v1
+      - name: main build
+        run: make
+        working-directory: src/lib/ecl
+      - name: clean build
+        run: make clean
+        working-directory: src/lib/ecl
+      - name: main test
+        run: make test
+        working-directory: src/lib/ecl

.github/workflows/ekf_update_change_indicator.yml

@@ -0,0 +1,27 @@
+name: EKF Update Change Indicator
+
+on: push
+
+jobs:
+  unit_tests:
+    runs-on: ubuntu-latest
+    container: px4io/px4-dev-base-focal:2021-05-04
+    env:
+      GIT_COMMITTER_EMAIL: bot@px4.io
+      GIT_COMMITTER_NAME: PX4BuildBot
+    steps:
+    - uses: actions/checkout@v2.3.1
+    - name: main test updates change indication files
+      run: cd ${GITHUB_WORKSPACE}/src/lib/ecl; make test
+    - name: Check if there exists diff and save result in variable
+      run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_ENV
+      working-directory: src/lib/ecl/test/change_indication
+    - name: auto-commit any changes to change indication
+      uses: stefanzweifel/git-auto-commit-action@v4
+      with:
+        commit_message: '[AUTO COMMIT] update change indication'
+        commit_user_name: ${GIT_COMMITTER_NAME}
+        commit_user_email: ${GIT_COMMITTER_EMAIL}
+    - if: ${{env.CHANGE_INDICATED}}
+      name: if there is a functional change, fail check
+      run: exit 1

.gitmodules

@@ -18,10 +18,6 @@
 	path = src/lib/matrix
 	url = https://github.com/PX4/PX4-Matrix.git
 	branch = master
-[submodule "src/lib/ecl"]
-	path = src/lib/ecl
-	url = https://github.com/PX4/PX4-ECL.git
-	branch = master
 [submodule "boards/atlflight/cmake_hexagon"]
 	path = boards/atlflight/cmake_hexagon
 	url = https://github.com/PX4/cmake_hexagon.git

src/examples/fake_magnetometer/CMakeLists.txt

@@ -41,6 +41,5 @@ px4_add_module(
 	DEPENDS
 		drivers_magnetometer
 		ecl_geo
-		git_ecl
 		px4_work_queue
 )

src/lib/CMakeLists.txt

@@ -31,7 +31,6 @@
 #
 ############################################################################
 
-px4_add_git_submodule(TARGET git_ecl PATH "ecl")
 px4_add_git_submodule(TARGET git_matrix PATH "matrix")
 px4_add_git_submodule(TARGET git_monocypher PATH "crypto/monocypher")
 

src/lib/ecl/.clang-format

@@ -0,0 +1,117 @@
+---
+Language:        Cpp
+# BasedOnStyle:  Google
+AccessModifierOffset: -8 # Modified
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Left
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Linux
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     120
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Preserve
+IncludeCategories:
+  - Regex:           '^<ext/.*\.h>'
+    Priority:        2
+  - Regex:           '^<.*\.h>'
+    Priority:        1
+  - Regex:           '^<.*'
+    Priority:        2
+  - Regex:           '.*'
+    Priority:        3
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth:     8 # Modified
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true # Modified
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: false
+PenaltyBreakAssignment: 200 # Modified
+PenaltyBreakBeforeFirstCallParameter: 20 # Modified
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+RawStringFormats:
+  - Delimiter:       pb
+    Language:        TextProto
+    BasedOnStyle:    google
+ReflowComments:  true
+SortIncludes:    true
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles:  false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard:        Auto
+TabWidth:        8 # Modified
+UseTab:          Always # Modified
+...
+

src/lib/ecl/.editorconfig

@@ -0,0 +1,5 @@
+# EditorConfig https://EditorConfig.org
+root = true
+
+[{Makefile,CMakeLists.txt}]
+indent_style = tab

src/lib/ecl/.gitignore

@@ -0,0 +1,18 @@
+*.DS_Store
+*.gcov
+*~
+*.orig
+
+.cache/
+build/
+
+*/*.a
+.ninja_deps
+.ninja_log
+build.ninja
+cmake_install.cmake
+CMakeCache.txt
+CMakeFiles
+compile_commands.json
+matrix-prefix/
+rules.ninja

src/lib/ecl/.travis.yml

@@ -0,0 +1,31 @@
+
+sudo: required
+
+services:
+  - docker
+
+language: cpp
+
+matrix:
+  fast_finish: true
+  include:
+    - env: BUILD_TARGET=coverity_scan
+      dist: trusty
+      if: branch = coverity_scan
+
+before_install:
+  - echo -n | openssl s_client -connect scan.coverity.com:443 | sed -ne '/-BEGIN CERTIFICATE-/,/-END CERTIFICATE-/p' | sudo tee -a /etc/ssl/certs/ca-
+
+script:
+  - make distclean
+  - make
+
+addons:
+  coverity_scan:
+    project:
+      name: "PX4/ecl"
+      description: "Build submitted via Travis CI"
+    notification_email: ci@px4.io
+    build_command_prepend: "make distclean"
+    build_command: "make"
+    branch_pattern: coverity_scan

src/lib/ecl/AlphaFilter/AlphaFilter.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 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 AlphaFilter.hpp
+ *
+ * @brief First order "alpha" IIR digital filter also known as leaky integrator or forgetting average.
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <float.h>
+
+template <typename T>
+class AlphaFilter
+{
+public:
+	AlphaFilter() = default;
+	explicit AlphaFilter(float alpha) : _alpha(alpha) {}
+
+	~AlphaFilter() = default;
+
+	/**
+	 * Set filter parameters for time abstraction
+	 *
+	 * Both parameters have to be provided in the same units.
+	 *
+	 * @param sample_interval interval between two samples
+	 * @param time_constant filter time constant determining convergence
+	 */
+	void setParameters(float sample_interval, float time_constant)
+	{
+		const float denominator = time_constant + sample_interval;
+
+		if (denominator > FLT_EPSILON) {
+			setAlpha(sample_interval / denominator);
+		}
+	}
+
+	/**
+	 * Set filter parameter alpha directly without time abstraction
+	 *
+	 * @param alpha [0,1] filter weight for the previous state. High value - long time constant.
+	 */
+	void setAlpha(float alpha) { _alpha = alpha; }
+
+	/**
+	 * Set filter state to an initial value
+	 *
+	 * @param sample new initial value
+	 */
+	void reset(const T &sample) { _filter_state = sample; }
+
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	const T &update(const T &sample)
+	{
+		_filter_state = updateCalculation(sample);
+		return _filter_state;
+	}
+
+	const T &getState() const { return _filter_state; }
+
+protected:
+	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
+
+	float _alpha{0.f};
+	T _filter_state{};
+};

src/lib/ecl/CMakeLists.txt

@@ -0,0 +1,243 @@
+############################################################################
+#
+#   Copyright (c) 2015-2018 ECL 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 ECL 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.
+#
+############################################################################
+
+cmake_minimum_required(VERSION 3.0)
+
+project(ECL CXX)
+
+if(NOT CMAKE_BUILD_TYPE)
+	# force debug builds if we test ECL as standalone
+	if(BUILD_TESTING AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
+		set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE)
+	else()
+		set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
+	endif()
+endif()
+message(STATUS "build type is ${CMAKE_BUILD_TYPE}")
+
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage")
+
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git)
+	execute_process(
+		COMMAND git describe --always --tags
+		OUTPUT_VARIABLE git_tag
+		OUTPUT_STRIP_TRAILING_WHITESPACE
+		WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+		)
+endif()
+
+message(STATUS "PX4 ECL: Very lightweight Estimation & Control Library ${git_tag}")
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# code coverage support
+option(COV_HTML "Display html for coverage" OFF)
+option(ECL_ASAN "Enable ECL address sanitizer" OFF)
+
+if (("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") OR ("${CMAKE_CXX_COMPILER_ID}" MATCHES "AppleClang"))
+	set(CMAKE_CXX_FLAGS_COVERAGE
+		"--coverage -ftest-coverage -fdiagnostics-absolute-paths -O0 -fprofile-arcs -fno-inline-functions -fno-elide-constructors"
+		CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
+	set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
+		"-ftest-coverage -fdiagnostics-absolute-paths"
+		CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
+else()
+	set(CMAKE_CXX_FLAGS_COVERAGE
+		# Bring back -fprofile-abs-path when GCC 9 is available on Ubuntu LTS
+		"--coverage -fprofile-arcs -ftest-coverage -O0 -fno-default-inline -fno-inline -fno-inline-small-functions -fno-elide-constructors"
+		CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
+	set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
+		"--coverage -ftest-coverage -lgcov"
+		CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
+endif()
+
+mark_as_advanced(CMAKE_CXX_FLAGS_COVERAGE CMAKE_C_FLAGS_COVERAGE CMAKE_EXE_LINKER_FLAGS_COVERAGE)
+
+
+set(ECL_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR} CACHE STRING "ECL source location" FORCE)
+
+if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
+	# ECL standalone build
+	add_definitions(-DECL_STANDALONE)
+	set(ECL_STANDALONE 1)
+
+	add_custom_target(prebuild_targets)
+
+	if(MSVC)
+		add_compile_options(
+			/W4
+			/WX
+
+			/D_USE_MATH_DEFINES
+
+			/wd4100 # warning C4100: unreferenced formal parameter
+			/wd4244 # warning C4244: '=': conversion from 'double' to 'int32_t', possible loss of data
+			)
+	elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
+		add_compile_options(
+			-pedantic
+
+			-Wall
+			-Wextra
+			-Werror
+
+			-Wno-missing-field-initializers # ignore for GCC 4.8 support
+		)
+	endif()
+
+	if(BUILD_TESTING)
+
+		include(CTest)
+
+		option(ECL_TESTS "Build ECL tests" ON)
+
+		add_custom_target(check
+			COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -C Debug
+			DEPENDS ecl_EKF
+			USES_TERMINAL
+		)
+	endif()
+
+	# fetch latest matrix from github
+	include(ExternalProject)
+	ExternalProject_Add(matrix
+			GIT_REPOSITORY "https://github.com/PX4/Matrix.git"
+			GIT_TAG f981cea2aebfc9cfd930dce73ba6d4d6681e99c1
+			UPDATE_COMMAND ""
+			PATCH_COMMAND ""
+			CONFIGURE_COMMAND ""
+			BUILD_COMMAND ""
+			INSTALL_COMMAND ""
+			)
+	add_dependencies(prebuild_targets matrix)
+	include_directories(${CMAKE_BINARY_DIR}/matrix-prefix/src/matrix)
+
+endif()
+
+if(NOT (${CMAKE_BUILD_TYPE} MATCHES "Coverage") AND NOT (${CMAKE_BUILD_TYPE} MATCHES "Debug"))
+	if(MAX_CUSTOM_OPT_LEVEL)
+		add_compile_options(${MAX_CUSTOM_OPT_LEVEL})
+	endif()
+endif()
+
+# santiziers (ASAN)
+if(ECL_ASAN)
+	message(STATUS "ecl address sanitizer enabled ")
+
+	# environment variables
+	#  ASAN_OPTIONS=detect_stack_use_after_return=1
+	#  ASAN_OPTIONS=check_initialization_order=1
+
+	add_compile_options(
+		-fsanitize=address
+		-g3
+		-O1
+		-fno-omit-frame-pointer
+	)
+
+	set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address)
+endif()
+
+add_subdirectory(airdata)
+add_subdirectory(EKF)
+add_subdirectory(geo)
+add_subdirectory(geo_lookup)
+
+if(BUILD_TESTING AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
+	add_subdirectory(test)
+endif()
+
+#=============================================================================
+# Coverage
+#
+if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage")
+
+	add_custom_target(coverage
+		COMMAND ${CMAKE_CTEST_COMMAND}
+		COMMAND lcov --capture --directory . --output-file coverage.info
+		COMMAND lcov --remove coverage.info  --output-file coverage.info '/usr/*' '${CMAKE_BINARY_DIR}/*' # filter out system
+		COMMAND lcov --summary coverage.info
+		WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+		DEPENDS check
+	)
+
+	add_custom_target(coverage_html
+		COMMAND genhtml coverage.info --output-directory out
+		WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+		DEPENDS coverage
+	)
+
+	add_custom_target(coverage_html_view
+		COMMAND x-www-browser out/index.html
+		WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+		DEPENDS coverage_html
+	)
+
+endif()
+
+#=============================================================================
+# Doxygen
+#
+# Only in standalone build
+if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
+
+	option(BUILD_DOXYGEN "Build doxygen documentation" OFF)
+
+	if (BUILD_DOXYGEN)
+		find_package(Doxygen)
+		if (DOXYGEN_FOUND)
+			# set input and output files
+			set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/docs/Doxyfile.in)
+			set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile)
+
+			# request to configure the file
+			configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY)
+
+			# note the option ALL which allows to build the docs together with the application
+			add_custom_target(doxygen ALL
+				COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT}
+				WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+				COMMENT "Generating documentation with Doxygen"
+				DEPENDS
+				VERBATIM
+				USES_TERMINAL
+			)
+
+		else()
+			message(FATAL_ERROR "Doxygen needs to be installed to generate documentation")
+		endif()
+	endif()
+endif()
+

src/lib/ecl/CONTRIBUTING.md

@@ -0,0 +1,36 @@
+# Contribution Guidelines
+
+## Auto-Formatting
+ECL uses clang-format to auto-format the code. Currently it is using the clang-format-6.0.
+The enforced style is based on the google style. Google's [Style Guide](https://google.github.io/styleguide/cppguide.html) is the place to look for advice.
+The format is not enforced on all files. The list of files on which the auto-format checks are run on can be found in `tools/format.sh`
+
+On Ubuntu (tested on 18.04) the following command can be used to check if the code is complying with the format requirements
+```
+make check_format
+```
+To auto-format the code run
+```
+make format
+```
+
+## Continuous Integration
+There are multiple checks run on a submitted PR:
+
+| Test  | Description |
+| ------------- | ------------- |
+| - **Build tests**         | Checks if the submitted code is building on various platforms. |
+| - **Unit tests**          | Run checks if the code is satisfying test cases in `tests/` and report code coverage. |
+| - **Format checks**       | Check if the files specified in `/tools/format.sh` match the style specified in `.clang-format`. Run [auto-formatting](#Auto-Formatting) |
+| - **Firmware build tests**| Pulls PX4/Firmware and checks if the current branch of ECL compiles with it. It tries to checkout a branch in PX4/Firmware with the name matching the current ECL branch name. If a branch with that name does not exist then it will checkout master. |
+
+## Unit tests
+# How to run the tests
+The test can be executed by running:
+```
+make test
+```
+
+# How to add a test
+tbd
+

src/lib/ecl/EKF/CMakeLists.txt

@@ -0,0 +1,63 @@
+############################################################################
+#
+#   Copyright (c) 2015-2018 ECL 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 ECL 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.
+#
+############################################################################
+
+add_library(ecl_EKF
+	airspeed_fusion.cpp
+	control.cpp
+	mag_control.cpp
+	covariance.cpp
+	drag_fusion.cpp
+	ekf.cpp
+	ekf_helper.cpp
+	estimator_interface.cpp
+	gps_checks.cpp
+	mag_fusion.cpp
+	optflow_fusion.cpp
+	sideslip_fusion.cpp
+	terrain_estimator.cpp
+	vel_pos_fusion.cpp
+	gps_yaw_fusion.cpp
+	imu_down_sampler.cpp
+	EKFGSF_yaw.cpp
+	sensor_range_finder.cpp
+	utils.cpp
+)
+
+add_dependencies(ecl_EKF prebuild_targets)
+target_compile_definitions(ecl_EKF PRIVATE -DMODULE_NAME="ecl/EKF")
+target_include_directories(ecl_EKF PUBLIC ${ECL_SOURCE_DIR})
+target_link_libraries(ecl_EKF PRIVATE ecl_geo ecl_geo_lookup)
+
+set_target_properties(ecl_EKF PROPERTIES PUBLIC_HEADER "ekf.h")
+
+target_compile_options(ecl_EKF PRIVATE -fno-associative-math)

src/lib/ecl/EKF/EKFGSF_yaw.cpp

@@ -0,0 +1,524 @@
+#include "EKFGSF_yaw.h"
+#include <cstdlib>
+
+EKFGSF_yaw::EKFGSF_yaw()
+{
+	// this flag must be false when we start
+	_ahrs_ekf_gsf_tilt_aligned = false;
+
+	// these objects are initialised in initialise() before being used internally, but can be reported for logging before then
+	memset(&_ahrs_ekf_gsf, 0, sizeof(_ahrs_ekf_gsf));
+	memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
+	_gsf_yaw = 0.0f;
+	_ahrs_accel.zero();
+}
+
+void EKFGSF_yaw::update(const imuSample& imu_sample,
+			bool run_EKF,			// set to true when flying or movement is suitable for yaw estimation
+			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
+			const Vector3f &imu_gyro_bias)  // estimated rate gyro bias (rad/sec)
+{
+	// copy to class variables
+	_delta_ang = imu_sample.delta_ang;
+	_delta_vel = imu_sample.delta_vel;
+	_delta_ang_dt = imu_sample.delta_ang_dt;
+	_delta_vel_dt = imu_sample.delta_vel_dt;
+	_run_ekf_gsf = run_EKF;
+	_true_airspeed = airspeed;
+
+	// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
+	const float filter_coef = fminf(10.0f * _delta_vel_dt * _tilt_gain, 1.0f);
+	const Vector3f accel = _delta_vel / fmaxf(_delta_vel_dt, 0.001f);
+	_ahrs_accel = _ahrs_accel * (1.0f - filter_coef) + accel * filter_coef;
+
+	// Initialise states first time
+	if (!_ahrs_ekf_gsf_tilt_aligned) {
+		// check for excessive acceleration to reduce likelihood of large initial roll/pitch errors
+		// due to vehicle movement
+		const float accel_norm_sq = accel.norm_squared();
+		const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
+		const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
+		const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
+		if (ok_to_align) {
+			initialiseEKFGSF();
+			ahrsAlignTilt();
+			_ahrs_ekf_gsf_tilt_aligned = true;
+		}
+		return;
+	}
+
+	// calculate common values used by the AHRS complementary filter models
+	_ahrs_accel_norm = _ahrs_accel.norm();
+
+	// AHRS prediction cycle for each model - this always runs
+	_ahrs_accel_fusion_gain = ahrsCalcAccelGain();
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+		predictEKF(model_index);
+	}
+
+	// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
+	if (_run_ekf_gsf && _vel_data_updated) {
+		if (!_ekf_gsf_vel_fuse_started) {
+			initialiseEKFGSF();
+			ahrsAlignYaw();
+			// Initialise to gyro bias estimate from main filter because there could be a large
+			// uncorrected rate gyro bias error about the gravity vector
+			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+				_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
+			}
+			_ekf_gsf_vel_fuse_started = true;
+		} else {
+			bool bad_update = false;
+			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+				// subsequent measurements are fused as direct state observations
+				if (!updateEKF(model_index)) {
+					bad_update = true;
+				}
+			}
+
+			if (!bad_update) {
+				float total_weight = 0.0f;
+				// calculate weighting for each model assuming a normal distribution
+				const float min_weight = 1e-5f;
+				uint8_t n_weight_clips = 0;
+				for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+					_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
+					if (_model_weights(model_index) < min_weight) {
+						n_weight_clips++;
+						_model_weights(model_index) = min_weight;
+					}
+					total_weight += _model_weights(model_index);
+				}
+
+				// normalise the weighting function
+				if (n_weight_clips < N_MODELS_EKFGSF) {
+					_model_weights /= total_weight;
+				} else {
+					// all weights have collapsed due to excessive innovation variances so reset filters
+					initialiseEKFGSF();
+				}
+			}
+		}
+	} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
+		// wait to fly again
+		_ekf_gsf_vel_fuse_started = false;
+	}
+
+	// Calculate a composite yaw vector as a weighted average of the states for each model.
+	// To avoid issues with angle wrapping, the yaw state is converted to a vector with length
+	// equal to the weighting value before it is summed.
+	Vector2f yaw_vector;
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+		yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2));
+		yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2));
+	}
+	_gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0));
+
+	// calculate a composite variance for the yaw state from a weighted average of the variance for each model
+	// models with larger innovations are weighted less
+	_gsf_yaw_variance = 0.0f;
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+		const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
+		_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
+	}
+
+	// prevent the same velocity data being used more than once
+	_vel_data_updated = false;
+}
+
+void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
+{
+	// generate attitude solution using simple complementary filter for the selected model
+
+	const Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias;
+
+	const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose();
+	const Vector3f gravity_direction_bf = R_to_body.col(2);
+
+	// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
+	// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
+	Vector3f tilt_correction;
+	if (_ahrs_accel_fusion_gain > 0.0f) {
+
+		Vector3f accel = _ahrs_accel;
+
+		if (_true_airspeed > FLT_EPSILON) {
+			// Calculate body frame centripetal acceleration with assumption X axis is aligned with the airspeed vector
+			// Use cross product of body rate and body frame airspeed vector
+			const Vector3f centripetal_accel_bf = Vector3f(0.0f, _true_airspeed * ang_rate(2), - _true_airspeed * ang_rate(1));
+
+			// correct measured accel for centripetal acceleration
+			accel -= centripetal_accel_bf;
+		}
+
+		tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
+
+	}
+
+	// Gyro bias estimation
+	constexpr float gyro_bias_limit = 0.05f;
+	const float spinRate = ang_rate.length();
+	if (spinRate < 0.175f) {
+		_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
+		_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit, gyro_bias_limit);
+	}
+
+	// delta angle from previous to current frame
+	const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt;
+
+	// Apply delta angle to rotation matrix
+	_ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);
+
+}
+
+void EKFGSF_yaw::ahrsAlignTilt()
+{
+	// Rotation matrix is constructed directly from acceleration measurement and will be the same for
+	// all models so only need to calculate it once. Assumptions are:
+	// 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences.
+	// 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity.
+
+	// Calculate earth frame Down axis unit vector rotated into body frame
+	const Vector3f down_in_bf = -_delta_vel.normalized();
+
+	// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
+	const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
+	Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf));
+	north_in_bf.normalize();
+
+	// Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf'
+	const Vector3f east_in_bf = down_in_bf % north_in_bf;
+
+	// Each column in a rotation matrix from earth frame to body frame represents the projection of the
+	// corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column.
+	// We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body
+	// frame are copied into corresponding rows instead.
+	Dcmf R;
+	R.setRow(0, north_in_bf);
+	R.setRow(1, east_in_bf);
+	R.setRow(2, down_in_bf);
+
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+		_ahrs_ekf_gsf[model_index].R = R;
+	}
+}
+
+void EKFGSF_yaw::ahrsAlignYaw()
+{
+	// Align yaw angle for each model
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+		Dcmf& R = _ahrs_ekf_gsf[model_index].R;
+		const float yaw = wrap_pi(_ekf_gsf[model_index].X(2));
+		R = updateYawInRotMat(yaw, R);
+
+		_ahrs_ekf_gsf[model_index].aligned = true;
+	}
+}
+
+void EKFGSF_yaw::predictEKF(const uint8_t model_index)
+{
+	// generate an attitude reference using IMU data
+	ahrsPredict(model_index);
+
+	// we don't start running the EKF part of the algorithm until there are regular velocity observations
+	if (!_ekf_gsf_vel_fuse_started) {
+		return;
+	}
+
+	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
+	const Dcmf& R = _ahrs_ekf_gsf[model_index].R;
+	_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
+					getEuler321Yaw(R) :
+					getEuler312Yaw(R);
+
+	// calculate delta velocity in a horizontal front-right frame
+	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
+	const float cos_yaw = cosf(_ekf_gsf[model_index].X(2));
+	const float sin_yaw = sinf(_ekf_gsf[model_index].X(2));
+	const float dvx =   del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;
+	const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw;
+
+	// sum delta velocities in earth frame:
+	_ekf_gsf[model_index].X(0) += del_vel_NED(0);
+	_ekf_gsf[model_index].X(1) += del_vel_NED(1);
+
+	// predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py
+
+	// Local short variable name copies required for readability
+	const float &P00 = _ekf_gsf[model_index].P(0,0);
+	const float &P01 = _ekf_gsf[model_index].P(0,1);
+	const float &P02 = _ekf_gsf[model_index].P(0,2);
+	const float &P11 = _ekf_gsf[model_index].P(1,1);
+	const float &P12 = _ekf_gsf[model_index].P(1,2);
+	const float &P22 = _ekf_gsf[model_index].P(2,2);
+	const float &psi = _ekf_gsf[model_index].X(2);
+
+	// Use fixed values for delta velocity and delta angle process noise variances
+	const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
+	const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2
+	const float dazVar = sq(_gyro_noise * _delta_ang_dt); // variance of yaw delta angle - rad^2
+
+	// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
+	const float S0 = cosf(psi);
+	const float S1 = ecl::powf(S0, 2);
+	const float S2 = sinf(psi);
+	const float S3 = ecl::powf(S2, 2);
+	const float S4 = S0*dvy + S2*dvx;
+	const float S5 = P02 - P22*S4;
+	const float S6 = S0*dvx - S2*dvy;
+	const float S7 = S0*S2;
+	const float S8 = P01 + S7*dvxVar - S7*dvyVar;
+	const float S9 = P12 + P22*S6;
+
+	_ekf_gsf[model_index].P(0,0) = P00 - P02*S4 + S1*dvxVar + S3*dvyVar - S4*S5;
+	_ekf_gsf[model_index].P(0,1) = -P12*S4 + S5*S6 + S8;
+	_ekf_gsf[model_index].P(1,1) = P11 + P12*S6 + S1*dvyVar + S3*dvxVar + S6*S9;
+	_ekf_gsf[model_index].P(0,2) = S5;
+	_ekf_gsf[model_index].P(1,2) = S9;
+	_ekf_gsf[model_index].P(2,2) = P22 + dazVar;
+
+	// covariance matrix is symmetrical, so copy upper half to lower half
+	_ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1);
+	_ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2);
+	_ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2);
+
+	// constrain variances
+	const float min_var = 1e-6f;
+	for (unsigned index = 0; index < 3; index++) {
+		_ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);
+	}
+}
+
+// Update EKF states and covariance for specified model index using velocity measurement
+bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
+{
+	// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
+	const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f));
+
+	// calculate velocity observation innovations
+	_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);
+	_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);
+
+	// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
+	const float &P00 = _ekf_gsf[model_index].P(0,0);
+	const float &P01 = _ekf_gsf[model_index].P(0,1);
+	const float &P02 = _ekf_gsf[model_index].P(0,2);
+	const float &P11 = _ekf_gsf[model_index].P(1,1);
+	const float &P12 = _ekf_gsf[model_index].P(1,2);
+	const float &P22 = _ekf_gsf[model_index].P(2,2);
+
+	// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
+	const float t0 = ecl::powf(P01, 2);
+	const float t1 = -t0;
+	const float t2 = P00*P11 + P00*velObsVar + P11*velObsVar + t1 + ecl::powf(velObsVar, 2);
+	if (fabsf(t2) < 1e-6f) {
+		return false;
+	}
+	const float t3 = 1.0F/t2;
+	const float t4 = P11 + velObsVar;
+	const float t5 = P01*t3;
+	const float t6 = -t5;
+	const float t7 = P00 + velObsVar;
+	const float t8 = P00*t4 + t1;
+	const float t9 = t5*velObsVar;
+	const float t10 = P11*t7;
+	const float t11 = t1 + t10;
+	const float t12 = P01*P12;
+	const float t13 = P02*t4;
+	const float t14 = P01*P02;
+	const float t15 = P12*t7;
+	const float t16 = t0*velObsVar;
+	const float t17 = ecl::powf(t2, -2);
+	const float t18 = t4*velObsVar + t8;
+	const float t19 = t17*t18;
+	const float t20 = t17*(t16 + t7*t8);
+	const float t21 = t0 - t10;
+	const float t22 = t17*t21;
+	const float t23 = t14 - t15;
+	const float t24 = P01*t23;
+	const float t25 = t12 - t13;
+	const float t26 = t16 - t21*t4;
+	const float t27 = t17*t26;
+	const float t28 = t11 + t7*velObsVar;
+	const float t30 = t17*t28;
+	const float t31 = P01*t25;
+	const float t32 = t23*t4 + t31;
+	const float t33 = t17*t32;
+	const float t35 = t24 + t25*t7;
+	const float t36 = t17*t35;
+
+	_ekf_gsf[model_index].S_det_inverse = t3;
+
+	_ekf_gsf[model_index].S_inverse(0,0) = t3*t4;
+	_ekf_gsf[model_index].S_inverse(0,1) = t6;
+	_ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
+	_ekf_gsf[model_index].S_inverse(1,0) = _ekf_gsf[model_index].S_inverse(0,1);
+
+	matrix::Matrix<float, 3, 2> K;
+	K(0,0) = t3*t8;
+	K(1,0) = t9;
+	K(2,0) = t3*(-t12 + t13);
+	K(0,1) = t9;
+	K(1,1) = t11*t3;
+	K(2,1) = t3*(-t14 + t15);
+
+	_ekf_gsf[model_index].P(0,0) = P00 - t16*t19 - t20*t8;
+	_ekf_gsf[model_index].P(0,1) = P01*(t18*t22 - t20*velObsVar + 1);
+	_ekf_gsf[model_index].P(1,1) = P11 - t16*t30 + t22*t26;
+	_ekf_gsf[model_index].P(0,2) = P02 + t19*t24 + t20*t25;
+	_ekf_gsf[model_index].P(1,2) = P12 + t23*t27 + t30*t31;
+	_ekf_gsf[model_index].P(2,2) = P22 - t23*t33 - t25*t36;
+	_ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1);
+	_ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2);
+	_ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2);
+
+	// constrain variances
+	const float min_var = 1e-6f;
+	for (unsigned index = 0; index < 3; index++) {
+		_ekf_gsf[model_index].P(index,index) = fmaxf(_ekf_gsf[model_index].P(index,index),min_var);
+	}
+
+	// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
+	const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
+
+	// Perform a chi-square innovation consistency test and calculate a compression scale factor
+	// that limits the magnitude of innovations to 5-sigma
+	// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
+	// This protects from large measurement spikes
+	const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f;
+
+	// Correct the state vector and capture the change in yaw angle
+	const float oldYaw = _ekf_gsf[model_index].X(2);
+
+	_ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;
+
+	const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;
+
+	// apply the change in yaw angle to the AHRS
+	// take advantage of sparseness in the yaw rotation matrix
+	const float cosYaw = cosf(yawDelta);
+	const float sinYaw = sinf(yawDelta);
+	const float R_prev00 = _ahrs_ekf_gsf[model_index].R(0, 0);
+	const float R_prev01 = _ahrs_ekf_gsf[model_index].R(0, 1);
+	const float R_prev02 = _ahrs_ekf_gsf[model_index].R(0, 2);
+
+	_ahrs_ekf_gsf[model_index].R(0, 0) = R_prev00 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 0) * sinYaw;
+	_ahrs_ekf_gsf[model_index].R(0, 1) = R_prev01 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 1) * sinYaw;
+	_ahrs_ekf_gsf[model_index].R(0, 2) = R_prev02 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 2) * sinYaw;
+	_ahrs_ekf_gsf[model_index].R(1, 0) = R_prev00 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 0) * cosYaw;
+	_ahrs_ekf_gsf[model_index].R(1, 1) = R_prev01 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 1) * cosYaw;
+	_ahrs_ekf_gsf[model_index].R(1, 2) = R_prev02 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 2) * cosYaw;
+
+	return true;
+}
+
+void EKFGSF_yaw::initialiseEKFGSF()
+{
+	_gsf_yaw = 0.0f;
+	_ekf_gsf_vel_fuse_started = false;
+	_gsf_yaw_variance = _m_pi2 * _m_pi2;
+	_model_weights.setAll(1.0f / (float)N_MODELS_EKFGSF);  // All filter models start with the same weight
+
+	memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
+	const float yaw_increment = 2.0f * _m_pi / (float)N_MODELS_EKFGSF;
+	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+		// evenly space initial yaw estimates in the region between +-Pi
+		_ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
+
+		// take velocity states and corresponding variance from last measurement
+		_ekf_gsf[model_index].X(0) = _vel_NE(0);
+		_ekf_gsf[model_index].X(1) = _vel_NE(1);
+		_ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy);
+		_ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0);
+
+		// use half yaw interval for yaw uncertainty
+		_ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment);
+	}
+}
+
+float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
+{
+	// calculate transpose(innovation) * inv(S) * innovation
+	const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
+
+	return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
+}
+
+bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
+{
+	if (_ekf_gsf_vel_fuse_started) {
+		*yaw_composite = _gsf_yaw;
+		*yaw_variance = _gsf_yaw_variance;
+		for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
+			yaw[model_index] = _ekf_gsf[model_index].X(2);
+			innov_VN[model_index] = _ekf_gsf[model_index].innov(0);
+			innov_VE[model_index] = _ekf_gsf[model_index].innov(1);
+			weight[model_index] = _model_weights(model_index);
+		}
+		return true;
+	}
+	return false;
+}
+
+float EKFGSF_yaw::ahrsCalcAccelGain() const
+{
+	// Calculate the acceleration fusion gain using a continuous function that is unity at 1g and zero
+	// at the min and max g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal
+	// acceleration correction as higher and more sustained g will be experienced.
+	// Use a quadratic instead of linear function to prevent vibration around 1g reducing the tilt correction effectiveness.
+	// see https://www.desmos.com/calculator/dbqbxvnwfg
+
+	float attenuation = 2.f;
+	const bool centripetal_accel_compensation_enabled = (_true_airspeed > FLT_EPSILON);
+
+	if (centripetal_accel_compensation_enabled
+	    && _ahrs_accel_norm > CONSTANTS_ONE_G) {
+		attenuation = 1.f;
+	}
+
+	const float delta_accel_g = (_ahrs_accel_norm - CONSTANTS_ONE_G) / CONSTANTS_ONE_G;
+	return _tilt_gain * sq(1.f - math::min(attenuation * fabsf(delta_accel_g), 1.f));
+}
+
+Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
+{
+	Matrix3f ret = R;
+	ret(0,0) += R(0,1) * g(2) - R(0,2) * g(1);
+	ret(0,1) += R(0,2) * g(0) - R(0,0) * g(2);
+	ret(0,2) += R(0,0) * g(1) - R(0,1) * g(0);
+	ret(1,0) += R(1,1) * g(2) - R(1,2) * g(1);
+	ret(1,1) += R(1,2) * g(0) - R(1,0) * g(2);
+	ret(1,2) += R(1,0) * g(1) - R(1,1) * g(0);
+	ret(2,0) += R(2,1) * g(2) - R(2,2) * g(1);
+	ret(2,1) += R(2,2) * g(0) - R(2,0) * g(2);
+	ret(2,2) += R(2,0) * g(1) - R(2,1) * g(0);
+
+	// Renormalise rows
+	for (uint8_t r = 0; r < 3; r++) {
+		const float rowLengthSq = ret.row(r).norm_squared();
+		if (rowLengthSq > FLT_EPSILON) {
+			// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
+			const float rowLengthInv = 1.5f - 0.5f * rowLengthSq;
+			ret.row(r) *=  rowLengthInv;
+		}
+        }
+
+	return ret;
+}
+
+bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance) const
+{
+	if(_ekf_gsf_vel_fuse_started) {
+		*yaw = _gsf_yaw;
+		*yaw_variance = _gsf_yaw_variance;
+		return true;
+	}
+	return false;
+}
+
+void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy)
+{
+	_vel_NE = velocity;
+	_vel_accuracy = accuracy;
+	_vel_data_updated = true;
+}

src/lib/ecl/EKF/EKFGSF_yaw.h

@@ -0,0 +1,135 @@
+#pragma once
+
+#include <geo/geo.h>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+#include "common.h"
+#include "utils.hpp"
+
+using matrix::AxisAnglef;
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Matrix3f;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
+static constexpr uint8_t N_MODELS_EKFGSF = 5;
+
+// Required math constants
+static constexpr float _m_2pi_inv = 0.159154943f;
+static constexpr float _m_pi = 3.14159265f;
+static constexpr float _m_pi2 = 1.57079632f;
+
+using namespace estimator;
+
+class EKFGSF_yaw
+{
+public:
+    	EKFGSF_yaw();
+
+    	// Update Filter States - this should be called whenever new IMU data is available
+	void update(const imuSample &imu_sample,
+			bool run_EKF,  			// set to true when flying or movement is suitable for yaw estimation
+			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
+			const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
+
+	void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
+			float accuracy);	   // 1-sigma accuracy of velocity measurement (m/s)
+
+	// get solution data for logging
+	bool getLogData(float *yaw_composite,
+			float *yaw_composite_variance,
+			float yaw[N_MODELS_EKFGSF],
+			float innov_VN[N_MODELS_EKFGSF],
+			float innov_VE[N_MODELS_EKFGSF],
+			float weight[N_MODELS_EKFGSF]) const;
+
+    	// get yaw estimate and the corresponding variance
+    	// return false if no yaw estimate available
+    	bool getYawData(float *yaw, float *yaw_variance) const;
+
+private:
+
+	// Parameters - these could be made tuneable
+	const float _gyro_noise{1.0e-1f}; 	// yaw rate noise used for covariance prediction (rad/sec)
+	const float _accel_noise{2.0f};		// horizontal accel noise used for covariance prediction (m/sec**2)
+	const float _tilt_gain{0.2f};		// gain from tilt error to gyro correction for complementary filter (1/sec)
+	const float _gyro_bias_gain{0.04f};	// gain applied to integral of gyro correction for complementary filter (1/sec)
+
+	// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters
+
+	Vector3f _delta_ang{};	// IMU delta angle (rad)
+	Vector3f _delta_vel{};	// IMU delta velocity (m/s)
+	float _delta_ang_dt{};	// _delta_ang integration time interval (sec)
+	float _delta_vel_dt{};	// _delta_vel integration time interval (sec)
+	float _true_airspeed{};	// true airspeed used for centripetal accel compensation (m/s)
+
+	struct _ahrs_ekf_gsf_struct{
+		Dcmf R;			// matrix that rotates a vector from body to earth frame
+		Vector3f gyro_bias;	// gyro bias learned and used by the quaternion calculation
+		bool aligned;		// true when AHRS has been aligned
+		float vel_NE[2];	// NE velocity vector from last GPS measurement (m/s)
+		bool fuse_gps;		// true when GPS should be fused on that frame
+		float accel_dt;		// time step used when generating _simple_accel_FR data (sec)
+	} _ahrs_ekf_gsf[N_MODELS_EKFGSF]{};
+
+	bool _ahrs_ekf_gsf_tilt_aligned{};	// true the initial tilt alignment has been calculated
+	float _ahrs_accel_fusion_gain{};	// gain from accel vector tilt error to rate gyro correction used by AHRS calculation
+	Vector3f _ahrs_accel{};			// low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
+	float _ahrs_accel_norm{};		// length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)
+
+	// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
+	float ahrsCalcAccelGain() const;
+
+	// update specified AHRS rotation matrix using IMU and optionally true airspeed data
+	void ahrsPredict(const uint8_t model_index);
+
+	// align all AHRS roll and pitch orientations using IMU delta velocity vector
+	void ahrsAlignTilt();
+
+	// align all AHRS yaw orientations to initial values
+	void ahrsAlignYaw();
+
+	// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
+	Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g);
+
+	// Declarations used by a bank of N_MODELS_EKFGSF EKFs
+
+	struct _ekf_gsf_struct{
+		matrix::Vector3f X; 				// Vel North (m/s),  Vel East (m/s), yaw (rad)s
+		matrix::SquareMatrix<float, 3> P; 		// covariance matrix
+		matrix::SquareMatrix<float, 2> S_inverse;	// inverse of the innovation covariance matrix
+		float S_det_inverse; 				// inverse of the innovation covariance matrix determinant
+		matrix::Vector2f innov; 			// Velocity N,E innovation (m/s)
+	} _ekf_gsf[N_MODELS_EKFGSF]{};
+
+	bool _vel_data_updated{};	// true when velocity data has been updated
+	bool _run_ekf_gsf{};		// true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data
+	Vector2f _vel_NE{};        // NE velocity observations (m/s)
+	float _vel_accuracy{};     // 1-sigma accuracy of velocity observations (m/s)
+	bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active
+
+	// initialise states and covariance data for the GSF and EKF filters
+	void initialiseEKFGSF();
+
+	// predict state and covariance for the specified EKF using inertial data
+	void predictEKF(const uint8_t model_index);
+
+	// update state and covariance for the specified EKF using a NE velocity measurement
+	// return false if update failed
+	bool updateEKF(const uint8_t model_index);
+
+	inline float sq(float x) const { return x * x; };
+
+	// Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates
+
+	matrix::Vector<float, N_MODELS_EKFGSF> _model_weights{};
+	float _gsf_yaw{}; 		// yaw estimate (rad)
+	float _gsf_yaw_variance{}; 	// variance of yaw estimate (rad^2)
+
+	// return the probability of the state estimate for the specified EKF assuming a gaussian error distribution
+	float gaussianDensity(const uint8_t model_index) const;
+};

src/lib/ecl/EKF/RingBuffer.h

@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RingBuffer.h
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * Template RingBuffer.
+ */
+
+#include <inttypes.h>
+#include <cstdio>
+#include <cstring>
+
+template <typename data_type>
+class RingBuffer
+{
+public:
+	explicit RingBuffer(size_t size) { allocate(size); }
+	RingBuffer() { allocate(1); }
+	~RingBuffer() { delete[] _buffer; }
+
+	// no copy, assignment, move, move assignment
+	RingBuffer(const RingBuffer &) = delete;
+	RingBuffer &operator=(const RingBuffer &) = delete;
+	RingBuffer(RingBuffer &&) = delete;
+	RingBuffer &operator=(RingBuffer &&) = delete;
+
+	bool allocate(uint8_t size)
+	{
+		if (valid() && (size == _size)) {
+			// no change
+			return true;
+		}
+
+		if (size == 0) {
+			return false;
+		}
+
+		if (_buffer != nullptr) {
+			delete[] _buffer;
+		}
+
+		_buffer = new data_type[size]{};
+
+		if (_buffer == nullptr) {
+			return false;
+		}
+
+		_size = size;
+
+		_head = 0;
+		_tail = 0;
+
+		_first_write = true;
+
+		return true;
+	}
+
+	bool valid() const { return (_buffer != nullptr) && (_size > 0); }
+
+	void push(const data_type &sample)
+	{
+		uint8_t head_new = _head;
+
+		if (!_first_write) {
+			head_new = (_head + 1) % _size;
+		}
+
+		_buffer[head_new] = sample;
+		_head = head_new;
+
+		// move tail if we overwrite it
+		if (_head == _tail && !_first_write) {
+			_tail = (_tail + 1) % _size;
+
+		} else {
+			_first_write = false;
+		}
+	}
+
+	uint8_t get_length() const { return _size; }
+
+	data_type &operator[](const uint8_t index) { return _buffer[index]; }
+
+	const data_type &get_newest() const { return _buffer[_head]; }
+	const data_type &get_oldest() const { return _buffer[_tail]; }
+
+	uint8_t get_oldest_index() const { return _tail; }
+
+	bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
+	{
+		// start looking from newest observation data
+		for (uint8_t i = 0; i < _size; i++) {
+			int index = (_head - i);
+			index = index < 0 ? _size + index : index;
+
+			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)1e5) {
+				*sample = _buffer[index];
+
+				// Now we can set the tail to the item which
+				// comes after the one we removed since we don't
+				// want to have any older data in the buffer
+				if (index == _head) {
+					_tail = _head;
+					_first_write = true;
+
+				} else {
+					_tail = (index + 1) % _size;
+				}
+
+				_buffer[index].time_us = 0;
+
+				return true;
+			}
+
+			if (index == _tail) {
+				// we have reached the tail and haven't got a
+				// match
+				return false;
+			}
+		}
+
+		return false;
+	}
+
+	int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
+
+private:
+	data_type *_buffer{nullptr};
+
+	uint8_t _head{0};
+	uint8_t _tail{0};
+	uint8_t _size{0};
+
+	bool _first_write{true};
+};

src/lib/ecl/EKF/Sensor.hpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 Estimation and Control Library (ECL). 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 ECL 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 Sensor.hpp
+ * Abstract class for sensors
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ *
+ */
+
+#pragma once
+
+#include "common.h"
+
+namespace estimator
+{
+namespace sensor
+{
+
+class Sensor
+{
+public:
+	virtual ~Sensor() {};
+
+	/*
+	 * run sanity checks on the current data
+	 * this has to be called immediately after
+	 * setting new data
+	 */
+	virtual void runChecks(){};
+
+	/*
+	 * return true if the sensor is healthy
+	 */
+	virtual bool isHealthy() const = 0;
+
+	/*
+	 * return true if the delayed sample is healthy
+	 * and can be fused in the estimator
+	 */
+	virtual bool isDataHealthy() const = 0;
+
+	/*
+	 * return true if the sensor data rate is
+	 * stable and high enough
+	 */
+	virtual bool isRegularlySendingData() const = 0;
+};
+
+} // namespace sensor
+} // namespace estimator

src/lib/ecl/EKF/airspeed_fusion.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 airspeed_fusion.cpp
+ * airspeed fusion methods.
+ * equations generated using EKF/python/ekf_derivation/main.py
+ *
+ * @author Carl Olsson <carlolsson.co@gmail.com>
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+#include "../ecl.h"
+#include "ekf.h"
+#include <mathlib/mathlib.h>
+
+void Ekf::fuseAirspeed()
+{
+	const float &vn = _state.vel(0); // Velocity in north direction
+	const float &ve = _state.vel(1); // Velocity in east direction
+	const float &vd = _state.vel(2); // Velocity in downwards direction
+	const float &vwn = _state.wind_vel(0); // Wind speed in north direction
+	const float &vwe = _state.wind_vel(1); // Wind speed in east direction
+
+	// Variance for true airspeed measurement - (m/sec)^2
+	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
+			    math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+
+	// determine if we need the sideslip fusion to correct states other than wind
+	const bool update_wind_only = !_is_wind_dead_reckoning;
+
+	// Intermediate variables
+	const float HK0 = vn - vwn;
+	const float HK1 = ve - vwe;
+	const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2);
+	const float v_tas_pred = sqrtf(HK2); // predicted airspeed
+	//const float HK3 = powf(HK2, -1.0F/2.0F);
+	if (v_tas_pred < 1.0f) {
+		// calculation can be badly conditioned for very low airspeed values so don't fuse this time
+		return;
+	}
+	const float HK3 = 1.0f / v_tas_pred;
+	const float HK4 = HK0*HK3;
+	const float HK5 = HK1*HK3;
+	const float HK6 = 1.0F/HK2;
+	const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd;
+	const float HK8 = HK1*P(5,23);
+	const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd;
+	const float HK10 = HK1*HK6;
+	const float HK11 = HK0*P(4,22);
+	const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd;
+	const float HK13 = HK0*HK6;
+	const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd;
+	const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd;
+	//const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
+
+	// innovation variance - check for badly conditioned calculation
+	_airspeed_innov_var = (-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS);
+	if (_airspeed_innov_var < R_TAS) { //
+		// Reset the estimator covariance matrix
+		// if we are getting aiding from other sources, warn and reset the wind states and covariances only
+		const char* action_string = nullptr;
+		if (update_wind_only) {
+			resetWindStates();
+			resetWindCovariance();
+			action_string = "wind";
+
+		} else {
+			initialiseCovariance();
+			_state.wind_vel.setZero();
+			action_string = "full";
+		}
+		ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);
+
+		_fault_status.flags.bad_airspeed = true;
+
+		return;
+	}
+	const float HK16 = HK3 / _airspeed_innov_var;
+	_fault_status.flags.bad_airspeed = false;
+
+	// Observation Jacobians
+	SparseVector24f<4,5,6,22,23> Hfusion;
+	Hfusion.at<4>() = HK4;
+	Hfusion.at<5>() = HK5;
+	Hfusion.at<6>() = HK3*vd;
+	Hfusion.at<22>() = -HK4;
+	Hfusion.at<23>() = -HK5;
+
+	Vector24f Kfusion; // Kalman gain vector
+	if (!update_wind_only) {
+		// we have no other source of aiding, so use airspeed measurements to correct states
+		for (unsigned row = 0; row <= 3; row++) {
+			Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
+		}
+
+		Kfusion(4) = HK12*HK16;
+		Kfusion(5) = HK16*HK9;
+		Kfusion(6) = HK16*HK7;
+
+		for (unsigned row = 7; row <= 21; row++) {
+			Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd);
+		}
+	}
+
+	Kfusion(22) = HK15*HK16;
+	Kfusion(23) = HK14*HK16;
+
+	// Calculate measurement innovation
+	_airspeed_innov = v_tas_pred - _airspeed_sample_delayed.true_airspeed;
+
+	// Compute the ratio of innovation to gate size
+	_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);
+
+	// If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health
+	if (_tas_test_ratio > 1.0f) {
+		_innov_check_fail_status.flags.reject_airspeed = true;
+		return;
+
+	} else {
+		_innov_check_fail_status.flags.reject_airspeed = false;
+	}
+
+	const bool is_fused = measurementUpdate(Kfusion, Hfusion, _airspeed_innov);
+
+	_fault_status.flags.bad_airspeed = !is_fused;
+
+	if (is_fused) {
+		_time_last_arsp_fuse = _time_last_imu;
+		_control_status.flags.fuse_aspd = true;
+	}
+}
+
+void Ekf::get_true_airspeed(float *tas) const
+{
+	const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
+	memcpy(tas, &tempvar, sizeof(float));
+}
+
+/*
+ * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
+*/
+void Ekf::resetWindStates()
+{
+	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
+	                        ? getEuler321Yaw(_state.quat_nominal)
+	                        : getEuler312Yaw(_state.quat_nominal);
+
+	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
+		// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
+		_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
+		_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
+
+	} else {
+		// If we don't have an airspeed measurement, then assume the wind is zero
+		_state.wind_vel.setZero();
+	}
+}

src/lib/ecl/EKF/common.h

@@ -0,0 +1,554 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 common.h
+ * Definition of base class for attitude estimators
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
+ *
+ */
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace estimator
+{
+
+using matrix::AxisAnglef;
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Matrix3f;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
+enum class velocity_frame_t : uint8_t {
+	LOCAL_FRAME_FRD,
+	BODY_FRAME_FRD
+};
+
+struct gps_message {
+	uint64_t time_usec{0};
+	int32_t lat;		///< Latitude in 1E-7 degrees
+	int32_t lon;		///< Longitude in 1E-7 degrees
+	int32_t alt;		///< Altitude in 1E-3 meters (millimeters) above MSL
+	float yaw;		///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
+	float yaw_offset;	///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
+	uint8_t fix_type;	///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
+	float eph;		///< GPS horizontal position accuracy in m
+	float epv;		///< GPS vertical position accuracy in m
+	float sacc;		///< GPS speed accuracy in m/s
+	float vel_m_s;		///< GPS ground speed (m/sec)
+	Vector3f vel_ned;	///< GPS ground speed NED
+	bool vel_ned_valid;	///< GPS ground speed is valid
+	uint8_t nsats;		///< number of satellites used
+	float pdop;		///< position dilution of precision
+};
+
+struct outputSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	Quatf  quat_nominal;	///< nominal quaternion describing vehicle attitude
+	Vector3f    vel;	///< NED velocity estimate in earth frame (m/sec)
+	Vector3f    pos;	///< NED position estimate in earth frame (m/sec)
+};
+
+struct outputVert {
+	uint64_t    time_us{0};		///< timestamp of the measurement (uSec)
+	float	    vert_vel;		///< Vertical velocity calculated using alternative algorithm (m/sec)
+	float	    vert_vel_integ;	///< Integral of vertical velocity (m)
+	float	    dt;			///< delta time (sec)
+};
+
+struct imuSample {
+	uint64_t    time_us{0};		///< timestamp of the measurement (uSec)
+	Vector3f    delta_ang;		///< delta angle in body frame (integrated gyro measurements) (rad)
+	Vector3f    delta_vel;		///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
+	float       delta_ang_dt;	///< delta angle integration period (sec)
+	float       delta_vel_dt;	///< delta velocity integration period (sec)
+	bool        delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping
+};
+
+struct gpsSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	Vector2f    pos;	///< NE earth frame gps horizontal position measurement (m)
+	float       hgt;	///< gps height measurement (m)
+	Vector3f    vel;	///< NED earth frame gps velocity measurement (m/sec)
+	float	    yaw;	///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
+	float	    hacc;	///< 1-std horizontal position error (m)
+	float	    vacc;	///< 1-std vertical position error (m)
+	float       sacc;	///< 1-std speed error (m/sec)
+};
+
+struct magSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	Vector3f    mag;	///< NED magnetometer body frame measurements (Gauss)
+};
+
+struct baroSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	float       hgt;	///< pressure altitude above sea level (m)
+};
+
+struct rangeSample {
+	uint64_t    time_us{0};	///< timestamp of the measurement (uSec)
+	float       rng;	    ///< range (distance to ground) measurement (m)
+	int8_t	    quality;    ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
+};
+
+struct airspeedSample {
+	uint64_t    time_us{0};		///< timestamp of the measurement (uSec)
+	float       true_airspeed;	///< true airspeed measurement (m/sec)
+	float       eas2tas;		///< equivalent to true airspeed factor
+};
+
+struct flowSample {
+	uint64_t time_us{0};	///< timestamp of the integration period leading edge (uSec)
+	Vector2f flow_xy_rad;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
+	Vector3f gyro_xyz;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
+	float    dt;		///< amount of integration time (sec)
+	uint8_t  quality;	///< quality indicator between 0 and 255
+};
+
+struct extVisionSample {
+	uint64_t time_us{0};	///< timestamp of the measurement (uSec)
+	Vector3f pos;	///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
+	Vector3f vel;	///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
+	Quatf quat;		///< quaternion defining rotation from body to earth frame
+	Vector3f posVar;	///< XYZ position variances (m**2)
+	Matrix3f velCov;	///< XYZ velocity covariances ((m/sec)**2)
+	float angVar;		///< angular heading variance (rad**2)
+	velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD;
+};
+
+struct dragSample {
+	uint64_t time_us{0};	///< timestamp of the measurement (uSec)
+	Vector2f accelXY;	///< measured specific force along the X and Y body axes (m/sec**2)
+};
+
+struct auxVelSample {
+	uint64_t time_us{0};	///< timestamp of the measurement (uSec)
+	Vector3f vel;		///< measured NE velocity relative to the local origin (m/sec)
+	Vector3f velVar;	///< estimated error variance of the NE velocity (m/sec)**2
+};
+
+// Integer definitions for vdist_sensor_type
+#define VDIST_SENSOR_BARO  0	///< Use baro height
+#define VDIST_SENSOR_GPS   1	///< Use GPS height
+#define VDIST_SENSOR_RANGE 2	///< Use range finder height
+#define VDIST_SENSOR_EV    3    ///< Use external vision
+
+// Bit locations for mag_declination_source
+#define MASK_USE_GEO_DECL   (1<<0)  ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
+#define MASK_SAVE_GEO_DECL  (1<<1)  ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
+#define MASK_FUSE_DECL      (1<<2)  ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
+
+// Bit locations for fusion_mode
+#define MASK_USE_GPS    (1<<0)		///< set to true to use GPS data
+#define MASK_USE_OF     (1<<1)		///< set to true to use optical flow data
+#define MASK_INHIBIT_ACC_BIAS (1<<2)	///< set to true to inhibit estimation of accelerometer delta velocity bias
+#define MASK_USE_EVPOS	(1<<3)		///< set to true to use external vision position data
+#define MASK_USE_EVYAW  (1<<4)		///< set to true to use external vision quaternion data for yaw
+#define MASK_USE_DRAG  (1<<5)		///< set to true to use the multi-rotor drag model to estimate wind
+#define MASK_ROTATE_EV  (1<<6)		///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
+#define MASK_USE_GPSYAW  (1<<7)		///< set to true to use GPS yaw data if available
+#define MASK_USE_EVVEL  (1<<8)		///< set to true to use external vision velocity data
+
+enum TerrainFusionMask : int32_t {
+	TerrainFuseRangeFinder = (1 << 0),
+	TerrainFuseOpticalFlow = (1 << 1)
+};
+
+// Integer definitions for mag_fusion_type
+#define MAG_FUSE_TYPE_AUTO      0	///< The selection of either heading or 3D magnetometer fusion will be automatic
+#define MAG_FUSE_TYPE_HEADING   1	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
+#define MAG_FUSE_TYPE_3D        2	///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
+#define MAG_FUSE_TYPE_UNUSED    3	///< Not implemented
+#define MAG_FUSE_TYPE_INDOOR    4	///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
+#define MAG_FUSE_TYPE_NONE	5	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
+
+// Maximum sensor intervals in usec
+#define GPS_MAX_INTERVAL  (uint64_t)5e5	///< Maximum allowable time interval between GPS measurements (uSec)
+#define BARO_MAX_INTERVAL (uint64_t)2e5	///< Maximum allowable time interval between pressure altitude measurements (uSec)
+#define RNG_MAX_INTERVAL  (uint64_t)2e5	///< Maximum allowable time interval between range finder  measurements (uSec)
+#define EV_MAX_INTERVAL   (uint64_t)2e5	///< Maximum allowable time interval between external vision system measurements (uSec)
+
+// bad accelerometer detection and mitigation
+#define BADACC_PROBATION  (uint64_t)10e6	///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
+#define BADACC_BIAS_PNOISE	4.9f	///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
+
+// ground effect compensation
+#define GNDEFFECT_TIMEOUT	10E6	///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
+
+struct parameters {
+	// measurement source control
+	int32_t fusion_mode{MASK_USE_GPS};		///< bitmasked integer that selects which aiding sources will be used
+	int32_t vdist_sensor_type{VDIST_SENSOR_BARO};	///< selects the primary source for height data
+	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
+				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
+	int32_t sensor_interval_min_ms{20};		///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
+
+	// measurement time delays
+	float mag_delay_ms{0.0f};		///< magnetometer measurement delay relative to the IMU (mSec)
+	float baro_delay_ms{0.0f};		///< barometer height measurement delay relative to the IMU (mSec)
+	float gps_delay_ms{110.0f};		///< GPS measurement delay relative to the IMU (mSec)
+	float airspeed_delay_ms{100.0f};	///< airspeed measurement delay relative to the IMU (mSec)
+	float flow_delay_ms{5.0f};		///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
+	float range_delay_ms{5.0f};		///< range finder measurement delay relative to the IMU (mSec)
+	float ev_delay_ms{175.0f};		///< off-board vision measurement delay relative to the IMU (mSec)
+	float auxvel_delay_ms{5.0f};		///< auxiliary velocity measurement delay relative to the IMU (mSec)
+
+	// input noise
+	float gyro_noise{1.5e-2f};		///< IMU angular rate noise used for covariance prediction (rad/sec)
+	float accel_noise{3.5e-1f};		///< IMU acceleration noise use for covariance prediction (m/sec**2)
+
+	// process noise
+	float gyro_bias_p_noise{1.0e-3f};	///< process noise for IMU rate gyro bias prediction (rad/sec**2)
+	float accel_bias_p_noise{1.0e-2f};	///< process noise for IMU accelerometer bias prediction (m/sec**3)
+	float mage_p_noise{1.0e-3f};		///< process noise for earth magnetic field prediction (Gauss/sec)
+	float magb_p_noise{1.0e-4f};		///< process noise for body magnetic field prediction (Gauss/sec)
+	float wind_vel_p_noise{1.0e-1f};	///< process noise for wind velocity prediction (m/sec**2)
+	const float wind_vel_p_noise_scaler{0.5f};	///< scaling of wind process noise with vertical velocity
+	float terrain_p_noise{5.0f};		///< process noise for terrain offset (m/sec)
+	float terrain_gradient{0.5f};		///< gradient of terrain used to estimate process noise due to changing position (m/m)
+	const float terrain_timeout{10.f};		///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
+
+	// initialization errors
+	float switch_on_gyro_bias{0.1f};	///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
+	float switch_on_accel_bias{0.2f};	///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
+	float initial_tilt_err{0.1f};		///< 1-sigma tilt error after initial alignment using gravity vector (rad)
+	const float initial_wind_uncertainty{1.0f};	///< 1-sigma initial uncertainty in wind velocity (m/sec)
+
+	// position and velocity fusion
+	float gps_vel_noise{5.0e-1f};		///< minimum allowed observation noise for gps velocity fusion (m/sec)
+	float gps_pos_noise{0.5f};		///< minimum allowed observation noise for gps position fusion (m)
+	float pos_noaid_noise{10.0f};		///< observation noise for non-aiding position fusion (m)
+	float baro_noise{2.0f};			///< observation noise for barometric height fusion (m)
+	float baro_innov_gate{5.0f};		///< barometric and GPS height innovation consistency gate size (STD)
+	float gps_pos_innov_gate{5.0f};		///< GPS horizontal position innovation consistency gate size (STD)
+	float gps_vel_innov_gate{5.0f};		///< GPS velocity innovation consistency gate size (STD)
+	float gnd_effect_deadzone{5.0f};	///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
+	float gnd_effect_max_hgt{0.5f};		///< Height above ground at which baro ground effect becomes insignificant (m)
+
+	// magnetometer fusion
+	float mag_heading_noise{3.0e-1f};	///< measurement noise used for simple heading fusion (rad)
+	float mag_noise{5.0e-2f};		///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
+	float mag_declination_deg{0.0f};	///< magnetic declination (degrees)
+	float heading_innov_gate{2.6f};		///< heading fusion innovation consistency gate size (STD)
+	float mag_innov_gate{3.0f};		///< magnetometer fusion innovation consistency gate size (STD)
+	int32_t mag_declination_source{7};	///< bitmask used to control the handling of declination data
+	int32_t mag_fusion_type{0};		///< integer used to specify the type of magnetometer fusion used
+	float mag_acc_gate{0.5f};		///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
+	float mag_yaw_rate_gate{0.25f};		///< yaw rate threshold used by mode select logic (rad/sec)
+	const float quat_max_variance{0.0001f};	///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this
+
+	// airspeed fusion
+	float tas_innov_gate{5.0f};		///< True Airspeed innovation consistency gate size (STD)
+	float eas_noise{1.4f};			///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
+	float arsp_thr{2.0f};			///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
+
+	// synthetic sideslip fusion
+	float beta_innov_gate{5.0f};		///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
+	float beta_noise{0.3f};			///< synthetic sideslip noise (rad)
+	const float beta_avg_ft_us{150000.0f};	///< The average time between synthetic sideslip measurements (uSec)
+
+	// range finder fusion
+	float range_noise{0.1f};		///< observation noise for range finder measurements (m)
+	float range_innov_gate{5.0f};		///< range finder fusion innovation consistency gate size (STD)
+	float rng_gnd_clearance{0.1f};		///< minimum valid value for range when on ground (m)
+	float rng_sens_pitch{0.0f};		///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
+	float range_noise_scaler{0.0f};		///< scaling from range measurement to noise (m/m)
+	const float vehicle_variance_scaler{0.0f};	///< gain applied to vehicle height variance used in calculation of height above ground observation variance
+	float max_hagl_for_range_aid{5.0f};	///< maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
+	float max_vel_for_range_aid{1.0f};	///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
+	int32_t range_aid{0};			///< allow switching primary height source to range finder if certain conditions are met
+	float range_aid_innov_gate{1.0f}; 	///< gate size used for innovation consistency checks for range aid fusion
+	float range_valid_quality_s{1.0f};	///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
+	float range_cos_max_tilt{0.7071f};	///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
+
+	// vision position fusion
+    float ev_vel_innov_gate{3.0f};		///< vision velocity fusion innovation consistency gate size (STD)
+    float ev_pos_innov_gate{5.0f};		///< vision position fusion innovation consistency gate size (STD)
+
+	// optical flow fusion
+	float flow_noise{0.15f};		///< observation noise for optical flow LOS rate measurements (rad/sec)
+	float flow_noise_qual_min{0.5f};	///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
+	int32_t flow_qual_min{1};		///< minimum acceptable quality integer from  the flow sensor
+	float flow_innov_gate{3.0f};		///< optical flow fusion innovation consistency gate size (STD)
+
+	// these parameters control the strictness of GPS quality checks used to determine if the GPS is
+	// good enough to set a local origin and commence aiding
+	int32_t gps_check_mask{21};		///< bitmask used to control which GPS quality checks are used
+	float req_hacc{5.0f};			///< maximum acceptable horizontal position error (m)
+	float req_vacc{8.0f};			///< maximum acceptable vertical position error (m)
+	float req_sacc{1.0f};			///< maximum acceptable speed error (m/s)
+	int32_t req_nsats{6};			///< minimum acceptable satellite count
+	float req_pdop{2.0f};			///< maximum acceptable position dilution of precision
+	float req_hdrift{0.3f};			///< maximum acceptable horizontal drift speed (m/s)
+	float req_vdrift{0.5f};			///< maximum acceptable vertical drift speed (m/s)
+
+	// XYZ offset of sensors in body axes (m)
+	Vector3f imu_pos_body;			///< xyz position of IMU in body frame (m)
+	Vector3f gps_pos_body;			///< xyz position of the GPS antenna in body frame (m)
+	Vector3f rng_pos_body;			///< xyz position of range sensor in body frame (m)
+	Vector3f flow_pos_body;			///< xyz position of range sensor focal point in body frame (m)
+	Vector3f ev_pos_body;			///< xyz position of VI-sensor focal point in body frame (m)
+
+	// output complementary filter tuning
+	float vel_Tau{0.25f};			///< velocity state correction time constant (1/sec)
+	float pos_Tau{0.25f};			///< position state correction time constant (1/sec)
+
+	// accel bias learning control
+	float acc_bias_lim{0.4f};		///< maximum accel bias magnitude (m/sec**2)
+	float acc_bias_learn_acc_lim{25.0f};	///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
+	float acc_bias_learn_gyr_lim{3.0f};	///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
+	float acc_bias_learn_tc{0.5f};		///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
+
+	const unsigned reset_timeout_max{7000000};	///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec)
+	const unsigned no_aid_timeout_max{1000000};	///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec)
+
+	int32_t valid_timeout_max{5000000};	///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
+
+	// static barometer pressure position error coefficient along body axes
+	float static_pressure_coef_xp {0.0f};	// (-)
+	float static_pressure_coef_xn {0.0f};	// (-)
+	float static_pressure_coef_yp {0.0f};	// (-)
+	float static_pressure_coef_yn {0.0f};	// (-)
+	float static_pressure_coef_z {0.0f};	// (-)
+	// upper limit on airspeed used for correction  (m/s**2)
+	float max_correction_airspeed {20.0f};
+
+	// multi-rotor drag specific force fusion
+	float drag_noise{2.5f};			///< observation noise variance for drag specific force measurements (m/sec**2)**2
+	float bcoef_x{100.0f};			///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
+	float bcoef_y{100.0f};			///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
+	float mcoef{0.1f};			///< rotor momentum drag coefficient for the X and Y axes (1/s)
+
+	// control of accel error detection and mitigation (IMU clipping)
+	const float vert_innov_test_lim{3.0f};	///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
+	const int bad_acc_reset_delay_us{500000};	///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)
+
+	// auxiliary velocity fusion
+	const float auxvel_noise{0.5f};		///< minimum observation noise, uses reported noise if greater (m/s)
+	const float auxvel_gate{5.0f};		///< velocity fusion innovation consistency gate size (STD)
+
+	// control of on-ground movement check
+	float is_moving_scaler{1.0f};		///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive.
+
+	// compute synthetic magnetomter Z value if possible
+	int32_t synthesize_mag_z{0};
+	int32_t check_mag_strength{0};
+
+	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
+	float EKFGSF_tas_default{15.0f};	///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
+	const unsigned EKFGSF_reset_delay{1000000};	///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
+	const float EKFGSF_yaw_err_max{0.262f}; 	///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
+	const unsigned EKFGSF_reset_count_limit{3};	///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
+};
+
+struct stateSample {
+	Quatf  quat_nominal;	///< quaternion defining the rotation from body to earth frame
+	Vector3f    vel;	///< NED velocity in earth frame in m/s
+	Vector3f    pos;	///< NED position in earth frame in m
+	Vector3f    delta_ang_bias;	///< delta angle bias estimate in rad
+	Vector3f    delta_vel_bias;	///< delta velocity bias estimate in m/s
+	Vector3f    mag_I;	///< NED earth magnetic field in gauss
+	Vector3f    mag_B;	///< magnetometer bias estimate in body frame in gauss
+	Vector2f    wind_vel;	///< horizontal wind velocity in earth frame in m/s
+};
+
+union fault_status_u {
+	struct {
+		bool bad_mag_x: 1;	///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
+		bool bad_mag_y: 1;	///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
+		bool bad_mag_z: 1;	///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
+		bool bad_hdg: 1;	///< 3 - true if the fusion of the heading angle has encountered a numerical error
+		bool bad_mag_decl: 1;	///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
+		bool bad_airspeed: 1;	///< 5 - true if fusion of the airspeed has encountered a numerical error
+		bool bad_sideslip: 1;	///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
+		bool bad_optflow_X: 1;	///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
+		bool bad_optflow_Y: 1;	///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
+		bool bad_vel_N: 1;	///< 9 - true if fusion of the North velocity has encountered a numerical error
+		bool bad_vel_E: 1;	///< 10 - true if fusion of the East velocity has encountered a numerical error
+		bool bad_vel_D: 1;	///< 11 - true if fusion of the Down velocity has encountered a numerical error
+		bool bad_pos_N: 1;	///< 12 - true if fusion of the North position has encountered a numerical error
+		bool bad_pos_E: 1;	///< 13 - true if fusion of the East position has encountered a numerical error
+		bool bad_pos_D: 1;	///< 14 - true if fusion of the Down position has encountered a numerical error
+		bool bad_acc_bias: 1;	///< 15 - true if bad delta velocity bias estimates have been detected
+		bool bad_acc_vertical: 1; ///< 16 - true if bad vertical accelerometer data has been detected
+		bool bad_acc_clipping: 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
+	} flags;
+	uint32_t value;
+
+};
+
+// define structure used to communicate innovation test failures
+union innovation_fault_status_u {
+	struct {
+		bool reject_hor_vel: 1;		///< 0 - true if horizontal velocity observations have been rejected
+		bool reject_ver_vel: 1;		///< 1 - true if vertical velocity observations have been rejected
+		bool reject_hor_pos: 1;		///< 2 - true if horizontal position observations have been rejected
+		bool reject_ver_pos: 1;		///< 3 - true if true if vertical position observations have been rejected
+		bool reject_mag_x: 1;		///< 4 - true if the X magnetometer observation has been rejected
+		bool reject_mag_y: 1;		///< 5 - true if the Y magnetometer observation has been rejected
+		bool reject_mag_z: 1;		///< 6 - true if the Z magnetometer observation has been rejected
+		bool reject_yaw: 1;		///< 7 - true if the yaw observation has been rejected
+		bool reject_airspeed: 1;	///< 8 - true if the airspeed observation has been rejected
+		bool reject_sideslip: 1;	///< 9 - true if the synthetic sideslip observation has been rejected
+		bool reject_hagl: 1;		///< 10 - true if the height above ground observation has been rejected
+		bool reject_optflow_X: 1;	///< 11 - true if the X optical flow observation has been rejected
+		bool reject_optflow_Y: 1;	///< 12 - true if the Y optical flow observation has been rejected
+	} flags;
+	uint16_t value;
+
+};
+
+// publish the status of various GPS quality checks
+union gps_check_fail_status_u {
+	struct {
+		uint16_t fix    : 1; ///< 0 - true if the fix type is insufficient (no 3D solution)
+		uint16_t nsats  : 1; ///< 1 - true if number of satellites used is insufficient
+		uint16_t pdop   : 1; ///< 2 - true if position dilution of precision is insufficient
+		uint16_t hacc   : 1; ///< 3 - true if reported horizontal accuracy is insufficient
+		uint16_t vacc   : 1; ///< 4 - true if reported vertical accuracy is insufficient
+		uint16_t sacc   : 1; ///< 5 - true if reported speed accuracy is insufficient
+		uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
+		uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
+		uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
+		uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
+	} flags;
+	uint16_t value;
+};
+
+// bitmask containing filter control status
+union filter_control_status_u {
+	struct {
+		uint32_t tilt_align  : 1; ///< 0 - true if the filter tilt alignment is complete
+		uint32_t yaw_align   : 1; ///< 1 - true if the filter yaw alignment is complete
+		uint32_t gps         : 1; ///< 2 - true if GPS measurement fusion is intended
+		uint32_t opt_flow    : 1; ///< 3 - true if optical flow measurements fusion is intended
+		uint32_t mag_hdg     : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
+		uint32_t mag_3D      : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is inteded
+		uint32_t mag_dec     : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
+		uint32_t in_air      : 1; ///< 7 - true when the vehicle is airborne
+		uint32_t wind        : 1; ///< 8 - true when wind velocity is being estimated
+		uint32_t baro_hgt    : 1; ///< 9 - true when baro height is being fused as a primary height reference
+		uint32_t rng_hgt     : 1; ///< 10 - true when range finder height is being fused as a primary height reference
+		uint32_t gps_hgt     : 1; ///< 11 - true when GPS height is being fused as a primary height reference
+		uint32_t ev_pos      : 1; ///< 12 - true when local position data fusion from external vision is intended
+		uint32_t ev_yaw      : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
+		uint32_t ev_hgt      : 1; ///< 14 - true when height data from external vision measurements is being fused
+		uint32_t fuse_beta   : 1; ///< 15 - true when synthetic sideslip measurements are being fused
+		uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
+		uint32_t fixed_wing  : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
+		uint32_t mag_fault   : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
+		uint32_t fuse_aspd   : 1; ///< 19 - true when airspeed measurements are being fused
+		uint32_t gnd_effect  : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
+		uint32_t rng_stuck   : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
+		uint32_t gps_yaw     : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
+		uint32_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
+		uint32_t ev_vel      : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
+		uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
+		uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
+	} flags;
+	uint32_t value;
+};
+
+ // Mavlink bitmask containing state of estimator solution
+union ekf_solution_status {
+	struct {
+		uint16_t attitude           : 1; ///< 0 - True if the attitude estimate is good
+		uint16_t velocity_horiz     : 1; ///< 1 - True if the horizontal velocity estimate is good
+		uint16_t velocity_vert      : 1; ///< 2 - True if the vertical velocity estimate is good
+		uint16_t pos_horiz_rel      : 1; ///< 3 - True if the horizontal position (relative) estimate is good
+		uint16_t pos_horiz_abs      : 1; ///< 4 - True if the horizontal position (absolute) estimate is good
+		uint16_t pos_vert_abs       : 1; ///< 5 - True if the vertical position (absolute) estimate is good
+		uint16_t pos_vert_agl       : 1; ///< 6 - True if the vertical position (above ground) estimate is good
+		uint16_t const_pos_mode     : 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
+		uint16_t pred_pos_horiz_rel : 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
+		uint16_t pred_pos_horiz_abs : 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
+		uint16_t gps_glitch         : 1; ///< 10 - True if the EKF has detected a GPS glitch
+		uint16_t accel_error        : 1; ///< 11 - True if the EKF has detected bad accelerometer data
+	} flags;
+	uint16_t value;
+};
+
+union terrain_fusion_status_u {
+	struct {
+		bool range_finder: 1;	///< 0 - true if we are fusing range finder data
+		bool flow: 1;		///< 1 - true if we are fusing flow data
+	} flags;
+	uint8_t value;
+};
+
+// define structure used to communicate information events
+union information_event_status_u {
+	struct {
+		bool gps_checks_passed		: 1; ///< 0 - true when gps quality checks are passing passed
+		bool reset_vel_to_gps		: 1; ///< 1 - true when the velocity states are reset to the gps measurement
+		bool reset_vel_to_flow		: 1; ///< 2 - true when the velocity states are reset using the optical flow measurement
+		bool reset_vel_to_vision	: 1; ///< 3 - true when the velocity states are reset to the vision system measurement
+		bool reset_vel_to_zero		: 1; ///< 4  - true when the velocity states are reset to zero
+		bool reset_pos_to_last_known	: 1; ///< 5 - true when the position states are reset to the last known position
+		bool reset_pos_to_gps		: 1; ///< 6 - true when the position states are reset to the gps measurement
+		bool reset_pos_to_vision	: 1; ///< 7 - true when the position states are reset to the vision system measurement
+		bool starting_gps_fusion	: 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates
+		bool starting_vision_pos_fusion	: 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
+		bool starting_vision_vel_fusion	: 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
+		bool starting_vision_yaw_fusion	: 1; ///< 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
+		bool yaw_aligned_to_imu_gps	: 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
+	} flags;
+	uint32_t value;
+};
+
+// define structure used to communicate information events
+union warning_event_status_u {
+	struct {
+		bool gps_quality_poor			: 1; ///< 0 - true when the gps is failing quality checks
+		bool gps_fusion_timout			: 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
+		bool gps_data_stopped			: 1; ///< 2 - true when the gps data has stopped for a significant time period
+		bool gps_data_stopped_using_alternate	: 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
+		bool height_sensor_timeout		: 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
+		bool stopping_navigation		: 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
+		bool invalid_accel_bias_cov_reset	: 1; ///< 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
+		bool bad_yaw_using_gps_course		: 1; ///< 7 - true when the fiter has detected an invalid yaw esitmate and has reset the yaw angle to the GPS ground course
+		bool stopping_mag_use			: 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
+		bool vision_data_stopped		: 1; ///< 9 - true when the vision system data has stopped for a significant time period
+		bool emergency_yaw_reset_mag_stopped	: 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
+	} flags;
+	uint32_t value;
+};
+
+}

src/lib/ecl/EKF/documentation/readme.txt

@@ -0,0 +1,9 @@
+The EKF uses a range of techniques acquired from several years of experience including an original method to handle the delayed time horizon problem.
+A list of references I have found useful has been included.
+
+- The python script used to derive the autogenerated expressions in the EKF can be found here: https://github.com/PX4/ecl/blob/master/EKF/python/ekf_derivation/main.py
+
+- A working Matlab model of the filter capable of replaying flight data can be found here: https://github.com/PX4/ecl/tree/master/EKF/matlab/EKF_replay
+
+Paul Riseborough
+https://github.com/priseborough

src/lib/ecl/EKF/drag_fusion.cpp

@@ -0,0 +1,327 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 drag_fusion.cpp
+ * Body frame drag fusion methods used for multi-rotor wind estimation.
+ * equations generated using EKF/python/ekf_derivation/main.py
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+
+#include "ekf.h"
+#include <ecl.h>
+#include <mathlib/mathlib.h>
+
+void Ekf::fuseDrag()
+{
+	SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;  // Observation Jacobians
+	Vector24f Kfusion; // Kalman gain vector
+
+	const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
+	const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
+
+	// correct rotor momentum drag for increase in required rotor mass flow with altitude
+	// obtained from momentum disc theory
+	const float mcoef_corrrected = _params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+
+	// drag model parameters
+	const bool using_bcoef_x = _params.bcoef_x > 1.0f;
+	const bool using_bcoef_y = _params.bcoef_y > 1.0f;
+	const bool using_mcoef   = _params.mcoef   > 0.001f;
+
+	if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
+		return;
+	}
+
+	// get latest estimated orientation
+	const float &q0 = _state.quat_nominal(0);
+	const float &q1 = _state.quat_nominal(1);
+	const float &q2 = _state.quat_nominal(2);
+	const float &q3 = _state.quat_nominal(3);
+
+	// get latest velocity in earth frame
+	const float &vn = _state.vel(0);
+	const float &ve = _state.vel(1);
+	const float &vd = _state.vel(2);
+
+	// get latest wind velocity in earth frame
+	const float &vwn = _state.wind_vel(0);
+	const float &vwe = _state.wind_vel(1);
+
+	// predicted specific forces
+	// calculate relative wind velocity in earth frame and rotate into body frame
+	const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
+	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
+	const Vector3f rel_wind_body = earth_to_body * rel_wind_earth;
+
+	// perform sequential fusion of XY specific forces
+	for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
+		// measured drag acceleration corrected for sensor bias
+		const float mea_acc = _drag_sample_delayed.accelXY(axis_index)  - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
+
+		// predicted drag force sign is opposite to predicted wind relative velocity
+		const float drag_sign = (rel_wind_body(axis_index) >= 0.f) ? -1.f : 1.f;
+
+		// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
+		// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
+		// parallel to the rotor disc and mass flow through the rotor disc.
+		float pred_acc = 0.0f; // predicted drag acceleration
+		if (axis_index == 0) {
+			float Kacc; // Derivative of specific force wrt airspeed
+			if (using_mcoef && using_bcoef_x) {
+				// Use a combination of bluff body and propeller momentum drag
+				const float bcoef_inv = 1.0f / _params.bcoef_x;
+				// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
+				// mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed
+				const float airspeed = (_params.bcoef_x  / rho) * (- mcoef_corrrected  + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
+				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
+				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * mcoef_corrrected;
+			} else if (using_mcoef) {
+				// Use propeller momentum drag only
+				Kacc = fmaxf(1e-1f, mcoef_corrrected);
+				pred_acc = - rel_wind_body(0) * mcoef_corrrected;
+			} else if (using_bcoef_x) {
+				// Use bluff body drag only
+				// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
+				// mea_acc = (0.5 * rho / _params.bcoef_x) * airspeed**2
+				const float airspeed = sqrtf((2.0f * _params.bcoef_x  * fabsf(mea_acc)) / rho);
+				const float bcoef_inv = 1.0f / _params.bcoef_x;
+				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
+				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign;
+			} else {
+				// skip this axis
+				continue;
+			}
+
+			// intermediate variables
+			const float HK0 = vn - vwn;
+			const float HK1 = ve - vwe;
+			const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
+			const float HK3 = 2*Kacc;
+			const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
+			const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
+			const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
+			const float HK7 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2);
+			const float HK8 = HK7*Kacc;
+			const float HK9 = q0*q3 + q1*q2;
+			const float HK10 = HK3*HK9;
+			const float HK11 = q0*q2 - q1*q3;
+			const float HK12 = 2*HK9;
+			const float HK13 = 2*HK11;
+			const float HK14 = 2*HK4;
+			const float HK15 = 2*HK2;
+			const float HK16 = 2*HK5;
+			const float HK17 = 2*HK6;
+			const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4);
+			const float HK19 = HK12*P(5,23);
+			const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23);
+			const float HK21 = ecl::powf(Kacc, 2);
+			const float HK22 = HK12*HK21;
+			const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22);
+			const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22);
+			const float HK25 = HK7*P(4,22);
+			const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4);
+			const float HK27 = HK21*HK7;
+			const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22);
+			const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4);
+			const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4);
+			const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4);
+			//const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
+
+			// calculate innovation variance and exit if badly conditioned
+			_drag_innov_var(0) = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
+			if (_drag_innov_var(0) < R_ACC) {
+				return;
+			}
+			const float HK32 = Kacc / _drag_innov_var(0);
+
+			// Observation Jacobians
+			Hfusion.at<0>() = -HK2*HK3;
+			Hfusion.at<1>() = -HK3*HK4;
+			Hfusion.at<2>() = HK3*HK5;
+			Hfusion.at<3>() = -HK3*HK6;
+			Hfusion.at<4>() = -HK8;
+			Hfusion.at<5>() = -HK10;
+			Hfusion.at<6>() = HK11*HK3;
+			Hfusion.at<22>() = HK8;
+			Hfusion.at<23>() = HK10;
+
+			// Kalman gains
+			// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
+			// Kfusion(0) = -HK18*HK32;
+			// Kfusion(1) = -HK29*HK32;
+			// Kfusion(2) = -HK30*HK32;
+			// Kfusion(3) = -HK31*HK32;
+			// Kfusion(4) = -HK26*HK32;
+			// Kfusion(5) = -HK23*HK32;
+			// Kfusion(6) = -HK24*HK32;
+			// Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
+			// Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
+			// Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
+			// Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
+			// Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
+			// Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
+			// Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
+			// Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
+			// Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
+			// Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
+			// Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
+			// Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
+			// Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
+			// Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
+			// Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
+			Kfusion(22) = -HK28*HK32;
+			Kfusion(23) = -HK20*HK32;
+
+
+		} else if (axis_index == 1) {
+			float Kacc; // Derivative of specific force wrt airspeed
+			if (using_mcoef && using_bcoef_y) {
+				// Use a combination of bluff body and propeller momentum drag
+				const float bcoef_inv = 1.0f / _params.bcoef_y;
+				// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
+				// mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed
+				const float airspeed = (_params.bcoef_y  / rho) * (- mcoef_corrrected  + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
+				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
+				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * mcoef_corrrected;
+			} else if (using_mcoef) {
+				// Use propeller momentum drag only
+				Kacc = fmaxf(1e-1f, mcoef_corrrected);
+				pred_acc = - rel_wind_body(1) * mcoef_corrrected;
+			} else if (using_bcoef_y) {
+				// Use bluff body drag only
+				// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
+				// mea_acc = (0.5 * rho / _params.bcoef_y) * airspeed**2
+				const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho);
+				const float bcoef_inv = 1.0f / _params.bcoef_y;
+				Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
+				pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign;
+			} else {
+				// nothing more to do
+				return;
+			}
+
+			// intermediate variables
+			const float HK0 = ve - vwe;
+			const float HK1 = vn - vwn;
+			const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
+			const float HK3 = 2*Kacc;
+			const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
+			const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
+			const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
+			const float HK7 = q0*q3 - q1*q2;
+			const float HK8 = HK3*HK7;
+			const float HK9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
+			const float HK10 = HK9*Kacc;
+			const float HK11 = q0*q1 + q2*q3;
+			const float HK12 = 2*HK11;
+			const float HK13 = 2*HK7;
+			const float HK14 = 2*HK5;
+			const float HK15 = 2*HK2;
+			const float HK16 = 2*HK4;
+			const float HK17 = 2*HK6;
+			const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5);
+			const float HK19 = ecl::powf(Kacc, 2);
+			const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23);
+			const float HK21 = HK13*P(4,22);
+			const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22);
+			const float HK23 = HK13*HK19;
+			const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5);
+			const float HK25 = HK9*P(5,23);
+			const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5);
+			const float HK27 = HK19*HK9;
+			const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23);
+			const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5);
+			const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5);
+			const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5);
+			// const float HK32 = Kacc/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
+
+			_drag_innov_var(1) = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
+			if (_drag_innov_var(1) < R_ACC) {
+				// calculation is badly conditioned
+				return;
+			}
+			const float HK32 = Kacc / _drag_innov_var(1);
+
+			// Observation Jacobians
+			Hfusion.at<0>() = -HK2*HK3;
+			Hfusion.at<1>() = -HK3*HK4;
+			Hfusion.at<2>() = -HK3*HK5;
+			Hfusion.at<3>() = HK3*HK6;
+			Hfusion.at<4>() = HK8;
+			Hfusion.at<5>() = -HK10;
+			Hfusion.at<6>() = -HK11*HK3;
+			Hfusion.at<22>() = -HK8;
+			Hfusion.at<23>() = HK10;
+
+			// Kalman gains
+			// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
+			// Kfusion(0) = -HK18*HK32;
+			// Kfusion(1) = -HK30*HK32;
+			// Kfusion(2) = -HK29*HK32;
+			// Kfusion(3) = -HK31*HK32;
+			// Kfusion(4) = -HK24*HK32;
+			// Kfusion(5) = -HK26*HK32;
+			// Kfusion(6) = -HK20*HK32;
+			// Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23));
+			// Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23));
+			// Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23));
+			// Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10));
+			// Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11));
+			// Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12));
+			// Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13));
+			// Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14));
+			// Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15));
+			// Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16));
+			// Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17));
+			// Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18));
+			// Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19));
+			// Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20));
+			// Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21));
+			Kfusion(22) = -HK22*HK32;
+			Kfusion(23) = -HK28*HK32;
+
+		}
+
+		// Apply an innovation consistency check with a 5 Sigma threshold
+		_drag_innov(axis_index) = pred_acc - mea_acc;
+		_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index));
+
+		// if the innovation consistency check fails then don't fuse the sample
+		if (_drag_test_ratio(axis_index) <= 1.0f) {
+			measurementUpdate(Kfusion, Hfusion, _drag_innov(axis_index));
+		}
+	}
+}

src/lib/ecl/EKF/ekf.cpp

@@ -0,0 +1,540 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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.cpp
+ * Core functions for ekf attitude and position estimator.
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ */
+
+#include "ekf.h"
+
+#include <ecl.h>
+#include <mathlib/mathlib.h>
+
+bool Ekf::init(uint64_t timestamp)
+{
+	bool ret = initialise_interface(timestamp);
+	reset();
+	return ret;
+}
+
+void Ekf::reset()
+{
+	_state.vel.setZero();
+	_state.pos.setZero();
+	_state.delta_ang_bias.setZero();
+	_state.delta_vel_bias.setZero();
+	_state.mag_I.setZero();
+	_state.mag_B.setZero();
+	_state.wind_vel.setZero();
+	_state.quat_nominal.setIdentity();
+
+	// TODO: who resets the output buffer content?
+	_output_new.vel.setZero();
+	_output_new.pos.setZero();
+	_output_new.quat_nominal.setIdentity();
+
+	_delta_angle_corr.setZero();
+
+	_imu_updated = false;
+	_NED_origin_initialised = false;
+	_gps_speed_valid = false;
+
+	_filter_initialised = false;
+	_terrain_initialised = false;
+	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+
+	_control_status.value = 0;
+	_control_status_prev.value = 0;
+
+	_dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
+
+	_ang_rate_delayed_raw.zero();
+
+	_fault_status.value = 0;
+	_innov_check_fail_status.value = 0;
+
+	_accel_magnitude_filt = 0.0f;
+	_ang_rate_magnitude_filt = 0.0f;
+	_prev_dvel_bias_var.zero();
+
+	_gps_alt_ref = 0.0f;
+
+	resetGpsDriftCheckFilters();
+}
+
+bool Ekf::update()
+{
+	bool updated = false;
+
+	if (!_filter_initialised) {
+		_filter_initialised = initialiseFilter();
+
+		if (!_filter_initialised) {
+			return false;
+		}
+	}
+
+	// Only run the filter if IMU data in the buffer has been updated
+	if (_imu_updated) {
+		// perform state and covariance prediction for the main filter
+		predictState();
+		predictCovariance();
+
+		// control fusion of observation data
+		controlFusionModes();
+
+		// run a separate filter for terrain estimation
+		runTerrainEstimator();
+
+		updated = true;
+
+		// run EKF-GSF yaw estimator
+		runYawEKFGSF();
+	}
+
+	// the output observer always runs
+	// Use full rate IMU data at the current time horizon
+	calculateOutputStates(_newest_high_rate_imu_sample);
+
+	return updated;
+}
+
+bool Ekf::initialiseFilter()
+{
+	// Filter accel for tilt initialization
+	const imuSample &imu_init = _imu_buffer.get_newest();
+
+	// protect against zero data
+	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
+		return false;
+	}
+
+	if (_is_first_imu_sample) {
+		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
+		_is_first_imu_sample = false;
+
+	} else {
+		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
+	}
+
+	// Sum the magnetometer measurements
+	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
+		if (_mag_sample_delayed.time_us != 0) {
+			if (_mag_counter == 0) {
+				_mag_lpf.reset(_mag_sample_delayed.mag);
+
+			} else {
+				_mag_lpf.update(_mag_sample_delayed.mag);
+			}
+
+			_mag_counter++;
+		}
+	}
+
+	// accumulate enough height measurements to be confident in the quality of the data
+	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
+		if (_baro_sample_delayed.time_us != 0) {
+			if (_baro_counter == 0) {
+				_baro_hgt_offset = _baro_sample_delayed.hgt;
+
+			} else {
+				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
+			}
+
+			_baro_counter++;
+		}
+	}
+
+	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+		if (_mag_counter < _obs_buffer_length) {
+			// not enough mag samples accumulated
+			return false;
+		}
+	}
+
+	if (_baro_counter < _obs_buffer_length) {
+		// not enough baro samples accumulated
+		return false;
+	}
+
+	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
+	setControlBaroHeight();
+
+	if (!initialiseTilt()) {
+		return false;
+	}
+
+	// calculate the initial magnetic field and yaw alignment
+	// but do not mark the yaw alignement complete as it needs to be
+	// reset once the leveling phase is done
+	resetMagHeading(_mag_lpf.getState(), false, false);
+
+	// initialise the state covariance matrix now we have starting values for all the states
+	initialiseCovariance();
+
+	// update the yaw angle variance using the variance of the measurement
+	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+		// using magnetic heading tuning parameter
+		increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
+	}
+
+	// try to initialise the terrain estimator
+	_terrain_initialised = initHagl();
+
+	// reset the essential fusion timeout counters
+	_time_last_hgt_fuse = _time_last_imu;
+	_time_last_hor_pos_fuse = _time_last_imu;
+	_time_last_delpos_fuse = _time_last_imu;
+	_time_last_hor_vel_fuse = _time_last_imu;
+	_time_last_hagl_fuse = _time_last_imu;
+	_time_last_flow_terrain_fuse = _time_last_imu;
+	_time_last_of_fuse = _time_last_imu;
+
+	// reset the output predictor state history to match the EKF initial values
+	alignOutputFilter();
+
+	return true;
+}
+
+bool Ekf::initialiseTilt()
+{
+	const float accel_norm = _accel_lpf.getState().norm();
+	const float gyro_norm = _gyro_lpf.getState().norm();
+
+	if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
+	    accel_norm > 1.2f * CONSTANTS_ONE_G ||
+	    gyro_norm > math::radians(15.0f)) {
+		return false;
+	}
+
+	// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
+	const Vector3f gravity_in_body = _accel_lpf.getState().normalized();
+	const float pitch = asinf(gravity_in_body(0));
+	const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2));
+
+	_state.quat_nominal = Quatf{Eulerf{roll, pitch, 0.0f}};
+	_R_to_earth = Dcmf(_state.quat_nominal);
+
+	return true;
+}
+
+void Ekf::predictState()
+{
+	// apply imu bias corrections
+	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
+
+	// subtract component of angular rate due to earth rotation
+	corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
+
+	const Quatf dq(AxisAnglef{corrected_delta_ang});
+
+	// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
+	_state.quat_nominal = (_state.quat_nominal * dq).normalized();
+
+	_R_to_earth = Dcmf(_state.quat_nominal);
+
+	// Calculate an earth frame delta velocity
+	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
+	const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
+
+	// calculate a filtered horizontal acceleration with a 1 sec time constant
+	// this are used for manoeuvre detection elsewhere
+	const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
+	_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
+
+	// save the previous value of velocity so we can use trapzoidal integration
+	const Vector3f vel_last = _state.vel;
+
+	// calculate the increment in velocity using the current orientation
+	_state.vel += corrected_delta_vel_ef;
+
+	// compensate for acceleration due to gravity
+	_state.vel(2) += CONSTANTS_ONE_G * _imu_sample_delayed.delta_vel_dt;
+
+	// predict position states via trapezoidal integration of velocity
+	_state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f;
+
+	constrainStates();
+
+	// calculate an average filter update time
+	float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
+
+	// filter and limit input between -50% and +100% of nominal value
+	input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
+	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
+
+	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
+	// protect angainst possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
+	// due to insufficient averaging
+	if (_imu_sample_delayed.delta_ang_dt > 0.25f * FILTER_UPDATE_PERIOD_S) {
+		_ang_rate_delayed_raw = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt;
+	}
+
+}
+
+/*
+ * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon.
+ * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon.
+ * Calculate delta angle, delta velocity and velocity corrections from the differences and apply them at the
+ * current time horizon so that the INS states track the EKF states at the delayed fusion time horizon.
+ * The inspiration for using a complementary filter to correct for time delays in the EKF
+ * is based on the work by A Khosravian:
+ * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements”
+ * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
+*/
+void Ekf::calculateOutputStates(const imuSample &imu)
+{
+	// Use full rate IMU data at the current time horizon
+
+	// correct delta angles for bias offsets
+	const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
+
+	// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
+	const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr);
+
+	// calculate a yaw change about the earth frame vertical
+	const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
+	_yaw_delta_ef += spin_del_ang_D;
+
+	// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
+	// Note fixed coefficients are used to save operations. The exact time constant is not important.
+	_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt;
+
+	const Quatf dq(AxisAnglef{delta_angle});
+
+	// rotate the previous INS quaternion by the delta quaternions
+	_output_new.time_us = imu.time_us;
+	_output_new.quat_nominal = _output_new.quat_nominal * dq;
+
+	// the quaternions must always be normalised after modification
+	_output_new.quat_nominal.normalize();
+
+	// calculate the rotation matrix from body to earth frame
+	_R_to_earth_now = Dcmf(_output_new.quat_nominal);
+
+	// correct delta velocity for bias offsets
+	const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
+
+	// rotate the delta velocity to earth frame
+	Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body};
+
+	// correct for measured acceleration due to gravity
+	delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
+
+	// calculate the earth frame velocity derivatives
+	if (imu.delta_vel_dt > 1e-4f) {
+		_vel_deriv = delta_vel_earth * (1.0f / imu.delta_vel_dt);
+	}
+
+	// save the previous velocity so we can use trapezoidal integration
+	const Vector3f vel_last(_output_new.vel);
+
+	// increment the INS velocity states by the measurement plus corrections
+	// do the same for vertical state used by alternative correction algorithm
+	_output_new.vel += delta_vel_earth;
+	_output_vert_new.vert_vel += delta_vel_earth(2);
+
+	// use trapezoidal integration to calculate the INS position states
+	// do the same for vertical state used by alternative correction algorithm
+	const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f);
+	_output_new.pos += delta_pos_NED;
+	_output_vert_new.vert_vel_integ += delta_pos_NED(2);
+
+	// accumulate the time for each update
+	_output_vert_new.dt += imu.delta_vel_dt;
+
+	// correct velocity for IMU offset
+	if (imu.delta_ang_dt > 1e-4f) {
+		// calculate the average angular rate across the last IMU update
+		const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt);
+
+		// calculate the velocity of the IMU relative to the body origin
+		const Vector3f vel_imu_rel_body = ang_rate % _params.imu_pos_body;
+
+		// rotate the relative velocity into earth frame
+		_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
+	}
+
+	// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
+	if (_imu_updated) {
+		_output_buffer.push(_output_new);
+		_output_vert_buffer.push(_output_vert_new);
+
+		// get the oldest INS state data from the ring buffer
+		// this data will be at the EKF fusion time horizon
+		// TODO: there is no guarantee that data is at delayed fusion horizon
+		//       Shouldnt we use pop_first_older_than?
+		const outputSample &output_delayed = _output_buffer.get_oldest();
+		const outputVert &output_vert_delayed = _output_vert_buffer.get_oldest();
+
+		// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
+		const Quatf q_error((_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized());
+
+		// convert the quaternion delta to a delta angle
+		const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;
+
+		const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)};
+
+		// calculate a gain that provides tight tracking of the estimator attitude states and
+		// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
+		const float time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6f, _dt_imu_avg);
+		const float att_gain = 0.5f * _dt_imu_avg / time_delay;
+
+		// calculate a corrrection to the delta angle
+		// that will cause the INS to track the EKF quaternions
+		_delta_angle_corr = delta_ang_error * att_gain;
+		_output_tracking_error(0) = delta_ang_error.norm();
+
+		/*
+		 * Loop through the output filter state history and apply the corrections to the velocity and position states.
+		 * This method is too expensive to use for the attitude states due to the quaternion operations required
+		 * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
+		 * to be used and reduces tracking error relative to EKF states.
+		 */
+
+		// Complementary filter gains
+		const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
+		const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
+
+		// calculate down velocity and position tracking errors
+		const float vert_vel_err = (_state.vel(2) - output_vert_delayed.vert_vel);
+		const float vert_vel_integ_err = (_state.pos(2) - output_vert_delayed.vert_vel_integ);
+
+		// calculate a velocity correction that will be applied to the output state history
+		// using a PD feedback tuned to a 5% overshoot
+		const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
+
+		applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
+
+		// calculate velocity and position tracking errors
+		const Vector3f vel_err(_state.vel - output_delayed.vel);
+		const Vector3f pos_err(_state.pos - output_delayed.pos);
+
+		_output_tracking_error(1) = vel_err.norm();
+		_output_tracking_error(2) = pos_err.norm();
+
+		// calculate a velocity correction that will be applied to the output state history
+		_vel_err_integ += vel_err;
+		const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
+
+		// calculate a position correction that will be applied to the output state history
+		_pos_err_integ += pos_err;
+		const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
+
+		applyCorrectionToOutputBuffer(vel_correction, pos_correction);
+	}
+}
+
+/*
+* Calculate a correction to be applied to vert_vel that casues vert_vel_integ to track the EKF
+* down position state at the fusion time horizon using an alternative algorithm to what
+* is used for the vel and pos state tracking. The algorithm applies a correction to the vert_vel
+* state history and propagates vert_vel_integ forward in time using the corrected vert_vel history.
+* This provides an alternative vertical velocity output that is closer to the first derivative
+* of the position but does degrade tracking relative to the EKF state.
+*/
+void Ekf::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
+{
+	// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
+	// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
+	uint8_t index = _output_vert_buffer.get_oldest_index();
+
+	const uint8_t size = _output_vert_buffer.get_length();
+
+	for (uint8_t counter = 0; counter < (size - 1); counter++) {
+		const uint8_t index_next = (index + 1) % size;
+		outputVert &current_state = _output_vert_buffer[index];
+		outputVert &next_state = _output_vert_buffer[index_next];
+
+		// correct the velocity
+		if (counter == 0) {
+			current_state.vert_vel += vert_vel_correction;
+		}
+
+		next_state.vert_vel += vert_vel_correction;
+
+		// position is propagated forward using the corrected velocity and a trapezoidal integrator
+		next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt;
+
+		// advance the index
+		index = (index + 1) % size;
+	}
+
+	// update output state to corrected values
+	_output_vert_new = _output_vert_buffer.get_newest();
+
+	// reset time delta to zero for the next accumulation of full rate IMU data
+	_output_vert_new.dt = 0.0f;
+}
+
+/*
+* Calculate corrections to be applied to vel and pos output state history.
+* The vel and pos state history are corrected individually so they track the EKF states at
+* the fusion time horizon. This option provides the most accurate tracking of EKF states.
+*/
+void Ekf::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction)
+{
+	// loop through the output filter state history and apply the corrections to the velocity and position states
+	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
+		// a constant velocity correction is applied
+		_output_buffer[index].vel += vel_correction;
+
+		// a constant position correction is applied
+		_output_buffer[index].pos += pos_correction;
+	}
+
+	// update output state to corrected values
+	_output_new = _output_buffer.get_newest();
+}
+
+/*
+ * Predict the previous quaternion output state forward using the latest IMU delta angle data.
+*/
+Quatf Ekf::calculate_quaternion() const
+{
+	// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
+	// corrections required to track the EKF quaternion states
+	const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr};
+
+	// increment the quaternions using the corrected delta angle vector
+	// the quaternions must always be normalised after modification
+	return Quatf{_output_new.quat_nominal * AxisAnglef{delta_angle}}.unit();
+}

src/lib/ecl/EKF/ekf.h

@@ -0,0 +1,995 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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.h
+ * Class for core functions for ekf attitude and position estimator.
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+
+#pragma once
+
+#include "estimator_interface.h"
+
+#include "EKFGSF_yaw.h"
+
+class Ekf final : public EstimatorInterface
+{
+public:
+	static constexpr uint8_t _k_num_states{24};		///< number of EKF states
+	typedef matrix::Vector<float, _k_num_states> Vector24f;
+	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
+	typedef matrix::SquareMatrix<float, 2> Matrix2f;
+	typedef matrix::Vector<float, 4> Vector4f;
+	template<int ... Idxs>
+	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+
+	Ekf() = default;
+	virtual ~Ekf() = default;
+
+	// initialise variables to sane values (also interface class)
+	bool init(uint64_t timestamp) override;
+
+	// should be called every time new data is pushed into the filter
+	bool update();
+
+	void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
+	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
+	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
+
+	void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
+	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
+	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
+
+	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); }
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); }
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); }
+
+	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); }
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
+
+	void getAuxVelInnov(float aux_vel_innov[2]) const;
+	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
+	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
+
+	void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
+	void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
+	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; }
+	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
+	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
+	const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
+	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; }
+	const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; }
+
+	void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
+	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
+
+	void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
+	void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
+	void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
+	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
+
+	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
+	void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
+	void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
+
+	void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; }
+	void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; }
+	void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; }
+
+	void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; }
+	void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; }
+	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; }
+
+	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
+	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
+	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
+
+	// get the state vector at the delayed time horizon
+	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
+
+	// get the wind velocity in m/s
+	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
+
+	// get the wind velocity var
+	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
+
+	// get the true airspeed in m/s
+	void get_true_airspeed(float *tas) const;
+
+	// get the full covariance matrix
+	const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
+
+	// get the diagonal elements of the covariance matrix
+	matrix::Vector<float, 24> covariances_diagonal() const { return P.diag(); }
+
+	// get the orientation (quaterion) covariances
+	matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); }
+
+	// get the linear velocity covariances
+	matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<3, 3>(4, 4); }
+
+	// get the position covariances
+	matrix::SquareMatrix<float, 3> position_covariances() const { return P.slice<3, 3>(7, 7); }
+
+	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
+	bool collect_gps(const gps_message &gps) override;
+
+	// get the ekf WGS-84 origin position and height and the system time it was last set
+	// return true if the origin is valid
+	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
+	bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
+
+	float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; }
+	bool setEkfGlobalOriginAltitude(const float altitude);
+
+
+	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
+	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
+
+	// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
+	void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const;
+
+	// get the 1-sigma horizontal and vertical velocity uncertainty
+	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const;
+
+	// get the vehicle control limits required by the estimator to keep within sensor limitations
+	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
+
+	// Reset all IMU bias states and covariances to initial alignment values.
+	void resetImuBias();
+	void resetGyroBias();
+	void resetAccelBias();
+
+	// Reset all magnetometer bias states and covariances to initial alignment values.
+	void resetMagBias();
+
+	Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
+
+	Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); }
+
+	// return an array containing the output predictor angular, velocity and position tracking
+	// error magnitudes (rad), (m/sec), (m)
+	const Vector3f &getOutputTrackingError() const { return _output_tracking_error; }
+
+	/*
+	First argument returns GPS drift  metrics in the following array locations
+	0 : Horizontal position drift rate (m/s)
+	1 : Vertical position drift rate (m/s)
+	2 : Filtered horizontal velocity (m/s)
+	Second argument returns true when IMU movement is blocking the drift calculation
+	Function returns true if the metrics have been updated and not returned previously by this function
+	*/
+	bool get_gps_drift_metrics(float drift[3], bool *blocked);
+
+	// return true if the global position estimate is valid
+	// return true if the origin is set we are not doing unconstrained free inertial navigation
+	// and have not started using synthetic position observations to constrain drift
+	bool global_position_is_valid() const
+	{
+		return (_NED_origin_initialised && local_position_is_valid());
+	}
+
+	// return true if the local position estimate is valid
+	bool local_position_is_valid() const
+	{
+		return (!_deadreckon_time_exceeded && !_using_synthetic_position);
+	}
+
+	bool isTerrainEstimateValid() const { return _hagl_valid; };
+
+	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
+
+	// get the estimated terrain vertical position relative to the NED origin
+	float getTerrainVertPos() const { return _terrain_vpos; };
+
+	// get the number of times the vertical terrain position has been reset
+	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
+
+	// get the terrain variance
+	float get_terrain_var() const { return _terrain_var; }
+
+	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
+	Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
+	const Vector3f &getMagBias() const { return _state.mag_B; }
+
+	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / _dt_ekf_avg; } // get the gyroscope bias variance in rad/s
+	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / _dt_ekf_avg; } // get the accelerometer bias variance in m/s**2
+	Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; }
+
+	// get GPS check status
+	void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; }
+
+	const auto &state_reset_status() const { return _state_reset_status; }
+
+	// return the amount the local vertical position changed in the last reset and the number of reset events
+	void get_posD_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.posD_change;
+		*counter = _state_reset_status.posD_counter;
+	}
+
+	// return the amount the local vertical velocity changed in the last reset and the number of reset events
+	void get_velD_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.velD_change;
+		*counter = _state_reset_status.velD_counter;
+	}
+
+	// return the amount the local horizontal position changed in the last reset and the number of reset events
+	void get_posNE_reset(float delta[2], uint8_t *counter) const
+	{
+		_state_reset_status.posNE_change.copyTo(delta);
+		*counter = _state_reset_status.posNE_counter;
+	}
+
+	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
+	void get_velNE_reset(float delta[2], uint8_t *counter) const
+	{
+		_state_reset_status.velNE_change.copyTo(delta);
+		*counter = _state_reset_status.velNE_counter;
+	}
+
+	// return the amount the quaternion has changed in the last reset and the number of reset events
+	void get_quat_reset(float delta_quat[4], uint8_t *counter) const
+	{
+		_state_reset_status.quat_change.copyTo(delta_quat);
+		*counter = _state_reset_status.quat_counter;
+	}
+
+	// get EKF innovation consistency check status information comprising of:
+	// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
+	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
+	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
+	// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
+	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
+					float &hagl, float &beta) const;
+
+	// return a bitmask integer that describes which state estimates can be used for flight control
+	void get_ekf_soln_status(uint16_t *status) const;
+
+	// return the quaternion defining the rotation from the External Vision to the EKF reference frame
+	matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
+
+	// use the latest IMU data at the current time horizon.
+	Quatf calculate_quaternion() const;
+
+	// set minimum continuous period without GPS fail required to mark a healthy GPS status
+	void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
+
+	// get solution data from the EKF-GSF emergency yaw estimator
+	// returns false when data is not available
+	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
+			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+
+private:
+
+	// set the internal states and status to their default value
+	void reset();
+
+	bool initialiseTilt();
+
+	// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
+	// and reset the velocity and position states to the GPS. This will cause the EKF
+	// to ignore the magnetometer for the remainder of flight.
+	// This should only be used as a last resort before activating a loss of navigation failsafe
+	void requestEmergencyNavReset() { _do_ekfgsf_yaw_reset = true; }
+
+	// check if the EKF is dead reckoning horizontal velocity using inertial data only
+	void update_deadreckoning_status();
+
+	void updateTerrainValidity();
+
+	struct {
+		uint8_t velNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t velD_counter;	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
+		uint8_t posNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
+		uint8_t posD_counter;	///< number of vertical position reset events (allow to wrap if count exceeds 255)
+		uint8_t quat_counter;	///< number of quaternion reset events (allow to wrap if count exceeds 255)
+		Vector2f velNE_change;  ///< North East velocity change due to last reset (m)
+		float velD_change;	///< Down velocity change due to last reset (m/sec)
+		Vector2f posNE_change;	///< North, East position change due to last reset (m)
+		float posD_change;	///< Down position change due to last reset (m)
+		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
+	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
+
+	float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
+
+	Vector3f _ang_rate_delayed_raw;	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
+
+	stateSample _state{};		///< state struct of the ekf running at the delayed time horizon
+
+	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
+
+	// variables used when position data is being fused using a relative position odometry model
+	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
+	Vector3f _pos_meas_prev;		///< previous value of NED position measurement fused using odometry assumption (m)
+	Vector2f _hpos_pred_prev;		///< previous value of NE position state used by odometry fusion (m)
+	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
+	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
+	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
+
+	// booleans true when fresh sensor data is available at the fusion time horizon
+	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
+	bool _mag_data_ready{false};	///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
+	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
+	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
+	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
+	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
+	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
+
+	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)
+	uint64_t _time_last_aiding{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
+	bool _using_synthetic_position{false};	///< true if we are using a synthetic position to constrain drift
+
+	uint64_t _time_last_hor_pos_fuse{0};	///< time the last fusion of horizontal position measurements was performed (uSec)
+	uint64_t _time_last_hgt_fuse{0};	///< time the last fusion of vertical position measurements was performed (uSec)
+	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
+	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
+	uint64_t _time_last_delpos_fuse{0};	///< time the last fusion of incremental horizontal position measurements was performed (uSec)
+	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
+	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
+	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
+	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
+	uint64_t _time_last_fake_pos{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
+
+	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
+	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
+	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
+
+
+	Vector2f _last_known_posNE;		///< last known local NE position vector (m)
+	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
+
+	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
+	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
+
+	Vector3f _earth_rate_NED;	///< earth rotation vector (NED) in rad/s
+
+	Dcmf _R_to_earth;	///< transformation matrix from body frame to earth frame from last EKF prediction
+
+	// used by magnetometer fusion mode selection
+	Vector2f _accel_lpf_NE;			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
+	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
+	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
+	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
+	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
+	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
+	uint8_t _num_bad_flight_yaw_events{0};	///< number of times a bad heading has been detected in flight and required a yaw reset
+	uint64_t _mag_use_not_inhibit_us{0};	///< last system time in usec before magnetometer use was inhibited
+	bool _mag_inhibit_yaw_reset_req{false};	///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
+	float _last_static_yaw{0.0f};		///< last yaw angle recorded when on ground motion checks were passing (rad)
+	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
+	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
+	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements
+	bool _non_mag_yaw_aiding_running_prev{false};  ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
+
+	bool _is_yaw_fusion_inhibited{false};		///< true when yaw sensor use is being inhibited
+
+	SquareMatrix24f P;	///< state covariance matrix
+
+	Vector3f _delta_vel_bias_var_accum;		///< kahan summation algorithm accumulator for delta velocity bias variance
+	Vector3f _delta_angle_bias_var_accum;	///< kahan summation algorithm accumulator for delta angle bias variance
+
+	Vector3f _last_vel_obs;			///< last velocity observation (m/s)
+	Vector3f _last_vel_obs_var;		///< last velocity observation variance (m/s)**2
+	Vector2f _last_fail_hvel_innov;		///< last failed horizontal velocity innovation (m/s)**2
+	float _vert_pos_innov_ratio{0.f};	///< vertical position innovation divided by estimated standard deviation of innovation
+	uint64_t _vert_pos_fuse_attempt_time_us{0};	///< last system time in usec vertical position measurement fuson was attempted
+	float _vert_vel_innov_ratio{0.f};		///< standard deviation of vertical velocity innovation
+	uint64_t _vert_vel_fuse_time_us{0};	///< last system time in usec time vertical velocity measurement fuson was attempted
+
+	Vector3f _gps_vel_innov;	///< GPS velocity innovations (m/sec)
+	Vector3f _gps_vel_innov_var;	///< GPS velocity innovation variances ((m/sec)**2)
+
+	Vector3f _gps_pos_innov;	///< GPS position innovations (m)
+	Vector3f _gps_pos_innov_var;	///< GPS position innovation variances (m**2)
+
+	Vector3f _ev_vel_innov;	///< external vision velocity innovations (m/sec)
+	Vector3f _ev_vel_innov_var;	///< external vision velocity innovation variances ((m/sec)**2)
+
+	Vector3f _ev_pos_innov;	///< external vision position innovations (m)
+	Vector3f _ev_pos_innov_var;	///< external vision position innovation variances (m**2)
+
+	Vector3f _baro_hgt_innov;		///< baro hgt innovations (m)
+	Vector3f _baro_hgt_innov_var;	///< baro hgt innovation variances (m**2)
+
+	Vector3f _rng_hgt_innov;	///< range hgt innovations (m)
+	Vector3f _rng_hgt_innov_var;	///< range hgt innovation variances (m**2)
+
+	Vector3f _aux_vel_innov;	///< horizontal auxiliary velocity innovations: (m/sec)
+	Vector3f _aux_vel_innov_var;	///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
+
+	float _heading_innov{0.0f};	///< heading measurement innovation (rad)
+	float _heading_innov_var{0.0f};	///< heading measurement innovation variance (rad**2)
+
+	Vector3f _mag_innov;		///< earth magnetic field innovations (Gauss)
+	Vector3f _mag_innov_var;	///< earth magnetic field innovation variance (Gauss**2)
+
+	Vector2f _drag_innov;		///< multirotor drag measurement innovation (m/sec**2)
+	Vector2f _drag_innov_var;	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
+
+	float _airspeed_innov{0.0f};		///< airspeed measurement innovation (m/sec)
+	float _airspeed_innov_var{0.0f};	///< airspeed measurement innovation variance ((m/sec)**2)
+
+	float _beta_innov{0.0f};	///< synthetic sideslip measurement innovation (rad)
+	float _beta_innov_var{0.0f};	///< synthetic sideslip measurement innovation variance (rad**2)
+
+	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
+	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
+
+	// optical flow processing
+	Vector2f _flow_innov;		///< flow measurement innovation (rad/sec)
+	Vector2f _flow_innov_var;	///< flow innovation variance ((rad/sec)**2)
+	Vector3f _flow_gyro_bias;	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
+	Vector2f _flow_vel_body;	///< velocity from corrected flow measurement (body frame)(m/s)
+	Vector2f _flow_vel_ne;		///< velocity from corrected flow measurement (local frame) (m/s)
+	Vector3f _imu_del_ang_of;	///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
+	float _delta_time_of{0.0f};	///< time in sec that _imu_del_ang_of was accumulated over (sec)
+	uint64_t _time_bad_motion_us{0};	///< last system time that on-ground motion exceeded limits (uSec)
+	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
+	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
+	Vector2f _flow_compensated_XY_rad;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
+
+	// output predictor states
+	Vector3f _delta_angle_corr;	///< delta angle correction vector (rad)
+	Vector3f _vel_err_integ;	///< integral of velocity tracking error (m)
+	Vector3f _pos_err_integ;	///< integral of position tracking error (m.s)
+	Vector3f _output_tracking_error; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
+
+	// variables used for the GPS quality checks
+	Vector3f _gps_pos_deriv_filt;	///< GPS NED position derivative (m/sec)
+	Vector2f _gps_velNE_filt;	///< filtered GPS North and East velocity (m/sec)
+	float _gps_velD_diff_filt{0.0f};	///< GPS filtered Down velocity (m/sec)
+	uint64_t _last_gps_fail_us{0};		///< last system time in usec that the GPS failed it's checks
+	uint64_t _last_gps_pass_us{0};		///< last system time in usec that the GPS passed it's checks
+	float _gps_error_norm{1.0f};		///< normalised gps error
+	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
+	bool _gps_checks_passed{false};		///> true when all active GPS checks have passed
+
+	// Variables used to publish the WGS-84 location of the EKF local NED origin
+	uint64_t _last_gps_origin_time_us{0};	///< time the origin was last set (uSec)
+	float _gps_alt_ref{0.0f};		///< WGS-84 height (m)
+
+	// Variables used by the initial filter alignment
+	bool _is_first_imu_sample{true};
+	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
+	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
+	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+
+	// Variables used to perform in flight resets and switch between height sources
+	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
+	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
+	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
+
+	// Variables used to control activation of post takeoff functionality
+	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
+	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
+	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
+	float _saved_mag_bf_variance[4] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
+	Matrix2f _saved_mag_ef_covmat;		///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
+	bool _velpos_reset_request{false};	///< true when a large yaw error has been fixed and a velocity and position state reset is required
+
+	gps_check_fail_status_u _gps_check_fail_status{};
+
+	// variables used to inhibit accel bias learning
+	bool _accel_bias_inhibit[3] {};		///< true when the accel bias learning is being inhibited for the specified axis
+	Vector3f _accel_vec_filt;		///< acceleration vector after application of a low pass filter (m/sec**2)
+	float _accel_magnitude_filt{0.0f};	///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
+	float _ang_rate_magnitude_filt{0.0f};		///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
+	Vector3f _prev_dvel_bias_var;		///< saved delta velocity XYZ bias variances (m/sec)**2
+
+	// Terrain height state estimation
+	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
+	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
+	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
+	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
+	uint64_t _time_last_fake_hagl_fuse{0};	///< last system time that a fake range sample was fused by the terrain estimator
+	bool _terrain_initialised{false};	///< true when the terrain estimator has been initialized
+	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
+	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
+
+	// height sensor status
+	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
+	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
+	bool _is_gps_yaw_faulty{false};		///< true if gps yaw data is rejected by the filter for too long
+
+	// imu fault status
+	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
+	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
+	bool _bad_vert_accel_detected{false};	///< true when bad vertical accelerometer data has been detected
+	uint16_t _clip_counter{0};		///< counter that increments when clipping ad decrements when not
+
+	// variables used to control range aid functionality
+	bool _is_range_aid_suitable{false};	///< true when range finder can be used in flight as the height reference instead of the primary height sensor
+
+	float _height_rate_lpf{0.0f};
+
+	// update the real time complementary filter states. This includes the prediction
+	// and the correction step
+	void calculateOutputStates(const imuSample &imu);
+	void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
+	void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction);
+
+	// initialise filter states of both the delayed ekf and the real time complementary filter
+	bool initialiseFilter(void);
+
+	// initialise ekf covariance matrix
+	void initialiseCovariance();
+
+	// predict ekf state
+	void predictState();
+
+	// predict ekf covariance
+	void predictCovariance();
+
+	// ekf sequential fusion of magnetometer measurements
+	void fuseMag();
+
+	// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
+	void fuseHeading();
+
+	// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
+	// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
+	// yaw_variance : variance of the yaw angle observation (rad^2)
+	// zero_innovation : Fuse data with innovation set to zero
+	void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation);
+
+	// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
+	// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
+	// yaw_variance : variance of the yaw angle observation (rad^2)
+	// zero_innovation : Fuse data with innovation set to zero
+	void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation);
+
+	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
+	// innovation : prediction - measurement
+	// variance : observaton variance
+	// gate_sigma : innovation consistency check gate size (Sigma)
+	// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
+	void updateQuaternion(const float innovation, const float variance, const float gate_sigma,
+			      const Vector4f &yaw_jacobian);
+
+	// fuse the yaw angle obtained from a dual antenna GPS unit
+	void fuseGpsYaw();
+
+	// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
+	// return true if the reset was successful
+	bool resetYawToGps();
+
+	// fuse magnetometer declination measurement
+	// argument passed in is the declination uncertainty in radians
+	void fuseDeclination(float decl_sigma);
+
+	// apply sensible limits to the declination and length of the NE mag field states estimates
+	void limitDeclination();
+
+	// fuse airspeed measurement
+	void fuseAirspeed();
+
+	// fuse synthetic zero sideslip measurement
+	void fuseSideslip();
+
+	// fuse body frame drag specific forces for multi-rotor wind estimation
+	void fuseDrag();
+
+	// fuse single velocity and position measurement
+	void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
+
+	void resetVelocity();
+
+	void resetVelocityToGps();
+
+	inline void resetHorizontalVelocityToOpticalFlow();
+
+	inline void resetVelocityToVision();
+
+	inline void resetHorizontalVelocityToZero();
+
+	inline void resetVelocityTo(const Vector3f &vel);
+
+	inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
+
+	inline void resetVerticalVelocityTo(float new_vert_vel);
+
+	void resetHorizontalPosition();
+
+	void resetHorizontalPositionToGps();
+
+	inline void resetHorizontalPositionToVision();
+
+	inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
+
+	inline void resetVerticalPositionTo(const float &new_vert_pos);
+
+	void resetHeight();
+
+	// fuse optical flow line of sight rate measurements
+	void fuseOptFlow();
+
+	bool fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
+				    Vector3f &innov_var, Vector2f &test_ratio);
+
+	bool fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
+				  Vector3f &innov_var, Vector2f &test_ratio);
+
+	bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
+				    Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
+
+	bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
+				  Vector3f &innov_var, Vector2f &test_ratio);
+
+	// calculate optical flow body angular rate compensation
+	// returns false if bias corrected body rate data is unavailable
+	bool calcOptFlowBodyRateComp();
+
+	// initialise the terrain vertical position estimator
+	// return true if the initialisation is successful
+	bool initHagl();
+
+	bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); }
+	bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); }
+
+	// run the terrain estimator
+	void runTerrainEstimator();
+
+	// update the terrain vertical position estimate using a height above ground measurement from the range finder
+	void fuseHagl();
+
+	// update the terrain vertical position estimate using an optical flow measurement
+	void fuseFlowForTerrain();
+
+	// reset the heading and magnetic field states using the declination and magnetometer measurements
+	// return true if successful
+	bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true);
+
+	// reset the heading using the external vision measurements
+	// return true if successful
+	bool resetYawToEv();
+
+	// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
+	// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
+	bool realignYawGPS();
+
+	// Return the magnetic declination in radians to be used by the alignment and fusion processing
+	float getMagDeclination();
+
+	// modify output filter to match the the EKF state at the fusion time horizon
+	void alignOutputFilter();
+
+	// update the rotation matrix which transforms EV navigation frame measurements into NED
+	void calcExtVisRotMat();
+
+	Vector3f getVisionVelocityInEkfFrame() const;
+
+	Vector3f getVisionVelocityVarianceInEkfFrame() const;
+
+	// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
+	// that is optimized by exploring the sparsity in H
+	template <size_t ...Idxs>
+	SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
+	{
+		SquareMatrix24f KHP;
+		constexpr size_t non_zeros = sizeof...(Idxs);
+		float KH[non_zeros];
+
+		for (unsigned row = 0; row < _k_num_states; row++) {
+			for (unsigned i = 0; i < H.non_zeros(); i++) {
+				KH[i] = K(row) * H.atCompressedIndex(i);
+			}
+
+			for (unsigned column = 0; column < _k_num_states; column++) {
+				float tmp = 0.f;
+
+				for (unsigned i = 0; i < H.non_zeros(); i++) {
+					const size_t index = H.index(i);
+					tmp += KH[i] * P(index, column);
+				}
+
+				KHP(row, column) = tmp;
+			}
+		}
+
+		return KHP;
+	}
+
+	// measurement update with a single measurement
+	// returns true if fusion is performed
+	template <size_t ...Idxs>
+	bool measurementUpdate(Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
+	{
+		for (unsigned i = 0; i < 3; i++) {
+			if (_accel_bias_inhibit[i]) {
+				K(13 + i) = 0.0f;
+			}
+		}
+
+		// apply covariance correction via P_new = (I -K*H)*P
+		// first calculate expression for KHP
+		// then calculate P - KHP
+		const SquareMatrix24f KHP = computeKHP(K, H);
+
+		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
+
+		if (is_healthy) {
+			// apply the covariance corrections
+			P -= KHP;
+
+			fixCovarianceErrors(true);
+
+			// apply the state corrections
+			fuse(K, innovation);
+		}
+
+		return is_healthy;
+	}
+
+	// if the covariance correction will result in a negative variance, then
+	// the covariance matrix is unhealthy and must be corrected
+	bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP);
+
+	// limit the diagonal of the covariance matrix
+	// force symmetry when the argument is true
+	void fixCovarianceErrors(bool force_symmetry);
+
+	// constrain the ekf states
+	void constrainStates();
+
+	// generic function which will perform a fusion step given a kalman gain K
+	// and a scalar innovation value
+	void fuse(const Vector24f &K, float innovation);
+
+	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override;
+
+	// calculate the earth rotation vector from a given latitude
+	Vector3f calcEarthRateNED(float lat_rad) const;
+
+	// return true id the GPS quality is good enough to set an origin and start aiding
+	bool gps_is_good(const gps_message &gps);
+
+	// Control the filter fusion modes
+	void controlFusionModes();
+
+	// control fusion of external vision observations
+	void controlExternalVisionFusion();
+
+	// control fusion of optical flow observations
+	void controlOpticalFlowFusion();
+	void updateOnGroundMotionForOpticalFlowChecks();
+	void resetOnGroundMotionForOpticalFlowChecks();
+
+	// control fusion of GPS observations
+	void controlGpsFusion();
+	void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
+
+	// control fusion of magnetometer observations
+	void controlMagFusion();
+
+	bool noOtherYawAidingThanMag() const;
+	bool otherHeadingSourcesHaveStopped();
+
+	void checkHaglYawResetReq();
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
+
+	void runOnGroundYawReset();
+	bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }
+	bool canResetMagHeading() const;
+	void runInAirYawReset();
+	bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; }
+	void runVelPosReset();
+
+	void selectMagAuto();
+	void check3DMagFusionSuitability();
+	void checkYawAngleObservability();
+	void checkMagBiasObservability();
+	bool isYawAngleObservable() const { return _yaw_angle_observable; }
+	bool isMagBiasObservable() const { return _mag_bias_observable; }
+	bool canUse3DMagFusion() const;
+
+	void checkMagDeclRequired();
+	void checkMagInhibition();
+	bool shouldInhibitMag() const;
+	void checkMagFieldStrength();
+	bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; }
+	bool isMeasuredMatchingGpsMagStrength() const;
+	bool isMeasuredMatchingAverageMagStrength() const;
+	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
+	void runMagAndMagDeclFusions();
+	void run3DMagAndDeclFusions();
+
+	// control fusion of range finder observations
+	void controlRangeFinderFusion();
+
+	// control fusion of air data observations
+	void controlAirDataFusion();
+
+	// control fusion of synthetic sideslip observations
+	void controlBetaFusion();
+
+	// control fusion of multi-rotor drag specific force observations
+	void controlDragFusion();
+
+	// control fusion of pressure altitude observations
+	void controlBaroFusion();
+
+	// control fusion of fake position observations to constrain drift
+	void controlFakePosFusion();
+
+	// control fusion of auxiliary velocity observations
+	void controlAuxVelFusion();
+
+	// control for height sensor timeouts, sensor changes and state resets
+	void controlHeightSensorTimeouts();
+
+	void checkVerticalAccelerationHealth();
+
+	// control for combined height fusion mode (implemented for switching between baro and range height)
+	void controlHeightFusion();
+
+	// determine if flight condition is suitable to use range finder instead of the primary height sensor
+	void checkRangeAidSuitability();
+	bool isRangeAidSuitable() const { return _is_range_aid_suitable; }
+
+	// set control flags to use baro height
+	void setControlBaroHeight();
+
+	// set control flags to use range height
+	void setControlRangeHeight();
+
+	// set control flags to use GPS height
+	void setControlGPSHeight();
+
+	// set control flags to use external vision height
+	void setControlEVHeight();
+
+	void stopMagFusion();
+	void stopMag3DFusion();
+	void stopMagHdgFusion();
+	void startMagHdgFusion();
+	void startMag3DFusion();
+
+	void startBaroHgtFusion();
+	void startGpsHgtFusion();
+
+	void updateBaroHgtOffset();
+
+	// return an estimation of the GPS altitude variance
+	float getGpsHeightVariance();
+
+	// calculate the measurement variance for the optical flow sensor
+	float calcOptFlowMeasVar();
+
+	// rotate quaternion covariances into variances for an equivalent rotation vector
+	Vector3f calcRotVecVariances();
+
+	// initialise the quaternion covariances using rotation vector variances
+	// do not call before quaternion states are initialised
+	void initialiseQuatCovariances(Vector3f &rot_vec_var);
+
+	// perform a limited reset of the magnetic field related state covariances
+	void resetMagRelatedCovariances();
+
+	void resetQuatCov();
+	void zeroQuatCov();
+	void resetMagCov();
+
+	// perform a limited reset of the wind state covariances
+	void resetWindCovariance();
+
+	// perform a reset of the wind states
+	void resetWindStates();
+
+	// check that the range finder data is continuous
+	void updateRangeDataContinuity();
+
+	// Increase the yaw error variance of the quaternions
+	// Argument is additional yaw variance in rad**2
+	void increaseQuatYawErrVariance(float yaw_variance);
+
+	// load and save mag field state covariance data for re-use
+	void loadMagCovData();
+	void saveMagCovData();
+	void clearMagCov();
+	void zeroMagCov();
+
+	void resetZDeltaAngBiasCov();
+
+	// uncorrelate quaternion states from other states
+	void uncorrelateQuatFromOtherStates();
+
+	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
+	// sensor measurement
+	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
+
+	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
+	{
+		return last_sensor_timestamp + timeout_period < _time_last_imu;
+	}
+
+	bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
+	{
+		return sensor_timestamp + acceptance_interval > _time_last_imu;
+	}
+
+	void startGpsFusion();
+	void stopGpsFusion();
+	void stopGpsPosFusion();
+	void stopGpsVelFusion();
+
+	void startGpsYawFusion();
+	void stopGpsYawFusion();
+
+	void startEvPosFusion();
+	void startEvVelFusion();
+	void startEvYawFusion();
+
+	void stopEvFusion();
+	void stopEvPosFusion();
+	void stopEvVelFusion();
+	void stopEvYawFusion();
+
+	void stopAuxVelFusion();
+
+	void stopFlowFusion();
+
+	void setVelPosFaultStatus(const int index, const bool status);
+
+	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
+	// yaw : Euler yaw angle (rad)
+	// yaw_variance : yaw error variance (rad^2)
+	// update_buffer : true if the state change should be also applied to the output observer buffer
+	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer);
+
+	// Declarations used to control use of the EKF-GSF yaw estimator
+
+	// yaw estimator instance
+	EKFGSF_yaw _yawEstimator;
+
+	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
+	bool _do_ekfgsf_yaw_reset{false};	// true when an emergency yaw reset has been requested
+	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
+
+	// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
+	void runYawEKFGSF();
+
+	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
+	// Resets the horizontal velocity and position to the default navigation sensor
+	// Returns true if the reset was successful
+	bool resetYawToEKFGSF();
+
+	void resetGpsDriftCheckFilters();
+};

src/lib/ecl/EKF/estimator_interface.cpp

@@ -0,0 +1,586 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 estimator_interface.cpp
+ * Definition of base class for attitude estimators
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
+ */
+
+#include "estimator_interface.h"
+
+#include <ecl.h>
+#include <mathlib/mathlib.h>
+
+// Accumulate imu data and store to buffer at desired rate
+void EstimatorInterface::setIMUData(const imuSample &imu_sample)
+{
+	// TODO: resolve misplaced responsibility
+	if (!_initialised) {
+		_initialised = init(imu_sample.time_us);
+	}
+
+	const float dt = math::constrain((imu_sample.time_us - _time_last_imu) / 1e6f, 1.0e-4f, 0.02f);
+
+	_time_last_imu = imu_sample.time_us;
+
+	if (_time_last_imu > 0) {
+		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
+	}
+
+	_newest_high_rate_imu_sample = imu_sample;
+
+	// Do not change order of computeVibrationMetric and checkIfVehicleAtRest
+	computeVibrationMetric(imu_sample);
+	_control_status.flags.vehicle_at_rest = checkIfVehicleAtRest(dt, imu_sample);
+
+	_imu_updated = _imu_down_sampler.update(imu_sample);
+
+	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
+	if (_imu_updated) {
+
+		_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
+
+		// get the oldest data from the buffer
+		_imu_sample_delayed = _imu_buffer.get_oldest();
+
+		// calculate the minimum interval between observations required to guarantee no loss of data
+		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
+		_min_obs_interval_us = (imu_sample.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
+
+		setDragData(imu_sample);
+	}
+}
+
+void EstimatorInterface::computeVibrationMetric(const imuSample &imu)
+{
+	// calculate a metric which indicates the amount of coning vibration
+	Vector3f temp = imu.delta_ang % _delta_ang_prev;
+	_vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm();
+
+	// calculate a metric which indicates the amount of high frequency gyro vibration
+	temp = imu.delta_ang - _delta_ang_prev;
+	_delta_ang_prev = imu.delta_ang;
+	_vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm();
+
+	// calculate a metric which indicates the amount of high frequency accelerometer vibration
+	temp = imu.delta_vel - _delta_vel_prev;
+	_delta_vel_prev = imu.delta_vel;
+	_vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm();
+}
+
+bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu)
+{
+	// detect if the vehicle is not moving when on ground
+	if (!_control_status.flags.in_air) {
+		if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)
+		    || (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler)
+		    || ((imu.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
+
+			_time_last_move_detect_us = imu.time_us;
+		}
+
+		return ((imu.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
+
+	} else {
+		_time_last_move_detect_us = imu.time_us;
+		return false;
+	}
+}
+
+
+void EstimatorInterface::setMagData(const magSample &mag_sample)
+{
+	if (!_initialised || _mag_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_mag_buffer.get_length() < _obs_buffer_length) {
+		_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
+
+		if (_mag_buffer_fail) {
+			printBufferAllocationFailed("mag");
+			return;
+		}
+	}
+
+	// downsample to highest possible sensor rate
+	// by taking the average of incoming sample
+	_mag_sample_count++;
+	_mag_data_sum += mag_sample.mag;
+	_mag_timestamp_sum += mag_sample.time_us / 1000; // Dividing by 1000 to avoid overflow
+
+	// limit data rate to prevent data being lost
+	if ((mag_sample.time_us - _time_last_mag) > _min_obs_interval_us) {
+		_time_last_mag = mag_sample.time_us;
+
+		magSample mag_sample_new;
+
+		// Use the time in the middle of the downsampling interval for the sample
+		mag_sample_new.time_us = 1000 * (_mag_timestamp_sum / _mag_sample_count);
+		mag_sample_new.time_us -= _params.mag_delay_ms * 1000;
+		mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		mag_sample_new.mag = _mag_data_sum / _mag_sample_count;
+
+		_mag_buffer.push(mag_sample_new);
+
+		_mag_sample_count = 0;
+		_mag_data_sum.setZero();
+		_mag_timestamp_sum = 0;
+	}
+}
+
+void EstimatorInterface::setGpsData(const gps_message &gps)
+{
+	if (!_initialised || _gps_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_gps_buffer.get_length() < _obs_buffer_length) {
+		_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
+
+		if (_gps_buffer_fail) {
+			printBufferAllocationFailed("GPS");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	const bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
+
+	// TODO: remove checks that are not timing related
+	if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
+		_time_last_gps = gps.time_usec;
+
+		gpsSample gps_sample_new;
+
+		gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000;
+		gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		gps_sample_new.vel = gps.vel_ned;
+
+		_gps_speed_valid = gps.vel_ned_valid;
+		gps_sample_new.sacc = gps.sacc;
+		gps_sample_new.hacc = gps.eph;
+		gps_sample_new.vacc = gps.epv;
+
+		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
+
+		gps_sample_new.yaw = gps.yaw;
+
+		if (ISFINITE(gps.yaw_offset)) {
+			_gps_yaw_offset = gps.yaw_offset;
+
+		} else {
+			_gps_yaw_offset = 0.0f;
+		}
+
+		// Only calculate the relative position if the WGS-84 location of the origin is set
+		if (collect_gps(gps)) {
+			float lpos_x = 0.0f;
+			float lpos_y = 0.0f;
+			map_projection_project(&_pos_ref, (gps.lat / 1.0e7), (gps.lon / 1.0e7), &lpos_x, &lpos_y);
+			gps_sample_new.pos(0) = lpos_x;
+			gps_sample_new.pos(1) = lpos_y;
+
+		} else {
+			gps_sample_new.pos(0) = 0.0f;
+			gps_sample_new.pos(1) = 0.0f;
+		}
+
+		_gps_buffer.push(gps_sample_new);
+	}
+}
+
+void EstimatorInterface::setBaroData(const baroSample &baro_sample)
+{
+	if (!_initialised || _baro_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_baro_buffer.get_length() < _obs_buffer_length) {
+		_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
+
+		if (_baro_buffer_fail) {
+			printBufferAllocationFailed("baro");
+			return;
+		}
+	}
+
+	// downsample to highest possible sensor rate
+	// by baro data by taking the average of incoming sample
+	_baro_sample_count++;
+	_baro_alt_sum += baro_sample.hgt;
+	_baro_timestamp_sum += baro_sample.time_us / 1000; // Dividing by 1000 to avoid overflow
+
+	// limit data rate to prevent data being lost
+	if ((baro_sample.time_us - _time_last_baro) > _min_obs_interval_us) {
+		_time_last_baro = baro_sample.time_us;
+
+		const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count;
+
+		baroSample baro_sample_new;
+		baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_alt_avg);
+
+		// Use the time in the middle of the downsampling interval for the sample
+		baro_sample_new.time_us = 1000 * (_baro_timestamp_sum / _baro_sample_count);
+		baro_sample_new.time_us -= _params.baro_delay_ms * 1000;
+		baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		_baro_buffer.push(baro_sample_new);
+
+		_baro_sample_count = 0;
+		_baro_alt_sum = 0.0f;
+		_baro_timestamp_sum = 0;
+	}
+}
+
+void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
+{
+	if (!_initialised || _airspeed_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_airspeed_buffer.get_length() < _obs_buffer_length) {
+		_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
+
+		if (_airspeed_buffer_fail) {
+			printBufferAllocationFailed("airspeed");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	if ((airspeed_sample.time_us - _time_last_airspeed) > _min_obs_interval_us) {
+		_time_last_airspeed = airspeed_sample.time_us;
+
+		airspeedSample airspeed_sample_new = airspeed_sample;
+
+		airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000;
+		airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		_airspeed_buffer.push(airspeed_sample_new);
+	}
+}
+
+void EstimatorInterface::setRangeData(const rangeSample &range_sample)
+{
+	if (!_initialised || _range_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_range_buffer.get_length() < _obs_buffer_length) {
+		_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);
+
+		if (_range_buffer_fail) {
+			printBufferAllocationFailed("range");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	if ((range_sample.time_us - _time_last_range) > _min_obs_interval_us) {
+		_time_last_range = range_sample.time_us;
+
+		rangeSample range_sample_new = range_sample;
+		range_sample_new.time_us -= _params.range_delay_ms * 1000;
+		range_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		_range_buffer.push(range_sample_new);
+	}
+}
+
+void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
+{
+	if (!_initialised || _flow_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_flow_buffer.get_length() < _imu_buffer_length) {
+		_flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length);
+
+		if (_flow_buffer_fail) {
+			printBufferAllocationFailed("flow");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	if ((flow.time_us - _time_last_optflow) > _min_obs_interval_us) {
+		_time_last_optflow = flow.time_us;
+
+		flowSample optflow_sample_new = flow;
+
+		optflow_sample_new.time_us -= _params.flow_delay_ms * 1000;
+		optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		_flow_buffer.push(optflow_sample_new);
+	}
+}
+
+// set attitude and position data derived from an external vision system
+void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
+{
+	if (!_initialised || _ev_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_ext_vision_buffer.get_length() < _obs_buffer_length) {
+		_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);
+
+		if (_ev_buffer_fail) {
+			printBufferAllocationFailed("vision");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	if ((evdata.time_us - _time_last_ext_vision) > _min_obs_interval_us) {
+		_time_last_ext_vision = evdata.time_us;
+
+		extVisionSample ev_sample_new = evdata;
+		// calculate the system time-stamp for the mid point of the integration period
+		ev_sample_new.time_us -= _params.ev_delay_ms * 1000;
+		ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		_ext_vision_buffer.push(ev_sample_new);
+	}
+}
+
+void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
+{
+	if (!_initialised || _auxvel_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_auxvel_buffer.get_length() < _obs_buffer_length) {
+		_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
+
+		if (_auxvel_buffer_fail) {
+			printBufferAllocationFailed("aux vel");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	if ((auxvel_sample.time_us - _time_last_auxvel) > _min_obs_interval_us) {
+		_time_last_auxvel = auxvel_sample.time_us;
+
+		auxVelSample auxvel_sample_new = auxvel_sample;
+
+		auxvel_sample_new.time_us -= _params.auxvel_delay_ms * 1000;
+		auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+
+		_auxvel_buffer.push(auxvel_sample_new);
+	}
+}
+
+void EstimatorInterface::setDragData(const imuSample &imu)
+{
+	// down-sample the drag specific force data by accumulating and calculating the mean when
+	// sufficient samples have been collected
+	if ((_params.fusion_mode & MASK_USE_DRAG) && !_drag_buffer_fail) {
+
+		// Allocate the required buffer size if not previously done
+		// Do not retry if allocation has failed previously
+		if (_drag_buffer.get_length() < _obs_buffer_length) {
+			_drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length);
+
+			if (_drag_buffer_fail) {
+				printBufferAllocationFailed("drag");
+				return;
+			}
+		}
+
+		_drag_sample_count ++;
+		// note acceleration is accumulated as a delta velocity
+		_drag_down_sampled.accelXY(0) += imu.delta_vel(0);
+		_drag_down_sampled.accelXY(1) += imu.delta_vel(1);
+		_drag_down_sampled.time_us += imu.time_us;
+		_drag_sample_time_dt += imu.delta_vel_dt;
+
+		// calculate the downsample ratio for drag specific force data
+		uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
+
+		if (min_sample_ratio < 5) {
+			min_sample_ratio = 5;
+		}
+
+		// calculate and store means from accumulated values
+		if (_drag_sample_count >= min_sample_ratio) {
+			// note conversion from accumulated delta velocity to acceleration
+			_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
+			_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
+			_drag_down_sampled.time_us /= _drag_sample_count;
+
+			// write to buffer
+			_drag_buffer.push(_drag_down_sampled);
+
+			// reset accumulators
+			_drag_sample_count = 0;
+			_drag_down_sampled.accelXY.zero();
+			_drag_down_sampled.time_us = 0;
+			_drag_sample_time_dt = 0.0f;
+		}
+	}
+}
+
+bool EstimatorInterface::initialise_interface(uint64_t timestamp)
+{
+	// find the maximum time delay the buffers are required to handle
+	// it's reasonable to assume that barometer is always used, and its delay is low
+	// it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used
+	float max_time_delay_ms = math::max(_params.baro_delay_ms, _params.auxvel_delay_ms);
+
+	// using airspeed
+	if (_params.arsp_thr > FLT_EPSILON) {
+		max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
+	}
+
+	// mag mode
+	if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
+	}
+
+	// range aid or range height
+	if (_params.range_aid || (_params.vdist_sensor_type == VDIST_SENSOR_RANGE)) {
+		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
+	}
+
+	if (_params.fusion_mode & MASK_USE_GPS) {
+		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
+	}
+
+	if (_params.fusion_mode & MASK_USE_OF) {
+		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
+	}
+
+	if (_params.fusion_mode & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
+		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
+	}
+
+	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
+	_imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
+
+	// set the observation buffer length to handle the minimum time of arrival between observations in combination
+	// with the worst case delay from current time to ekf fusion time
+	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
+	const float ekf_delay_ms = max_time_delay_ms * 1.5f;
+	_obs_buffer_length = ceilf(ekf_delay_ms / _params.sensor_interval_min_ms);
+
+	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
+	_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
+
+	if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) {
+		printBufferAllocationFailed("IMU and output");
+		return false;
+	}
+
+	_imu_sample_delayed.time_us = timestamp;
+	_imu_sample_delayed.delta_vel_clipping[0] = false;
+	_imu_sample_delayed.delta_vel_clipping[1] = false;
+	_imu_sample_delayed.delta_vel_clipping[2] = false;
+
+	_fault_status.value = 0;
+
+	return true;
+}
+
+bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
+{
+	return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
+}
+
+bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const
+{
+	const int nb_sources = getNumberOfActiveHorizontalAidingSources();
+	return aiding_flag ? nb_sources > 1 : nb_sources > 0;
+}
+
+int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
+{
+	return int(_control_status.flags.gps)
+	       + int(_control_status.flags.opt_flow)
+	       + int(_control_status.flags.ev_pos)
+	       + int(_control_status.flags.ev_vel)
+	       // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning
+	       // and so is treated as a single aiding source.
+	       + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta);
+}
+
+bool EstimatorInterface::isHorizontalAidingActive() const
+{
+	return getNumberOfActiveHorizontalAidingSources() > 0;
+}
+
+void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
+{
+	if (buffer_name) {
+		ECL_ERR("%s buffer allocation failed", buffer_name);
+	}
+}
+
+void EstimatorInterface::print_status()
+{
+	ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size());
+	ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size());
+	ECL_INFO("mag buffer: %d (%d Bytes)", _mag_buffer.get_length(), _mag_buffer.get_total_size());
+	ECL_INFO("baro buffer: %d (%d Bytes)", _baro_buffer.get_length(), _baro_buffer.get_total_size());
+	ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size());
+	ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size());
+	ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size());
+	ECL_INFO("vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size());
+	ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size());
+	ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
+	ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size());
+}

src/lib/ecl/EKF/estimator_interface.h

@@ -0,0 +1,443 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2020 Estimation and Control Library (ECL). 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 ECL 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 estimator_interface.h
+ * Definition of base class for attitude estimators
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ *
+ */
+
+#pragma once
+
+#include <ecl.h>
+#include "common.h"
+#include "RingBuffer.h"
+#include <AlphaFilter/AlphaFilter.hpp>
+#include "imu_down_sampler.hpp"
+#include "sensor_range_finder.hpp"
+#include "utils.hpp"
+
+#include <geo/geo.h>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+using namespace estimator;
+
+class EstimatorInterface
+{
+public:
+	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
+	virtual bool collect_gps(const gps_message &gps) = 0;
+
+	void setIMUData(const imuSample &imu_sample);
+
+	/*
+	Returns  following IMU vibration metrics in the following array locations
+	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
+	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
+	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
+	*/
+	const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
+
+	void setMagData(const magSample &mag_sample);
+
+	void setGpsData(const gps_message &gps);
+
+	void setBaroData(const baroSample &baro_sample);
+
+	void setAirspeedData(const airspeedSample &airspeed_sample);
+
+	void setRangeData(const rangeSample &range_sample);
+
+	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
+	void setOpticalFlowData(const flowSample &flow);
+
+	// set external vision position and attitude data
+	void setExtVisionData(const extVisionSample &evdata);
+
+	void setAuxVelData(const auxVelSample &auxvel_sample);
+
+	// return a address to the parameters struct
+	// in order to give access to the application
+	parameters *getParamHandle() { return &_params; }
+
+	// set vehicle landed status data
+	void set_in_air_status(bool in_air)
+	{
+		if (!in_air) {
+			_time_last_on_ground_us = _time_last_imu;
+
+		} else {
+			_time_last_in_air = _time_last_imu;
+		}
+		_control_status.flags.in_air = in_air;
+	}
+
+	// return true if the attitude is usable
+	bool attitude_valid() const { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
+
+	// get vehicle landed status data
+	bool get_in_air_status() const { return _control_status.flags.in_air; }
+
+	// get wind estimation status
+	bool get_wind_status() const { return _control_status.flags.wind; }
+
+	// set vehicle is fixed wing status
+	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
+
+	// set flag if synthetic sideslip measurement should be fused
+	void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
+
+	// set flag if static pressure rise due to ground effect is expected
+	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
+	// flag will clear after GNDEFFECT_TIMEOUT uSec
+	void set_gnd_effect_flag(bool gnd_effect)
+	{
+		_control_status.flags.gnd_effect = gnd_effect;
+		_time_last_gnd_effect_on = _time_last_imu;
+	}
+
+	// set air density used by the multi-rotor specific drag force fusion
+	void set_air_density(float air_density) { _air_density = air_density; }
+
+	// set sensor limitations reported by the rangefinder
+	void set_rangefinder_limits(float min_distance, float max_distance)
+	{
+		_range_sensor.setLimits(min_distance, max_distance);
+	}
+
+	// set sensor limitations reported by the optical flow sensor
+	void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
+	{
+		_flow_max_rate = max_flow_rate;
+		_flow_min_distance = min_distance;
+		_flow_max_distance = max_distance;
+	}
+
+	// the flags considered are opt_flow, gps, ev_vel and ev_pos
+	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
+
+	/*
+	 * Check if there are any other active source of horizontal aiding
+	 * Warning: does not tell if the selected source is
+	 * active, use isOnlyActiveSourceOfHorizontalAiding() for this
+	 *
+	 * The flags considered are opt_flow, gps, ev_vel and ev_pos
+	 *
+	 * @param aiding_flag a flag in _control_status.flags
+	 * @return true if an other source than aiding_flag is active
+	 */
+	bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const;
+
+	// Return true if at least one source of horizontal aiding is active
+	// the flags considered are opt_flow, gps, ev_vel and ev_pos
+	bool isHorizontalAidingActive() const;
+
+	int getNumberOfActiveHorizontalAidingSources() const;
+
+	// return true if the EKF is dead reckoning the position using inertial data only
+	bool inertial_dead_reckoning() const { return _is_dead_reckoning; }
+
+	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
+
+	// get the velocity of the body frame origin in local NED earth frame
+	Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
+
+	// get the velocity derivative in earth frame
+	const Vector3f &getVelocityDerivative() const { return _vel_deriv; }
+
+	// get the derivative of the vertical position of the body frame origin in local NED earth frame
+	float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
+
+	// get the position of the body frame origin in local earth frame
+	Vector3f getPosition() const
+	{
+		// rotate the position of the IMU relative to the boy origin into earth frame
+		const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
+		// subtract from the EKF position (which is at the IMU) to get position at the body origin
+		return _output_new.pos - pos_offset_earth;
+	}
+
+	// Get the value of magnetic declination in degrees to be saved for use at the next startup
+	// Returns true when the declination can be saved
+	// At the next startup, set param.mag_declination_deg to the value saved
+	bool get_mag_decl_deg(float *val) const
+	{
+		if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
+			*val = math::degrees(_mag_declination_gps);
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
+	// get EKF mode status
+	const filter_control_status_u &control_status() const { return _control_status; }
+	const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; }
+
+	const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
+	const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
+
+	// get EKF internal fault status
+	const fault_status_u &fault_status() const { return _fault_status; }
+	const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }
+
+	const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; }
+	const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; }
+
+	const warning_event_status_u &warning_event_status() const { return _warning_events; }
+	const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; }
+	void clear_warning_events() { _warning_events.value = 0; }
+
+	const information_event_status_u &information_event_status() const { return _information_events; }
+	const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; }
+	void clear_information_events() { _information_events.value = 0; }
+
+	bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
+
+	// Getter for the average imu update period in s
+	float get_dt_imu_avg() const { return _dt_imu_avg; }
+
+	// Getter for the imu sample on the delayed time horizon
+	const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
+
+	// Getter for the baro sample on the delayed time horizon
+	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
+
+	const bool& global_origin_valid() const { return _NED_origin_initialised; }
+	const map_projection_reference_s& global_origin() const { return _pos_ref; }
+
+	void print_status();
+
+	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
+	static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
+
+protected:
+
+	EstimatorInterface() = default;
+	virtual ~EstimatorInterface() = default;
+
+	virtual bool init(uint64_t timestamp) = 0;
+
+	parameters _params;		// filter parameters
+
+	/*
+	 OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
+	 which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
+	 arrive too soon after the previous measurement will not be processed.
+	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S)
+	 This can be adjusted to match the max sensor data rate plus some margin for jitter.
+	*/
+	uint8_t _obs_buffer_length{0};
+
+	/*
+	IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
+	EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
+	max sensor time offet (msec) =  IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
+	This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
+	*/
+	uint8_t _imu_buffer_length{0};
+
+	float _dt_imu_avg{0.0f};	// average imu update period in s
+
+	imuSample _imu_sample_delayed{};	// captures the imu sample on the delayed time horizon
+
+	// measurement samples capturing measurements on the delayed time horizon
+	magSample _mag_sample_delayed{};
+	baroSample _baro_sample_delayed{};
+	gpsSample _gps_sample_delayed{};
+	sensor::SensorRangeFinder _range_sensor{};
+	airspeedSample _airspeed_sample_delayed{};
+	flowSample _flow_sample_delayed{};
+	extVisionSample _ev_sample_delayed{};
+	dragSample _drag_sample_delayed{};
+	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
+	auxVelSample _auxvel_sample_delayed{};
+
+	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
+
+	// Sensor limitations
+	float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
+	float _flow_min_distance{0.0f};	///< minimum distance that the optical flow sensor can operate at (m)
+	float _flow_max_distance{0.0f};	///< maximum distance that the optical flow sensor can operate at (m)
+
+	// Output Predictor
+	outputSample _output_new{};		// filter output on the non-delayed time horizon
+	outputVert _output_vert_new{};		// vertical filter output on the non-delayed time horizon
+	imuSample _newest_high_rate_imu_sample{};		// imu sample capturing the newest imu data
+	Matrix3f _R_to_earth_now;		// rotation matrix from body to earth frame at current time
+	Vector3f _vel_imu_rel_body_ned;		// velocity of IMU relative to body origin in NED earth frame
+	Vector3f _vel_deriv;		// velocity derivative at the IMU in NED earth frame (m/s/s)
+
+	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
+	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
+
+	bool _NED_origin_initialised{false};
+	bool _gps_speed_valid{false};
+	float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
+	float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
+	struct map_projection_reference_s _pos_ref {};   // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
+	struct map_projection_reference_s _gps_pos_prev {};   // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
+	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
+	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
+
+	// innovation consistency check monitoring ratios
+	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
+	Vector3f _mag_test_ratio;		// magnetometer XYZ innovation consistency check ratios
+	Vector2f _gps_vel_test_ratio;		// GPS velocity innovation consistency check ratios
+	Vector2f _gps_pos_test_ratio;		// GPS position innovation consistency check ratios
+	Vector2f _ev_vel_test_ratio;		// EV velocity innovation consistency check ratios
+	Vector2f _ev_pos_test_ratio ;		// EV position innovation consistency check ratios
+	Vector2f _aux_vel_test_ratio;		// Auxiliary horizontal velocity innovation consistency check ratio
+	Vector2f _baro_hgt_test_ratio;		// baro height innovation consistency check ratios
+	Vector2f _rng_hgt_test_ratio;		// range finder height innovation consistency check ratios
+	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
+	float _tas_test_ratio{};		// tas innovation consistency check ratio
+	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
+	float _beta_test_ratio{};		// sideslip innovation consistency check ratio
+	Vector2f _drag_test_ratio;		// drag innovation consistency check ratio
+	innovation_fault_status_u _innov_check_fail_status{};
+
+	bool _is_dead_reckoning{false};		// true if we are no longer fusing measurements that constrain horizontal velocity drift
+	bool _deadreckon_time_exceeded{true};	// true if the horizontal nav solution has been deadreckoning for too long and is invalid
+	bool _is_wind_dead_reckoning{false};	// true if we are navigationg reliant on wind relative measurements
+
+	float _gps_drift_metrics[3] {};	// Array containing GPS drift metrics
+					// [0] Horizontal position drift rate (m/s)
+					// [1] Vertical position drift rate (m/s)
+					// [2] Filtered horizontal velocity (m/s)
+	uint64_t _time_last_move_detect_us{0};	// timestamp of last movement detection event in microseconds
+	uint64_t _time_last_on_ground_us{0};	///< last time we were on the ground (uSec)
+	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
+	bool _gps_drift_updated{false};	// true when _gps_drift_metrics has been updated and is ready for retrieval
+
+	// data buffer instances
+	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters
+	RingBuffer<outputSample> _output_buffer{12};
+	RingBuffer<outputVert> _output_vert_buffer{12};
+
+	RingBuffer<gpsSample> _gps_buffer;
+	RingBuffer<magSample> _mag_buffer;
+	RingBuffer<baroSample> _baro_buffer;
+	RingBuffer<rangeSample> _range_buffer;
+	RingBuffer<airspeedSample> _airspeed_buffer;
+	RingBuffer<flowSample> 	_flow_buffer;
+	RingBuffer<extVisionSample> _ext_vision_buffer;
+	RingBuffer<dragSample> _drag_buffer;
+	RingBuffer<auxVelSample> _auxvel_buffer;
+
+	// timestamps of latest in buffer saved measurement in microseconds
+	uint64_t _time_last_imu{0};
+	uint64_t _time_last_gps{0};
+	uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
+	uint64_t _time_last_baro{0};
+	uint64_t _time_last_range{0};
+	uint64_t _time_last_airspeed{0};
+	uint64_t _time_last_ext_vision{0};
+	uint64_t _time_last_optflow{0};
+	uint64_t _time_last_auxvel{0};
+	//last time the baro ground effect compensation was turned on externally (uSec)
+	uint64_t _time_last_gnd_effect_on{0};
+
+	fault_status_u _fault_status{};
+
+	// allocate data buffers and initialize interface variables
+	bool initialise_interface(uint64_t timestamp);
+
+	float _mag_declination_gps{NAN};         // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
+
+	// this is the current status of the filter control modes
+	filter_control_status_u _control_status{};
+
+	// this is the previous status of the filter control modes - used to detect mode transitions
+	filter_control_status_u _control_status_prev{};
+
+	virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0;
+
+	// these are used to record single frame events for external monitoring and should NOT be used for
+	// state logic becasue they will be cleared externally after being read.
+	warning_event_status_u _warning_events{};
+	information_event_status_u _information_events{};
+
+private:
+
+	inline void setDragData(const imuSample &imu);
+
+	inline void computeVibrationMetric(const imuSample &imu);
+	inline bool checkIfVehicleAtRest(float dt, const imuSample &imu);
+
+	void printBufferAllocationFailed(const char *buffer_name);
+
+	ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S};
+
+	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
+
+	// IMU vibration and movement monitoring
+	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
+	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
+	Vector3f _vibe_metrics;	// IMU vibration metrics
+					// [0] Level of coning vibration in the IMU delta angles (rad^2)
+					// [1] high frequency vibration level in the IMU delta angle data (rad)
+					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
+
+	// Used to down sample barometer data
+	uint64_t _baro_timestamp_sum{0};	// summed timestamp to provide the timestamp of the averaged sample
+	float _baro_alt_sum{0.0f};			// summed pressure altitude readings (m)
+	uint8_t _baro_sample_count{0};		// number of barometric altitude measurements summed
+
+	// Used by the multi-rotor specific drag force fusion
+	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
+	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
+
+	// Used to downsample magnetometer data
+	uint64_t _mag_timestamp_sum{0};
+	Vector3f _mag_data_sum;
+	uint8_t _mag_sample_count{0};
+
+	// observation buffer final allocation failed
+	bool _gps_buffer_fail{false};
+	bool _mag_buffer_fail{false};
+	bool _baro_buffer_fail{false};
+	bool _range_buffer_fail{false};
+	bool _airspeed_buffer_fail{false};
+	bool _flow_buffer_fail{false};
+	bool _ev_buffer_fail{false};
+	bool _drag_buffer_fail{false};
+	bool _auxvel_buffer_fail{false};
+
+};

src/lib/ecl/EKF/gps_checks.cpp

@@ -0,0 +1,280 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 gps_checks.cpp
+ * Perform pre-flight and in-flight GPS quality checks
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+
+#include "ekf.h"
+
+#include <ecl.h>
+#include <geo_lookup/geo_mag_declination.h>
+#include <mathlib/mathlib.h>
+
+// GPS pre-flight check bit locations
+#define MASK_GPS_NSATS  (1<<0)
+#define MASK_GPS_PDOP   (1<<1)
+#define MASK_GPS_HACC   (1<<2)
+#define MASK_GPS_VACC   (1<<3)
+#define MASK_GPS_SACC   (1<<4)
+#define MASK_GPS_HDRIFT (1<<5)
+#define MASK_GPS_VDRIFT (1<<6)
+#define MASK_GPS_HSPD   (1<<7)
+#define MASK_GPS_VSPD   (1<<8)
+
+bool Ekf::collect_gps(const gps_message &gps)
+{
+	// Run GPS checks always
+	_gps_checks_passed = gps_is_good(gps);
+
+	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
+		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
+		const double lat = gps.lat * 1.0e-7;
+		const double lon = gps.lon * 1.0e-7;
+
+		if (!map_projection_initialized(&_pos_ref)) {
+			map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
+
+			// if we are already doing aiding, correct for the change in position since the EKF started navigating
+			if (isHorizontalAidingActive()) {
+				double est_lat;
+				double est_lon;
+				map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
+				map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
+			}
+		}
+
+		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
+		_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
+		_NED_origin_initialised = true;
+		_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
+		_last_gps_origin_time_us = _time_last_imu;
+
+		const bool declination_was_valid = ISFINITE(_mag_declination_gps);
+
+		// set the magnetic field data returned by the geo library using the current GPS position
+		_mag_declination_gps = get_mag_declination_radians(lat, lon);
+		_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+		_mag_strength_gps = get_mag_strength_gauss(lat, lon);
+
+		// request a reset of the yaw using the new declination
+		if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
+			// try to reset the yaw using the EKF-GSF yaw estimator
+			_do_ekfgsf_yaw_reset = true;
+			_ekfgsf_yaw_reset_time = 0;
+
+		} else {
+			if (!declination_was_valid) {
+				_mag_yaw_reset_req = true;
+			}
+		}
+
+		// save the horizontal and vertical position uncertainty of the origin
+		_gps_origin_eph = gps.eph;
+		_gps_origin_epv = gps.epv;
+
+		// if the user has selected GPS as the primary height source, switch across to using it
+		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
+			startGpsHgtFusion();
+		}
+
+		_information_events.flags.gps_checks_passed = true;
+		ECL_INFO("GPS checks passed");
+
+	} else if (!_NED_origin_initialised) {
+		// a rough 2D fix is still sufficient to lookup declination
+		if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
+
+			const bool declination_was_valid = ISFINITE(_mag_declination_gps);
+
+			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
+			const double lat = gps.lat * 1.0e-7;
+			const double lon = gps.lon * 1.0e-7;
+
+			// set the magnetic field data returned by the geo library using the current GPS position
+			_mag_declination_gps = get_mag_declination_radians(lat, lon);
+			_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+			_mag_strength_gps = get_mag_strength_gauss(lat, lon);
+
+			// request mag yaw reset if there's a mag declination for the first time
+			if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+				if (!declination_was_valid && ISFINITE(_mag_declination_gps)) {
+					_mag_yaw_reset_req = true;
+				}
+			}
+
+			_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
+		}
+	}
+
+	// start collecting GPS if there is a 3D fix and the NED origin has been set
+	return _NED_origin_initialised && (gps.fix_type >= 3);
+}
+
+/*
+ * Return true if the GPS solution quality is adequate to set an origin for the EKF
+ * and start GPS aiding.
+ * All activated checks must pass for 10 seconds.
+ * Checks are activated using the EKF2_GPS_CHECK bitmask parameter
+ * Checks are adjusted using the EKF2_REQ_* parameters
+*/
+bool Ekf::gps_is_good(const gps_message &gps)
+{
+	// Check the fix type
+	_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
+
+	// Check the number of satellites
+	_gps_check_fail_status.flags.nsats = (gps.nsats < _params.req_nsats);
+
+	// Check the position dilution of precision
+	_gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop);
+
+	// Check the reported horizontal and vertical position accuracy
+	_gps_check_fail_status.flags.hacc = (gps.eph > _params.req_hacc);
+	_gps_check_fail_status.flags.vacc = (gps.epv > _params.req_vacc);
+
+	// Check the reported speed accuracy
+	_gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc);
+
+	// check if GPS quality is degraded
+	_gps_error_norm = fmaxf((gps.eph / _params.req_hacc), (gps.epv / _params.req_vacc));
+	_gps_error_norm = fmaxf(_gps_error_norm, (gps.sacc / _params.req_sacc));
+
+	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
+	constexpr float filt_time_const = 10.0f;
+	const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f, filt_time_const);
+	const float filter_coef = dt / filt_time_const;
+
+	// The following checks are only valid when the vehicle is at rest
+	const double lat = gps.lat * 1.0e-7;
+	const double lon = gps.lon * 1.0e-7;
+
+	if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
+		// Calculate position movement since last measurement
+		float delta_pos_n = 0.0f;
+		float delta_pos_e = 0.0f;
+
+		// calculate position movement since last GPS fix
+		if (_gps_pos_prev.timestamp > 0) {
+			map_projection_project(&_gps_pos_prev, lat, lon, &delta_pos_n, &delta_pos_e);
+
+		} else {
+			// no previous position has been set
+			map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
+			_gps_alt_prev = 1e-3f * (float)gps.alt;
+		}
+
+		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
+		const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
+		Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - 1e-3f * (float)gps.alt));
+		pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
+
+		// Apply a low pass filter
+		_gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
+
+		// Calculate the horizontal drift speed and fail if too high
+		_gps_drift_metrics[0] = Vector2f(_gps_pos_deriv_filt.xy()).norm();
+		_gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift);
+
+		// Fail if the vertical drift speed is too high
+		_gps_drift_metrics[1] = fabsf(_gps_pos_deriv_filt(2));
+		_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
+
+		// Check the magnitude of the filtered horizontal GPS velocity
+		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
+							     -10.0f * _params.req_hdrift,
+							      10.0f * _params.req_hdrift);
+		_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
+		_gps_drift_metrics[2] = _gps_velNE_filt.norm();
+		_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);
+
+		_gps_drift_updated = true;
+
+	} else if (_control_status.flags.in_air) {
+		// These checks are always declared as passed when flying
+		// If on ground and moving, the last result before movement commenced is kept
+		_gps_check_fail_status.flags.hdrift = false;
+		_gps_check_fail_status.flags.vdrift = false;
+		_gps_check_fail_status.flags.hspeed = false;
+		_gps_drift_updated = false;
+
+		resetGpsDriftCheckFilters();
+
+	} else {
+		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
+		_gps_drift_updated = true;
+
+		resetGpsDriftCheckFilters();
+	}
+
+	// save GPS fix for next time
+	map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
+	_gps_alt_prev = 1e-3f * (float)gps.alt;
+
+	// Check  the filtered difference between GPS and EKF vertical velocity
+	const float vz_diff_limit = 10.0f * _params.req_vdrift;
+	const float vertVel = math::constrain(gps.vel_ned(2) - _state.vel(2), -vz_diff_limit, vz_diff_limit);
+	_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
+	_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
+
+	// assume failed first time through
+	if (_last_gps_fail_us == 0) {
+		_last_gps_fail_us = _time_last_imu;
+	}
+
+	// if any user selected checks have failed, record the fail time
+	if (
+		_gps_check_fail_status.flags.fix ||
+		(_gps_check_fail_status.flags.nsats   && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
+		(_gps_check_fail_status.flags.pdop    && (_params.gps_check_mask & MASK_GPS_PDOP)) ||
+		(_gps_check_fail_status.flags.hacc    && (_params.gps_check_mask & MASK_GPS_HACC)) ||
+		(_gps_check_fail_status.flags.vacc    && (_params.gps_check_mask & MASK_GPS_VACC)) ||
+		(_gps_check_fail_status.flags.sacc    && (_params.gps_check_mask & MASK_GPS_SACC)) ||
+		(_gps_check_fail_status.flags.hdrift  && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
+		(_gps_check_fail_status.flags.vdrift  && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
+		(_gps_check_fail_status.flags.hspeed  && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
+		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
+	) {
+		_last_gps_fail_us = _time_last_imu;
+
+	} else {
+		_last_gps_pass_us = _time_last_imu;
+	}
+
+	// continuous period without fail of x seconds required to return a healthy status
+	return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us);
+}

src/lib/ecl/EKF/gps_yaw_fusion.cpp

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 Estimation and Control Library (ECL). 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 ECL 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 gps_yaw_fusion.cpp
+ * Definition of functions required to use yaw obtained from GPS dual antenna measurements.
+ * Equations generated using EKF/python/ekf_derivation/main.py
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+
+#include "ekf.h"
+
+#include <ecl.h>
+#include <mathlib/mathlib.h>
+#include <cstdlib>
+
+void Ekf::fuseGpsYaw()
+{
+	// assign intermediate state variables
+	const float &q0 = _state.quat_nominal(0);
+	const float &q1 = _state.quat_nominal(1);
+	const float &q2 = _state.quat_nominal(2);
+	const float &q3 = _state.quat_nominal(3);
+
+	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
+	const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
+
+	// define the predicted antenna array vector and rotate into earth frame
+	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+
+	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+		return;
+	}
+
+	// calculate predicted antenna yaw angle
+	const float predicted_hdg =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
+
+	// using magnetic heading process noise
+	// TODO extend interface to use yaw uncertainty provided by GPS if available
+	const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+
+	// calculate intermediate variables
+	const float HK0 = sinf(_gps_yaw_offset);
+	const float HK1 = q0*q3;
+	const float HK2 = q1*q2;
+	const float HK3 = 2*HK0*(HK1 - HK2);
+	const float HK4 = cosf(_gps_yaw_offset);
+	const float HK5 = ecl::powf(q1, 2);
+	const float HK6 = ecl::powf(q2, 2);
+	const float HK7 = ecl::powf(q0, 2) - ecl::powf(q3, 2);
+	const float HK8 = HK4*(HK5 - HK6 + HK7);
+	const float HK9 = HK3 - HK8;
+	if (fabsf(HK9) < 1e-3f) {
+		return;
+	}
+	const float HK10 = 1.0F/HK9;
+	const float HK11 = HK4*q0;
+	const float HK12 = HK0*q3;
+	const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2);
+	const float HK14 = HK10*HK13;
+	const float HK15 = HK0*q0 + HK4*q3;
+	const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
+	const float HK17 = ecl::powf(HK13, 2)/ecl::powf(HK9, 2) + 1;
+	if (fabsf(HK17) < 1e-3f) {
+		return;
+	}
+	const float HK18 = 2/HK17;
+	// const float HK19 = 1.0F/(-HK3 + HK8);
+	const float HK19_inverse = -HK3 + HK8;
+	if (fabsf(HK19_inverse) < 1e-6f) {
+		return;
+	}
+	const float HK19 = 1.0F/HK19_inverse;
+	const float HK20 = HK4*q1;
+	const float HK21 = HK0*q2;
+	const float HK22 = HK13*HK19;
+	const float HK23 = HK0*q1 - HK4*q2;
+	const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23);
+	const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23);
+	const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
+	const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
+	const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
+	const float HK29 = 4/ecl::powf(HK17, 2);
+	const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
+	const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
+	// const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+
+	// check if the innovation variance calculation is badly conditioned
+	_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+
+	if (_heading_innov_var < R_YAW) {
+	// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		_fault_status.flags.bad_hdg = true;
+
+		// we reinitialise the covariance matrix and abort this fusion step
+		initialiseCovariance();
+		ECL_ERR("GPS yaw numerical error - covariance reset");
+		return;
+	}
+
+	_fault_status.flags.bad_hdg = false;
+	const float HK32 = HK18/_heading_innov_var;
+
+	// calculate the innovation and define the innovation gate
+	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
+	_heading_innov = predicted_hdg - measured_hdg;
+
+	// wrap the innovation to the interval between +-pi
+	_heading_innov = wrap_pi(_heading_innov);
+
+	// innovation test ratio
+	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
+
+	// we are no longer using 3-axis fusion so set the reported test levels to zero
+	_mag_test_ratio.setZero();
+
+	if (_yaw_test_ratio > 1.0f) {
+		_innov_check_fail_status.flags.reject_yaw = true;
+
+		// if we are in air we don't want to fuse the measurement
+		// we allow to use it when on the ground because the large innovation could be caused
+		// by interference or a large initial gyro bias
+		if (_control_status.flags.in_air) {
+			return;
+
+		} else {
+			// constrain the innovation to the maximum set by the gate
+			const float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
+			_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
+		}
+
+	} else {
+		_innov_check_fail_status.flags.reject_yaw = false;
+	}
+
+	// calculate observation jacobian
+	// Observation jacobian and Kalman gain vectors
+	SparseVector24f<0,1,2,3> Hfusion;
+	Hfusion.at<0>() = -HK16*HK18;
+	Hfusion.at<1>() = -HK18*HK24;
+	Hfusion.at<2>() = -HK18*HK25;
+	Hfusion.at<3>() = HK18*HK26;
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	Vector24f Kfusion;
+	Kfusion(0) = HK27*HK32;
+	Kfusion(1) = HK28*HK32;
+	Kfusion(2) = HK30*HK32;
+	Kfusion(3) = HK31*HK32;
+	for (unsigned row = 4; row <= 23; row++) {
+		Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
+	}
+
+	const bool is_fused = measurementUpdate(Kfusion, Hfusion, _heading_innov);
+	_fault_status.flags.bad_hdg = !is_fused;
+
+	if (is_fused) {
+		_time_last_gps_yaw_fuse = _time_last_imu;
+	}
+}
+
+bool Ekf::resetYawToGps()
+{
+	// define the predicted antenna array vector and rotate into earth frame
+	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+
+	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+		return false;
+	}
+
+	// GPS yaw measurement is alreday compensated for antenna offset in the driver
+	const float measured_yaw = _gps_sample_delayed.yaw;
+
+	const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+
+	_time_last_gps_yaw_fuse = _time_last_imu;
+
+	return true;
+}

src/lib/ecl/EKF/imu_down_sampler.cpp

@@ -0,0 +1,65 @@
+#include "imu_down_sampler.hpp"
+
+ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec} { reset(); }
+
+// integrate imu samples until target dt reached
+// assumes that dt of the gyroscope is close to the dt of the accelerometer
+// returns true if target dt is reached
+bool ImuDownSampler::update(const imuSample &imu_sample_new)
+{
+	if (_do_reset) {
+		reset();
+	}
+
+	// accumulate time deltas
+	_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
+	_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
+	_imu_down_sampled.time_us = imu_sample_new.time_us;
+	_imu_down_sampled.delta_vel_clipping[0] += imu_sample_new.delta_vel_clipping[0];
+	_imu_down_sampled.delta_vel_clipping[1] += imu_sample_new.delta_vel_clipping[1];
+	_imu_down_sampled.delta_vel_clipping[2] += imu_sample_new.delta_vel_clipping[2];
+
+	// use a quaternion to accumulate delta angle data
+	// this quaternion represents the rotation from the start to end of the accumulation period
+	const Quatf delta_q(AxisAnglef(imu_sample_new.delta_ang));
+	_delta_angle_accumulated = _delta_angle_accumulated * delta_q;
+	_delta_angle_accumulated.normalize();
+
+	// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
+	const Dcmf delta_R(delta_q.inversed());
+	_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
+
+	// accumulate the most recent delta velocity data at the updated rotation frame
+	// assume effective sample time is halfway between the previous and current rotation frame
+	_imu_down_sampled.delta_vel += (imu_sample_new.delta_vel + delta_R * imu_sample_new.delta_vel) * 0.5f;
+
+	// check if the target time delta between filter prediction steps has been exceeded
+	if (_imu_down_sampled.delta_ang_dt >= _target_dt - _imu_collection_time_adj) {
+		// accumulate the amount of time to advance the IMU collection time so that we meet the
+		// average EKF update rate requirement
+		_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - _target_dt);
+		_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt,
+							   0.5f * _target_dt);
+
+		_imu_down_sampled.delta_ang = AxisAnglef(_delta_angle_accumulated);
+
+		return true;
+
+	} else {
+
+		return false;
+	}
+}
+
+void ImuDownSampler::reset()
+{
+	_imu_down_sampled.delta_ang.setZero();
+	_imu_down_sampled.delta_vel.setZero();
+	_imu_down_sampled.delta_ang_dt = 0.0f;
+	_imu_down_sampled.delta_vel_dt = 0.0f;
+	_imu_down_sampled.delta_vel_clipping[0] = false;
+	_imu_down_sampled.delta_vel_clipping[1] = false;
+	_imu_down_sampled.delta_vel_clipping[2] = false;
+	_delta_angle_accumulated.setIdentity();
+	_do_reset = false;
+}

src/lib/ecl/EKF/imu_down_sampler.hpp

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 Estimation and Control Library (ECL). 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 ECL 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Downsamples IMU data to a lower rate such that EKF predicition can happen less frequent
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+#include "common.h"
+
+using namespace estimator;
+
+class ImuDownSampler
+{
+public:
+	explicit ImuDownSampler(float target_dt_sec);
+	~ImuDownSampler() = default;
+
+	bool update(const imuSample &imu_sample_new);
+
+	imuSample getDownSampledImuAndTriggerReset()
+	{
+		_do_reset = true;
+		return _imu_down_sampled;
+	}
+
+private:
+	void reset();
+
+	imuSample _imu_down_sampled{};
+	Quatf _delta_angle_accumulated{};
+	const float _target_dt;  // [sec]
+	float _imu_collection_time_adj{0.f};
+	bool _do_reset{true};
+};

src/lib/ecl/EKF/mag_control.cpp

@@ -0,0 +1,368 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 Estimation and Control Library (ECL). 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 ECL 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 mag_control.cpp
+ * Control functions for ekf magnetic field fusion
+ */
+
+#include "ekf.h"
+#include <mathlib/mathlib.h>
+
+void Ekf::controlMagFusion()
+{
+	// handle undefined behaviour
+	if (_params.mag_fusion_type > MAG_FUSE_TYPE_NONE) {
+		return;
+	}
+
+	// When operating without a magnetometer and no other source of yaw aiding is active,
+	// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
+	// ground and to prevent uncontrolled yaw variance growth
+	if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
+		if (noOtherYawAidingThanMag())
+		{
+			_is_yaw_fusion_inhibited = true;
+			fuseHeading();
+		}
+		return;
+	}
+
+	checkMagFieldStrength();
+
+	// If we are on ground, reset the flight alignment flag so that the mag fields will be
+	// re-initialised next time we achieve flight altitude
+	if (!_control_status.flags.in_air) {
+		_control_status.flags.mag_aligned_in_flight = false;
+		_num_bad_flight_yaw_events = 0;
+	}
+
+	if (_control_status.flags.mag_fault || !_control_status.flags.tilt_align) {
+		stopMagFusion();
+		return;
+	}
+
+	_mag_yaw_reset_req |= otherHeadingSourcesHaveStopped();
+	_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
+	_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
+
+	if (noOtherYawAidingThanMag() && _mag_data_ready) {
+		// Determine if we should use simple magnetic heading fusion which works better when
+		// there are large external disturbances or the more accurate 3-axis fusion
+		switch (_params.mag_fusion_type) {
+		default:
+		/* fallthrough */
+		case MAG_FUSE_TYPE_AUTO:
+			selectMagAuto();
+			break;
+
+		case MAG_FUSE_TYPE_INDOOR:
+		/* fallthrough */
+		case MAG_FUSE_TYPE_HEADING:
+			startMagHdgFusion();
+			break;
+
+		case MAG_FUSE_TYPE_3D:
+			startMag3DFusion();
+			break;
+		}
+
+		if (_control_status.flags.in_air) {
+			checkHaglYawResetReq();
+			runInAirYawReset();
+			runVelPosReset(); // TODO: review this; a vel/pos reset can be requested from COG reset (for fixedwing) only
+
+		} else {
+			runOnGroundYawReset();
+		}
+
+		if (!_control_status.flags.yaw_align) {
+			// Having the yaw aligned is mandatory to continue
+			return;
+		}
+
+		checkMagDeclRequired();
+		checkMagInhibition();
+
+		runMagAndMagDeclFusions();
+	}
+}
+
+bool Ekf::noOtherYawAidingThanMag() const
+{
+	// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
+	return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw;
+}
+
+void Ekf::checkHaglYawResetReq()
+{
+	// We need to reset the yaw angle after climbing away from the ground to enable
+	// recovery from ground level magnetic interference.
+	if (!_control_status.flags.mag_aligned_in_flight) {
+		// Check if height has increased sufficiently to be away from ground magnetic anomalies
+		// and request a yaw reset if not already requested.
+		static constexpr float mag_anomalies_max_hagl = 1.5f;
+		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
+		_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
+	}
+}
+
+void Ekf::runOnGroundYawReset()
+{
+	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
+		const bool has_realigned_yaw = canResetMagHeading()
+					       ? resetMagHeading(_mag_lpf.getState())
+					       : false;
+
+		if (has_realigned_yaw) {
+			_mag_yaw_reset_req = false;
+			_control_status.flags.yaw_align = true;
+
+			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
+			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
+			if (_mag_inhibit_yaw_reset_req) {
+				_mag_inhibit_yaw_reset_req = false;
+				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
+				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+			}
+		}
+	}
+}
+
+bool Ekf::canResetMagHeading() const
+{
+	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
+}
+
+void Ekf::runInAirYawReset()
+{
+	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
+		bool has_realigned_yaw = false;
+
+		if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
+		else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
+
+		if (has_realigned_yaw) {
+			_mag_yaw_reset_req = false;
+			_control_status.flags.yaw_align = true;
+			_control_status.flags.mag_aligned_in_flight = true;
+
+			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
+			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
+			if (_mag_inhibit_yaw_reset_req) {
+				_mag_inhibit_yaw_reset_req = false;
+				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
+				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+			}
+		}
+
+	}
+}
+
+void Ekf::runVelPosReset()
+{
+	if (_velpos_reset_request) {
+		resetVelocity();
+		resetHorizontalPosition();
+		_velpos_reset_request = false;
+	}
+}
+
+void Ekf::selectMagAuto()
+{
+	check3DMagFusionSuitability();
+	canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
+}
+
+void Ekf::check3DMagFusionSuitability()
+{
+	checkYawAngleObservability();
+	checkMagBiasObservability();
+
+	if (isMagBiasObservable() || isYawAngleObservable()) {
+		_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
+	}
+}
+
+void Ekf::checkYawAngleObservability()
+{
+	// Check if there has been enough change in horizontal velocity to make yaw observable
+	// Apply hysteresis to check to avoid rapid toggling
+	_yaw_angle_observable = _yaw_angle_observable
+				? _accel_lpf_NE.norm() > _params.mag_acc_gate
+				: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
+
+	_yaw_angle_observable = _yaw_angle_observable
+				&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
+}
+
+void Ekf::checkMagBiasObservability()
+{
+	// check if there is enough yaw rotation to make the mag bias states observable
+	if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
+		// initial yaw motion is detected
+		_mag_bias_observable = true;
+
+	} else if (_mag_bias_observable) {
+		// require sustained yaw motion of 50% the initial yaw rate threshold
+		const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
+		const float min_yaw_change_req =  0.5f * _params.mag_yaw_rate_gate * yaw_dt;
+		_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
+	}
+
+	_yaw_delta_ef = 0.0f;
+	_time_yaw_started = _imu_sample_delayed.time_us;
+}
+
+bool Ekf::canUse3DMagFusion() const
+{
+	// Use of 3D fusion requires an in-air heading alignment but it should not
+	// be used when the heading and mag biases are not observable for more than 2 seconds
+	return _control_status.flags.mag_aligned_in_flight
+	       && ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
+}
+
+void Ekf::checkMagDeclRequired()
+{
+	// if we are using 3-axis magnetometer fusion, but without external NE aiding,
+	// then the declination must be fused as an observation to prevent long term heading drift
+	// fusing declination when gps aiding is available is optional, but recommended to prevent
+	// problem if the vehicle is static for extended periods of time
+	const bool user_selected = (_params.mag_declination_source & MASK_FUSE_DECL);
+	const bool not_using_ne_aiding = !_control_status.flags.gps;
+	_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
+}
+
+void Ekf::checkMagInhibition()
+{
+	_is_yaw_fusion_inhibited = shouldInhibitMag();
+	if (!_is_yaw_fusion_inhibited) {
+		_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
+	}
+
+	// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
+	if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
+		_mag_inhibit_yaw_reset_req = true;
+	}
+}
+
+bool Ekf::shouldInhibitMag() const
+{
+	// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
+	// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
+	// is available, assume that we are operating indoors and the magnetometer should not be used.
+	// Also inhibit mag fusion when a strong magnetic field interference is detected or the user
+	// has explicitly stopped magnetometer use.
+	const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
+
+	const bool heading_not_required_for_navigation = !_control_status.flags.gps
+							 && !_control_status.flags.ev_pos
+							 && !_control_status.flags.ev_vel;
+
+	return (user_selected && heading_not_required_for_navigation)
+	       || isStrongMagneticDisturbance();
+}
+
+void Ekf::checkMagFieldStrength()
+{
+	if (_params.check_mag_strength) {
+		_control_status.flags.mag_field_disturbed = _NED_origin_initialised
+							    ? !isMeasuredMatchingGpsMagStrength()
+							    : !isMeasuredMatchingAverageMagStrength();
+
+	} else {
+		_control_status.flags.mag_field_disturbed = false;
+	}
+}
+
+bool Ekf::isMeasuredMatchingGpsMagStrength() const
+{
+	constexpr float wmm_gate_size = 0.2f; // +/- Gauss
+	return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
+}
+
+bool Ekf::isMeasuredMatchingAverageMagStrength() const
+{
+	constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
+	constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
+	return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(),
+					  average_earth_mag_field_strength,
+					  average_earth_mag_gate_size);
+}
+
+bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
+{
+	return (measured >= expected - gate)
+		&& (measured <= expected + gate);
+}
+
+void Ekf::runMagAndMagDeclFusions()
+{
+	if (_control_status.flags.mag_3D) {
+		run3DMagAndDeclFusions();
+	} else if (_control_status.flags.mag_hdg) {
+		fuseHeading();
+	}
+}
+
+void Ekf::run3DMagAndDeclFusions()
+{
+	if (!_mag_decl_cov_reset) {
+		// After any magnetic field covariance reset event the earth field state
+		// covariances need to be corrected to incorporate knowledge of the declination
+		// before fusing magnetomer data to prevent rapid rotation of the earth field
+		// states for the first few observations.
+		fuseDeclination(0.02f);
+		_mag_decl_cov_reset = true;
+		fuseMag();
+
+	} else {
+		// The normal sequence is to fuse the magnetometer data first before fusing
+		// declination angle at a higher uncertainty to allow some learning of
+		// declination angle over time.
+		fuseMag();
+		if (_control_status.flags.mag_dec) {
+			fuseDeclination(0.5f);
+		}
+	}
+}
+
+bool Ekf::otherHeadingSourcesHaveStopped()
+{
+    // detect rising edge of noOtherYawAidingThanMag()
+    bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
+
+    _non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
+
+    return  result;
+}

src/lib/ecl/EKF/mag_fusion.cpp

@@ -0,0 +1,962 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 heading_fusion.cpp
+ * Magnetometer fusion methods.
+ * Equations generated using EKF/python/ekf_derivation/main.py
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ *
+ */
+
+#include "ekf.h"
+#include <ecl.h>
+#include <mathlib/mathlib.h>
+
+void Ekf::fuseMag()
+{
+	// assign intermediate variables
+	const float &q0 = _state.quat_nominal(0);
+	const float &q1 = _state.quat_nominal(1);
+	const float &q2 = _state.quat_nominal(2);
+	const float &q3 = _state.quat_nominal(3);
+
+	const float &magN = _state.mag_I(0);
+	const float &magE = _state.mag_I(1);
+	const float &magD = _state.mag_I(2);
+
+	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
+	const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
+
+	// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
+	const char* numerical_error_covariance_reset_string = "numerical error - covariance reset";
+	const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
+	const float HKX1 = magD*q3 + magE*q2 + magN*q1;
+	const float HKX2 = magE*q1;
+	const float HKX3 = magD*q0;
+	const float HKX4 = magN*q2;
+	const float HKX5 = magD*q1 + magE*q0 - magN*q3;
+	const float HKX6 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2);
+	const float HKX7 = q0*q3 + q1*q2;
+	const float HKX8 = q1*q3;
+	const float HKX9 = q0*q2;
+	const float HKX10 = 2*HKX7;
+	const float HKX11 = -2*HKX8 + 2*HKX9;
+	const float HKX12 = 2*HKX1;
+	const float HKX13 = 2*HKX0;
+	const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4;
+	const float HKX15 = 2*HKX5;
+	const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19);
+	const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19);
+	const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19);
+	const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19);
+	const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19);
+	const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19);
+	const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
+	const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);
+
+	_mag_innov_var(0) = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG;
+
+	if (_mag_innov_var(0) < R_MAG) {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		_fault_status.flags.bad_mag_x = true;
+
+		// we need to re-initialise covariances and abort this fusion step
+		resetMagRelatedCovariances();
+		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
+		return;
+	}
+
+	_fault_status.flags.bad_mag_x = false;
+
+	const float HKX24 = 1.0F/_mag_innov_var(0);
+
+	// intermediate variables for calculation of innovations variances for Y and Z axes
+	// don't calculate all terms needed for observation jacobians and Kalman gains because
+	// these will have to be recalculated when the X and Y axes are fused
+	const float IV0 = q0*q1;
+	const float IV1 = q2*q3;
+	const float IV2 = 2*IV0 + 2*IV1;
+	const float IV3 = 2*q0*q3 - 2*q1*q2;
+	const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+	const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+	const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+	const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0;
+	const float IV8 = ecl::powf(q2, 2);
+	const float IV9 = ecl::powf(q3, 2);
+	const float IV10 = ecl::powf(q0, 2) - ecl::powf(q1, 2);
+	const float IV11 = IV10 + IV8 - IV9;
+	const float IV12 = IV7*P(2,3);
+	const float IV13 = IV5*P(0,1);
+	const float IV14 = IV6*P(0,1);
+	const float IV15 = IV4*P(2,3);
+	const float IV16 = 2*q0*q2 + 2*q1*q3;
+	const float IV17 = 2*IV0 - 2*IV1;
+	const float IV18 = IV10 - IV8 + IV9;
+
+	_mag_innov_var(1) = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
+	_mag_innov_var(2) = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
+
+	// chedk innovation variances for being badly conditioned
+
+	if (_mag_innov_var(1) < R_MAG) {
+		// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
+		_fault_status.flags.bad_mag_y = true;
+
+		// we need to re-initialise covariances and abort this fusion step
+		resetMagRelatedCovariances();
+		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
+		return;
+	}
+
+	_fault_status.flags.bad_mag_y = false;
+
+	if (_mag_innov_var(2) < R_MAG) {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		_fault_status.flags.bad_mag_z = true;
+
+		// we need to re-initialise covariances and abort this fusion step
+		resetMagRelatedCovariances();
+		ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
+		return;
+	}
+
+	_fault_status.flags.bad_mag_z = false;
+
+	// rotate magnetometer earth field state into body frame
+	const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+
+	const Vector3f mag_I_rot = R_to_body * _state.mag_I;
+
+	// compute magnetometer innovations
+	_mag_innov = mag_I_rot + _state.mag_B - _mag_sample_delayed.mag;
+
+	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
+	if (_control_status.flags.synthetic_mag_z) {
+		_mag_innov(2) = 0.0f;
+	}
+
+	// Perform an innovation consistency check and report the result
+	bool all_innovation_checks_passed = true;
+
+	for (uint8_t index = 0; index <= 2; index++) {
+		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
+
+		if (_mag_test_ratio(index) > 1.0f) {
+			all_innovation_checks_passed = false;
+			_innov_check_fail_status.value |= (1 << (index + 3));
+
+		} else {
+			_innov_check_fail_status.value &= ~(1 << (index + 3));
+		}
+	}
+
+	// we are no longer using heading fusion so set the reported test level to zero
+	_yaw_test_ratio = 0.0f;
+
+	// if any axis fails, abort the mag fusion
+	if (!all_innovation_checks_passed) {
+		return;
+	}
+
+	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
+	// before they are used to constrain heading drift
+	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
+
+	// Observation jacobian and Kalman gain vectors
+	SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
+	Vector24f Kfusion;
+
+	// update the states and covariance using sequential fusion of the magnetometer components
+	for (uint8_t index = 0; index <= 2; index++) {
+
+		// Calculate Kalman gains and observation jacobians
+		if (index == 0) {
+			// Calculate X axis observation jacobians
+			Hfusion.at<0>() = 2*HKX0;
+			Hfusion.at<1>() = 2*HKX1;
+			Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4;
+			Hfusion.at<3>() = 2*HKX5;
+			Hfusion.at<16>() = HKX6;
+			Hfusion.at<17>() = 2*HKX7;
+			Hfusion.at<18>() = 2*HKX8 - 2*HKX9;
+			Hfusion.at<19>() = 1;
+
+			// Calculate X axis Kalman gains
+			if (update_all_states) {
+				Kfusion(0) = HKX16*HKX24;
+				Kfusion(1) = HKX22*HKX24;
+				Kfusion(2) = HKX19*HKX24;
+				Kfusion(3) = HKX21*HKX24;
+
+				for (unsigned row = 4; row <= 15; row++) {
+					Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19));
+				}
+
+				for (unsigned row = 22; row <= 23; row++) {
+					Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
+				}
+			}
+
+			Kfusion(16) = HKX17*HKX24;
+			Kfusion(17) = HKX20*HKX24;
+			Kfusion(18) = HKX18*HKX24;
+			Kfusion(19) = HKX23*HKX24;
+
+			for (unsigned row = 20; row <= 21; row++) {
+				Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
+			}
+
+		} else if (index == 1) {
+
+			// recalculate innovation variance becasue states and covariances have changed due to previous fusion
+			const float HKY0 = magD*q1 + magE*q0 - magN*q3;
+			const float HKY1 = magD*q0 - magE*q1 + magN*q2;
+			const float HKY2 = magD*q3 + magE*q2 + magN*q1;
+			const float HKY3 = magD*q2;
+			const float HKY4 = magE*q3;
+			const float HKY5 = magN*q0;
+			const float HKY6 = q1*q2;
+			const float HKY7 = q0*q3;
+			const float HKY8 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
+			const float HKY9 = q0*q1 + q2*q3;
+			const float HKY10 = 2*HKY9;
+			const float HKY11 = -2*HKY6 + 2*HKY7;
+			const float HKY12 = 2*HKY2;
+			const float HKY13 = 2*HKY0;
+			const float HKY14 = 2*HKY1;
+			const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
+			const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
+			const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
+			const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
+			const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
+			const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
+			const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
+			const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
+			const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
+
+			_mag_innov_var(1) = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
+
+			if (_mag_innov_var(1) < R_MAG) {
+				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+				_fault_status.flags.bad_mag_y = true;
+
+				// we need to re-initialise covariances and abort this fusion step
+				resetMagRelatedCovariances();
+				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
+				return;
+			}
+			const float HKY24 = 1.0F/_mag_innov_var(1);
+
+			// Calculate Y axis observation jacobians
+			Hfusion.setZero();
+			Hfusion.at<0>() = 2*HKY0;
+			Hfusion.at<1>() = 2*HKY1;
+			Hfusion.at<2>() = 2*HKY2;
+			Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5;
+			Hfusion.at<16>() = 2*HKY6 - 2*HKY7;
+			Hfusion.at<17>() = HKY8;
+			Hfusion.at<18>() = 2*HKY9;
+			Hfusion.at<20>() = 1;
+
+			// Calculate Y axis Kalman gains
+			if (update_all_states) {
+				Kfusion(0) = HKY16*HKY24;
+				Kfusion(1) = HKY21*HKY24;
+				Kfusion(2) = HKY22*HKY24;
+				Kfusion(3) = HKY19*HKY24;
+
+				for (unsigned row = 4; row <= 15; row++) {
+					Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20));
+				}
+
+				for (unsigned row = 22; row <= 23; row++) {
+					Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row));
+				}
+			}
+
+			Kfusion(16) = HKY18*HKY24;
+			Kfusion(17) = HKY17*HKY24;
+			Kfusion(18) = HKY20*HKY24;
+			Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
+			Kfusion(20) = HKY23*HKY24;
+			Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
+
+		} else if (index == 2) {
+
+			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
+			if (_control_status.flags.synthetic_mag_z) {
+				continue;
+			}
+
+			// recalculate innovation variance becasue states and covariances have changed due to previous fusion
+			const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
+			const float HKZ1 = magN*q3;
+			const float HKZ2 = magD*q1;
+			const float HKZ3 = magE*q0;
+			const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
+			const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
+			const float HKZ6 = q0*q2 + q1*q3;
+			const float HKZ7 = q2*q3;
+			const float HKZ8 = q0*q1;
+			const float HKZ9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) - ecl::powf(q2, 2) + ecl::powf(q3, 2);
+			const float HKZ10 = 2*HKZ6;
+			const float HKZ11 = -2*HKZ7 + 2*HKZ8;
+			const float HKZ12 = 2*HKZ5;
+			const float HKZ13 = 2*HKZ0;
+			const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
+			const float HKZ15 = 2*HKZ4;
+			const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
+			const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
+			const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
+			const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
+			const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
+			const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
+			const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
+			const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
+
+			_mag_innov_var(2) = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
+
+			if (_mag_innov_var(2) < R_MAG) {
+				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+				_fault_status.flags.bad_mag_z = true;
+
+				// we need to re-initialise covariances and abort this fusion step
+				resetMagRelatedCovariances();
+				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
+				return;
+			}
+
+			const float HKZ24 = 1.0F/_mag_innov_var(2);
+
+			// calculate Z axis observation jacobians
+			Hfusion.setZero();
+			Hfusion.at<0>() = 2*HKZ0;
+			Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
+			Hfusion.at<2>() = 2*HKZ4;
+			Hfusion.at<3>() = 2*HKZ5;
+			Hfusion.at<16>() = 2*HKZ6;
+			Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8;
+			Hfusion.at<18>() = HKZ9;
+			Hfusion.at<21>() = 1;
+
+			// Calculate Z axis Kalman gains
+			if (update_all_states) {
+				Kfusion(0) = HKZ16*HKZ24;
+				Kfusion(1) = HKZ19*HKZ24;
+				Kfusion(2) = HKZ22*HKZ24;
+				Kfusion(3) = HKZ21*HKZ24;
+
+				for (unsigned row = 4; row <= 15; row++) {
+					Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21));
+				}
+
+				for (unsigned row = 22; row <= 23; row++) {
+					Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row));
+				}
+			}
+
+			Kfusion(16) = HKZ20*HKZ24;
+			Kfusion(17) = HKZ18*HKZ24;
+			Kfusion(18) = HKZ17*HKZ24;
+
+			for (unsigned row = 19; row <= 20; row++) {
+				Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21));
+			}
+
+			Kfusion(21) = HKZ23*HKZ24;
+		}
+
+		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));
+
+		if (index == 0) {
+			_fault_status.flags.bad_mag_x = !is_fused;
+
+		} else if (index == 1) {
+			_fault_status.flags.bad_mag_y = !is_fused;
+
+		} else if (index == 2) {
+			_fault_status.flags.bad_mag_z = !is_fused;
+		}
+
+		if (is_fused) {
+			limitDeclination();
+		}
+	}
+}
+
+void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
+{
+	// assign intermediate state variables
+	const float &q0 = _state.quat_nominal(0);
+	const float &q1 = _state.quat_nominal(1);
+	const float &q2 = _state.quat_nominal(2);
+	const float &q3 = _state.quat_nominal(3);
+
+	const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
+	const float measurement = wrap_pi(yaw);
+
+	// calculate 321 yaw observation matrix
+	// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
+	bool canUseA = false;
+	const float SA0 = 2*q3;
+	const float SA1 = 2*q2;
+	const float SA2 = SA0*q0 + SA1*q1;
+	const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+	float SA4, SA5_inv;
+	if (sq(SA3) > 1e-6f) {
+		SA4 = 1.0F/sq(SA3);
+		SA5_inv = sq(SA2)*SA4 + 1;
+		canUseA = fabsf(SA5_inv) > 1e-6f;
+	}
+
+	bool canUseB = false;
+	const float SB0 = 2*q0;
+	const float SB1 = 2*q1;
+	const float SB2 = SB0*q3 + SB1*q2;
+	const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+	float SB3, SB5_inv;
+	if (sq(SB2) > 1e-6f) {
+		SB3 = 1.0F/sq(SB2);
+		SB5_inv = SB3*sq(SB4) + 1;
+		canUseB = fabsf(SB5_inv) > 1e-6f;
+	}
+
+	Vector4f H_YAW;
+
+	if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
+		const float SA5 = 1.0F/SA5_inv;
+		const float SA6 = 1.0F/SA3;
+		const float SA7 = SA2*SA4;
+		const float SA8 = 2*SA7;
+		const float SA9 = 2*SA6;
+
+		H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
+		H_YAW(1) = SA5*(SA1*SA6 - SA8*q1);
+		H_YAW(2) = SA5*(SA1*SA7 + SA9*q1);
+		H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
+	} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
+		const float SB5 = 1.0F/SB5_inv;
+		const float SB6 = 1.0F/SB2;
+		const float SB7 = SB3*SB4;
+		const float SB8 = 2*SB7;
+		const float SB9 = 2*SB6;
+
+		H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3);
+		H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
+		H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
+		H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
+	} else {
+		return;
+	}
+
+	// calculate the yaw innovation and wrap to the interval between +-pi
+	float innovation;
+	if (zero_innovation) {
+		innovation = 0.0f;
+	} else {
+		innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement);
+	}
+
+	// define the innovation gate size
+	float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
+
+	// Update the quaternion states and covariance matrix
+	updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
+}
+
+void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
+{
+	// assign intermediate state variables
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);
+
+	const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
+	const float measurement = wrap_pi(yaw);
+
+	// calculate 312 yaw observation matrix
+	// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
+	bool canUseA = false;
+	const float SA0 = 2*q3;
+	const float SA1 = 2*q2;
+	const float SA2 = SA0*q0 - SA1*q1;
+	const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
+	float SA4, SA5_inv;
+	if (sq(SA3) > 1e-6f) {
+		SA4 = 1.0F/sq(SA3);
+		SA5_inv = sq(SA2)*SA4 + 1;
+		canUseA = fabsf(SA5_inv) > 1e-6f;
+	}
+
+	bool canUseB = false;
+	const float SB0 = 2*q0;
+	const float SB1 = 2*q1;
+	const float SB2 = -SB0*q3 + SB1*q2;
+	const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
+	float SB3, SB5_inv;
+	if (sq(SB2) > 1e-6f) {
+		SB3 = 1.0F/sq(SB2);
+		SB5_inv = SB3*sq(SB4) + 1;
+		canUseB = fabsf(SB5_inv) > 1e-6f;
+	}
+
+	Vector4f H_YAW;
+
+	if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
+		const float SA5 = 1.0F/SA5_inv;
+		const float SA6 = 1.0F/SA3;
+		const float SA7 = SA2*SA4;
+		const float SA8 = 2*SA7;
+		const float SA9 = 2*SA6;
+
+		H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
+		H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1);
+		H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1);
+		H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
+	} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
+		const float SB5 = 1.0F/SB5_inv;
+		const float SB6 = 1.0F/SB2;
+		const float SB7 = SB3*SB4;
+		const float SB8 = 2*SB7;
+		const float SB9 = 2*SB6;
+
+		H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3);
+		H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
+		H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
+		H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
+	} else {
+		return;
+	}
+
+	float innovation;
+	if (zero_innovation) {
+		innovation = 0.0f;
+	} else {
+		// calculate the the innovation and wrap to the interval between +-pi
+		innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement);
+	}
+
+	// define the innovation gate size
+	float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
+
+	// Update the quaternion states and covariance matrix
+	updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
+}
+
+// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
+void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f& yaw_jacobian)
+{
+	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
+	// calculate the innovation variance
+	_heading_innov_var = variance;
+	for (unsigned row = 0; row <= 3; row++) {
+		float tmp = 0.0f;
+
+		for (uint8_t col = 0; col <= 3; col++) {
+			tmp += P(row,col) * yaw_jacobian(col);
+		}
+
+		_heading_innov_var += yaw_jacobian(row) * tmp;
+	}
+
+	float heading_innov_var_inv;
+
+	// check if the innovation variance calculation is badly conditioned
+	if (_heading_innov_var >= variance) {
+		// the innovation variance contribution from the state covariances is not negative, no fault
+		_fault_status.flags.bad_hdg = false;
+		heading_innov_var_inv = 1.0f / _heading_innov_var;
+
+	} else {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		_fault_status.flags.bad_hdg = true;
+
+		// we reinitialise the covariance matrix and abort this fusion step
+		initialiseCovariance();
+		ECL_ERR("mag yaw fusion numerical error - covariance reset");
+		return;
+	}
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	Vector24f Kfusion;
+
+	for (uint8_t row = 0; row <= 15; row++) {
+		for (uint8_t col = 0; col <= 3; col++) {
+			Kfusion(row) += P(row,col) * yaw_jacobian(col);
+		}
+
+		Kfusion(row) *= heading_innov_var_inv;
+	}
+
+	if (_control_status.flags.wind) {
+		for (uint8_t row = 22; row <= 23; row++) {
+			for (uint8_t col = 0; col <= 3; col++) {
+				Kfusion(row) += P(row,col) * yaw_jacobian(col);
+			}
+
+			Kfusion(row) *= heading_innov_var_inv;
+		}
+	}
+
+	// innovation test ratio
+	_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
+
+	// we are no longer using 3-axis fusion so set the reported test levels to zero
+	_mag_test_ratio.setZero();
+
+	// set the magnetometer unhealthy if the test fails
+	if (_yaw_test_ratio > 1.0f) {
+		_innov_check_fail_status.flags.reject_yaw = true;
+
+		// if we are in air we don't want to fuse the measurement
+		// we allow to use it when on the ground because the large innovation could be caused
+		// by interference or a large initial gyro bias
+		if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6)) {
+			// constrain the innovation to the maximum set by the gate
+			// we need to delay this forced fusion to avoid starting it
+			// immediately after touchdown, when the drone is still armed
+			float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
+			_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);
+
+			// also reset the yaw gyro variance to converge faster and avoid
+			// being stuck on a previous bad estimate
+			resetZDeltaAngBiasCov();
+
+		} else {
+			return;
+		}
+
+	} else {
+		_innov_check_fail_status.flags.reject_yaw = false;
+		_heading_innov = innovation;
+	}
+
+	// apply covariance correction via P_new = (I -K*H)*P
+	// first calculate expression for KHP
+	// then calculate P - KHP
+	SquareMatrix24f KHP;
+	float KH[4];
+
+	for (unsigned row = 0; row < _k_num_states; row++) {
+
+		KH[0] = Kfusion(row) * yaw_jacobian(0);
+		KH[1] = Kfusion(row) * yaw_jacobian(1);
+		KH[2] = Kfusion(row) * yaw_jacobian(2);
+		KH[3] = Kfusion(row) * yaw_jacobian(3);
+
+		for (unsigned column = 0; column < _k_num_states; column++) {
+			float tmp = KH[0] * P(0,column);
+			tmp += KH[1] * P(1,column);
+			tmp += KH[2] * P(2,column);
+			tmp += KH[3] * P(3,column);
+			KHP(row,column) = tmp;
+		}
+	}
+
+	const bool healthy = checkAndFixCovarianceUpdate(KHP);
+
+	_fault_status.flags.bad_hdg = !healthy;
+
+	if (healthy) {
+		// apply the covariance corrections
+		P -= KHP;
+
+		fixCovarianceErrors(true);
+
+		// apply the state corrections
+		fuse(Kfusion, _heading_innov);
+
+	}
+}
+
+void Ekf::fuseHeading()
+{
+	Vector3f mag_earth_pred;
+	float measured_hdg;
+
+	// Calculate the observation variance
+	float R_YAW;
+	if (_control_status.flags.mag_hdg) {
+		// using magnetic heading tuning parameter
+		R_YAW = sq(_params.mag_heading_noise);
+
+	} else if (_control_status.flags.ev_yaw) {
+		// using error estimate from external vision data
+		R_YAW = _ev_sample_delayed.angVar;
+
+	} else {
+		// default value
+		R_YAW = 0.01f;
+	}
+
+	// update transformation matrix from body to world frame using the current state estimate
+	_R_to_earth = Dcmf(_state.quat_nominal);
+
+	if (shouldUse321RotationSequence(_R_to_earth)) {
+		const float predicted_hdg = getEuler321Yaw(_R_to_earth);
+
+		if (_control_status.flags.mag_hdg) {
+			// Rotate the measurements into earth frame using the zero yaw angle
+			const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
+			mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
+
+			// the angle of the projection onto the horizontal gives the yaw angle
+			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+
+		} else if (_control_status.flags.ev_yaw) {
+			measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
+
+		} else {
+			measured_hdg = predicted_hdg;
+		}
+
+		// handle special case where yaw measurement is unavailable
+		bool fuse_zero_innov = false;
+		if (_is_yaw_fusion_inhibited) {
+			// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
+			// conditioned covariance matrix developing over time.
+			if (!_control_status.flags.vehicle_at_rest) {
+				// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
+				// unconstrained quaternion variance growth and record the predicted heading
+				// to use as an observation when movement ceases.
+				// TODO a better way of determining when this is necessary
+				const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
+				if (sumQuatVar > _params.quat_max_variance) {
+					fuse_zero_innov = true;
+					R_YAW = 0.25f;
+
+				}
+				_last_static_yaw = predicted_hdg;
+
+			} else {
+				// Vehicle is at rest so use the last moving prediction as an observation
+				// to prevent the heading from drifting and to enable yaw gyro bias learning
+				// before takeoff.
+				measured_hdg = _last_static_yaw;
+
+			}
+		} else {
+			_last_static_yaw = predicted_hdg;
+
+		}
+
+		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
+
+	} else {
+		const float predicted_hdg = getEuler312Yaw(_R_to_earth);
+
+		if (_control_status.flags.mag_hdg) {
+
+			// rotate the magnetometer measurements into earth frame using a zero yaw angle
+			const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
+			mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
+
+			// the angle of the projection onto the horizontal gives the yaw angle
+			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+
+		} else if (_control_status.flags.ev_yaw) {
+			measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
+
+		} else {
+			measured_hdg = predicted_hdg;
+		}
+
+		// handle special case where yaw measurement is unavailable
+		bool fuse_zero_innov = false;
+		if (_is_yaw_fusion_inhibited) {
+			// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
+			// conditioned covariance matrix developing over time.
+			if (!_control_status.flags.vehicle_at_rest) {
+				// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
+				// unconstrained quaterniion variance growth and record the predicted heading
+				// to use as an observation when movement ceases.
+				// TODO a better way of determining when this is necessary
+				const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
+				if (sumQuatVar > _params.quat_max_variance) {
+					fuse_zero_innov = true;
+					R_YAW = 0.25f;
+
+				}
+				_last_static_yaw = predicted_hdg;
+
+			} else {
+				// Vehicle is at rest so use the last moving prediction as an observation
+				// to prevent the heading from drifting and to enable yaw gyro bias learning
+				// before takeoff.
+				measured_hdg = _last_static_yaw;
+
+			}
+		} else {
+			_last_static_yaw = predicted_hdg;
+
+		}
+
+		fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
+
+	}
+}
+
+void Ekf::fuseDeclination(float decl_sigma)
+{
+	// assign intermediate state variables
+	const float &magN = _state.mag_I(0);
+	const float &magE = _state.mag_I(1);
+
+	// minimum North field strength before calculation becomes badly conditioned (T)
+	constexpr float N_field_min = 0.001f;
+
+	// observation variance (rad**2)
+	const float R_DECL = sq(decl_sigma);
+
+	// Calculate intermediate variables
+	if (fabsf(magN) < sq(N_field_min)) {
+		// calculation is badly conditioned close to +-90 deg declination
+		return;
+	}
+	const float HK0 = ecl::powf(magN, -2);
+	const float HK1 = HK0*ecl::powf(magE, 2) + 1.0F;
+	const float HK2 = 1.0F/HK1;
+	const float HK3 = 1.0F/magN;
+	const float HK4 = HK2*HK3;
+	const float HK5 = HK3*magE;
+	const float HK6 = HK5*P(16,17) - P(17,17);
+	const float HK7 = ecl::powf(HK1, -2);
+	const float HK8 = HK5*P(16,16) - P(16,17);
+	const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL;
+	float HK9;
+	if (innovation_variance > R_DECL) {
+		HK9 = HK4/innovation_variance;
+	} else {
+		// variance calculation is badly conditioned
+		return;
+	}
+
+	// Calculate the observation Jacobian
+	// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
+	// Note Hfusion indices do not match state indices
+	SparseVector24f<16,17> Hfusion;
+	Hfusion.at<16>() = -HK0*HK2*magE;
+	Hfusion.at<17>() = HK4;
+
+	// Calculate the Kalman gains
+	Vector24f Kfusion;
+	for (unsigned row = 0; row <= 15; row++) {
+		Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
+	}
+
+	Kfusion(16) = -HK8*HK9;
+	Kfusion(17) = -HK6*HK9;
+
+	for (unsigned row = 18; row <= 23; row++) {
+		Kfusion(row) = -HK9*(HK5*P(16,row) - P(17,row));
+	}
+
+	const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f);
+
+	const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation);
+
+	_fault_status.flags.bad_mag_decl = !is_fused;
+
+	if (is_fused) {
+		limitDeclination();
+	}
+}
+
+void Ekf::limitDeclination()
+{
+	// get a reference value for the earth field declinaton and minimum plausible horizontal field strength
+	// set to 50% of the horizontal strength from geo tables if location is known
+	float decl_reference;
+	float h_field_min = 0.001f;
+	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
+		// use parameter value until GPS is available, then use value returned by geo library
+		if (_NED_origin_initialised || ISFINITE(_mag_declination_gps)) {
+			decl_reference = _mag_declination_gps;
+			h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
+
+		} else {
+			decl_reference = math::radians(_params.mag_declination_deg);
+		}
+	} else {
+		// always use the parameter value
+		decl_reference = math::radians(_params.mag_declination_deg);
+	}
+
+	// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
+	// and can result in a reversal of the NE field states which the filter cannot recover from
+	// apply a circular limit
+	float h_field = sqrtf(_state.mag_I(0)*_state.mag_I(0) + _state.mag_I(1)*_state.mag_I(1));
+	if (h_field < h_field_min) {
+		if (h_field > 0.001f * h_field_min) {
+			const float h_scaler = h_field_min / h_field;
+			_state.mag_I(0) *= h_scaler;
+			_state.mag_I(1) *= h_scaler;
+		} else {
+			// too small to scale radially so set to expected value
+			const float mag_declination = getMagDeclination();
+			_state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination);
+			_state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination);
+		}
+		h_field = h_field_min;
+	}
+
+	// do not allow the declination estimate to vary too much relative to the reference value
+	constexpr float decl_tolerance = 0.5f;
+	const float decl_max = decl_reference + decl_tolerance;
+	const float decl_min = decl_reference - decl_tolerance;
+	const float decl_estimate = atan2f(_state.mag_I(1) , _state.mag_I(0));
+	if (decl_estimate > decl_max)  {
+		_state.mag_I(0) = h_field * cosf(decl_max);
+		_state.mag_I(1) = h_field * sinf(decl_max);
+	} else if (decl_estimate < decl_min)  {
+		_state.mag_I(0) = h_field * cosf(decl_min);
+		_state.mag_I(1) = h_field * sinf(decl_min);
+	}
+}
+
+float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f& mag_meas, const Vector3f& mag_earth_predicted)
+{
+	// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
+	// of the earth magnetic field vector at the current location
+	const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));
+
+	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component
+	const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2));
+
+	return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs;
+}

src/lib/ecl/EKF/matlab/EKF_replay/Common/AlignTilt.m

@@ -0,0 +1,20 @@
+function quat = AlignTilt( ...
+    quat, ... % quaternion state vector
+    initAccel)  % initial accelerometer vector
+% check length
+lengthAccel = sqrt(dot([initAccel(1);initAccel(2);initAccel(3)],[initAccel(1);initAccel(2);initAccel(3)]));
+% if length is > 0.7g and < 1.4g initialise tilt using gravity vector
+if (lengthAccel > 5 && lengthAccel < 14)
+    % calculate length of the tilt vector
+    tiltMagnitude = atan2(sqrt(dot([initAccel(1);initAccel(2)],[initAccel(1);initAccel(2)])),-initAccel(3));
+    % take the unit cross product of the accel vector and the -Z vector to
+    % give the tilt unit vector
+    if (tiltMagnitude > 1e-3)
+        tiltUnitVec = cross([initAccel(1);initAccel(2);initAccel(3)],[0;0;-1]);
+        tiltUnitVec = tiltUnitVec/sqrt(dot(tiltUnitVec,tiltUnitVec));
+        tiltVec = tiltMagnitude*tiltUnitVec;
+        quat = [cos(0.5*tiltMagnitude); tiltVec/tiltMagnitude*sin(0.5*tiltMagnitude)];
+    end
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Common/ConvertToC.m

@@ -0,0 +1,220 @@
+function ConvertToC(fileName)
+delimiter = '';
+
+%% Format string for each line of text:
+%   column1: text (%s)
+% For more information, see the TEXTSCAN documentation.
+formatSpec = '%s%[^\n\r]';
+
+%% Open the text file.
+fileID = fopen(strcat(fileName,'.m'),'r');
+
+%% Read columns of data according to format string.
+% This call is based on the structure of the file used to generate this
+% code. If an error occurs for a different file, try regenerating the code
+% from the Import Tool.
+dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter,  'ReturnOnError', false, 'Bufsize', 65535);
+
+%% Close the text file.
+fclose(fileID);
+
+%% Create output variable
+SymbolicOutput = [dataArray{1:end-1}];
+
+%% Clear temporary variables
+clearvars filename delimiter formatSpec fileID dataArray ans;
+
+%% Convert indexing and replace brackets
+
+% replace 1-D indexes
+for arrayIndex = 1:99
+    strIndex = int2str(arrayIndex);
+    strRep = sprintf('[%d]',(arrayIndex-1));
+    strPat = strcat('\(',strIndex,'\)');
+    for lineIndex = 1:length(SymbolicOutput)
+        str = char(SymbolicOutput(lineIndex));
+        SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
+    end
+end
+
+% replace 2-D left indexes
+for arrayIndex = 1:99
+    strIndex = int2str(arrayIndex);
+    strRep = sprintf('[%d,',(arrayIndex-1));
+    strPat = strcat('\(',strIndex,'\,');
+    for lineIndex = 1:length(SymbolicOutput)
+        str = char(SymbolicOutput(lineIndex));
+        SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
+    end
+end
+
+% replace 2-D right indexes
+for arrayIndex = 1:99
+    strIndex = int2str(arrayIndex);
+    strRep = sprintf(',%d]',(arrayIndex-1));
+    strPat = strcat('\,',strIndex,'\)');
+    for lineIndex = 1:length(SymbolicOutput)
+        str = char(SymbolicOutput(lineIndex));
+        SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
+    end
+end
+
+% replace commas
+for lineIndex = 1:length(SymbolicOutput)
+    str = char(SymbolicOutput(lineIndex));
+    SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
+end
+
+%% replace . operators
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    strIn = regexprep(strIn,'\.\*','\*');
+    strIn = regexprep(strIn,'\.\/','\/');
+    strIn = regexprep(strIn,'\.\^','\^');
+    SymbolicOutput(lineIndex) = cellstr(strIn);
+end
+
+%% Replace ^2
+
+% replace where adjacent to ) parenthesis
+for lineIndex = 1:length(SymbolicOutput)
+    idxsq = regexp(SymbolicOutput(lineIndex),'\)\^2');
+    if ~isempty(idxsq{1})
+        strIn = SymbolicOutput(lineIndex);
+        idxlp = regexp(strIn,'\(');
+        idxrp = regexp(strIn,'\)');
+        for pwrIndex = 1:length(idxsq{1})
+            counter = 1;
+            index = idxsq{1}(pwrIndex);
+            endIndex(pwrIndex) = index;
+            while (counter > 0 && index > 0)
+                index = index - 1;
+                counter = counter + ~isempty(find(idxrp{1} == index, 1));
+                counter = counter - ~isempty(find(idxlp{1} == index, 1));
+            end
+            startIndex(pwrIndex) = index;
+            %            strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
+            strRep = strcat('sq',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)));
+            %            cellStrPat(pwrIndex) = cellstr(strPat);
+            cellStrRep(pwrIndex) = cellstr(strRep);
+        end
+        for pwrIndex = 1:length(idxsq{1})
+            strRep = char(cellStrRep(pwrIndex));
+            strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+2) = strRep;
+        end
+        SymbolicOutput(lineIndex) = strIn;
+    end
+end
+
+% replace where adjacent to ] parenthesis
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    [match,idxsq1,idxsq2] = regexp(strIn,'\w*\[\w*\]\^2','match','start','end');
+    [idxsq3] = regexp(strIn,'\[\w*\]\^2','start');
+    if ~isempty(match)
+        for pwrIndex = 1:length(match)
+            strVar   = strIn(idxsq1(pwrIndex):idxsq3(pwrIndex)-1);
+            strIndex = strIn(idxsq3(pwrIndex)+1:idxsq2(pwrIndex)-3);
+            strPat = strcat(strVar,'\[',strIndex,'\]\^2');
+            strRep = strcat('sq(',strVar,'[',strIndex,']',')');
+            strIn = regexprep(strIn,strPat,strRep);
+        end
+        SymbolicOutput(lineIndex) = cellstr(strIn);
+    end
+end
+
+% replace where adjacent to alpanumeric characters
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    [match,idxsq1,idxsq2] = regexp(strIn,'\w*\^2','match','start','end');
+    [idxsq3] = regexp(strIn,'\^2','start');
+    if ~isempty(match)
+        for pwrIndex = 1:length(match)
+            strVar   = strIn(idxsq1(pwrIndex)+2*(pwrIndex-1):idxsq2(pwrIndex)-2+2*(pwrIndex-1));
+            strPat = strcat(strVar,'\^2');
+            strRep = strcat('sq(',strVar,')');
+            strIn = regexprep(strIn,strPat,strRep);
+        end
+        SymbolicOutput(lineIndex) = cellstr(strIn);
+    end
+end
+
+%% Replace ^(1/2)
+
+% replace where adjacent to ) parenthesis
+for lineIndex = 1:length(SymbolicOutput)
+    idxsq = regexp(SymbolicOutput(lineIndex),'\)\^\(1\/2\)');
+    if ~isempty(idxsq{1})
+        strIn = SymbolicOutput(lineIndex);
+        idxlp = regexp(strIn,'\(');
+        idxrp = regexp(strIn,'\)');
+        for pwrIndex = 1:length(idxsq{1})
+            counter = 1;
+            index = idxsq{1}(pwrIndex);
+            endIndex(pwrIndex) = index;
+            while (counter > 0 && index > 0)
+                index = index - 1;
+                counter = counter + ~isempty(find(idxrp{1} == index, 1));
+                counter = counter - ~isempty(find(idxlp{1} == index, 1));
+            end
+            startIndex(pwrIndex) = index;
+            %            strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
+            strRep = strcat('(sqrt',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),')');
+            %            cellStrPat(pwrIndex) = cellstr(strPat);
+            cellStrRep(pwrIndex) = cellstr(strRep);
+        end
+        for pwrIndex = 1:length(idxsq{1})
+            strRep = char(cellStrRep(pwrIndex));
+            strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+6) = strRep;
+        end
+        SymbolicOutput(lineIndex) = strIn;
+    end
+end
+
+%% Replace Divisions
+% Compiler looks after this type of optimisation for us
+% for lineIndex = 1:length(SymbolicOutput)
+%     strIn = char(SymbolicOutput(lineIndex));
+%     strIn = regexprep(strIn,'\/2','\*0\.5');
+%     strIn = regexprep(strIn,'\/4','\*0\.25');
+%     SymbolicOutput(lineIndex) = cellstr(strIn);
+% end
+
+%% Convert declarations
+for lineIndex = 1:length(SymbolicOutput)
+    str = char(SymbolicOutput(lineIndex));
+    if ~isempty(regexp(str,'zeros', 'once'))
+        index1 = regexp(str,' = zeros[','once')-1;
+        index2 = regexp(str,' = zeros[','end','once')+1;
+        index3 = regexp(str,'\]\[','once')-1;
+        index4 = index3 + 3;
+        index5 = max(regexp(str,'\]'))-1;
+        str1 = {'float '};
+        str2 = str(1:index1);
+        str3 = '[';
+        str4 = str(index2:index3);
+        str4 = num2str(str2num(str4)+1);
+        str5 = '][';
+        str6 = str(index4:index5);
+        str6 = num2str(str2num(str6)+1);
+        str7 = '];';
+        SymbolicOutput(lineIndex) = strcat(str1,str2,str3,str4,str5,str6,str7);
+    end
+end
+
+%% Change covariance matrix variable name to P
+for lineIndex = 1:length(SymbolicOutput)
+    strIn = char(SymbolicOutput(lineIndex));
+    strIn = regexprep(strIn,'OP\[','P[');
+    SymbolicOutput(lineIndex) = cellstr(strIn);
+end
+
+%% Write to file
+fileName = strcat(fileName,'.cpp');
+fid = fopen(fileName,'wt');
+for lineIndex = 1:length(SymbolicOutput)
+    fprintf(fid,char(SymbolicOutput(lineIndex)));
+    fprintf(fid,'\n');
+end
+fclose(fid);
+clear all;

src/lib/ecl/EKF/matlab/EKF_replay/Common/ConvertToM.m

@@ -0,0 +1,46 @@
+function ConvertToM(nStates)
+%% Initialize variables
+fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
+delimiter = '';
+
+%% Format string for each line of text:
+%   column1: text (%s)
+% For more information, see the TEXTSCAN documentation.
+formatSpec = '%s%[^\n\r]';
+
+%% Open the text file.
+fileID = fopen(fileName,'r');
+
+%% Read columns of data according to format string.
+% This call is based on the structure of the file used to generate this
+% code. If an error occurs for a different file, try regenerating the code
+% from the Import Tool.
+dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter,  'ReturnOnError', false,'Bufsize',65535);
+
+%% Close the text file.
+fclose(fileID);
+
+%% Create output variable
+SymbolicOutput = [dataArray{1:end-1}];
+
+%% Clear temporary variables
+clearvars filename delimiter formatSpec fileID dataArray ans;
+
+%% replace brackets and commas
+for lineIndex = 1:length(SymbolicOutput)
+    SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
+    SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
+    SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
+end
+
+%% Write to file
+fileName = strcat('M_code',int2str(nStates),'.txt');
+fid = fopen(fileName,'wt');
+for lineIndex = 1:length(SymbolicOutput)
+    fprintf(fid,char(SymbolicOutput(lineIndex)));
+    fprintf(fid,'\n');
+end
+fclose(fid);
+clear all;
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Common/EulToQuat.m

@@ -0,0 +1,23 @@
+function quaterion = EulToQuat(Euler)
+
+% Convert from a 321 Euler rotation sequence specified in radians to a
+% Quaternion
+
+quaterion = zeros(4,1);
+
+Euler    = Euler * 0.5;
+cosPhi   = cos(Euler(1));
+sinPhi   = sin(Euler(1));
+cosTheta = cos(Euler(2));
+sinTheta = sin(Euler(2));
+cosPsi   = cos(Euler(3));
+sinPsi   = sin(Euler(3));
+
+quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi);
+quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi);
+quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
+quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
+
+return;
+
+   

src/lib/ecl/EKF/matlab/EKF_replay/Common/LLH2NED.m

@@ -0,0 +1,12 @@
+function posNED = LLH2NED(LLH,refLLH)
+
+radius = 6378137;
+flattening = 1/298.257223563;
+e = sqrt(flattening*(2-flattening));
+Rm = radius*(1-e^2)/(1-e^2*sin(refLLH(1)*pi/180)^2)^(3/2);
+Rn = radius/(1-e^2*sin(refLLH(1)*pi/180)^2)^(1/2);
+posN = (LLH(1,:)-refLLH(1))*pi/180.*(Rm+LLH(3,:));
+posE = (LLH(2,:)-refLLH(2))*pi/180.*(Rn+LLH(3,:))*cos(refLLH(1)*pi/180);
+posD = -(LLH(3,:)-refLLH(3));
+posNED = [posN;posE;posD];
+end

src/lib/ecl/EKF/matlab/EKF_replay/Common/NormQuat.m

@@ -0,0 +1,5 @@
+% normalise the quaternion
+function quaternion = normQuat(quaternion)
+
+quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
+quaternion(1:4) = quaternion / quatMag;

src/lib/ecl/EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m

@@ -0,0 +1,29 @@
+function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
+
+% Loop through symbolic expression, identifying repeated expressions and
+% bringing them out as shared expression or sub expressions
+% do this until no further repeated expressions found
+% This can significantly reduce computations
+
+syms SubExpIn SubExpArray ;
+
+SubExpArray(1,1) = 'invalid';
+index = 0;
+f_complete = 0;
+while f_complete==0
+    index = index + 1;
+    SubExpIn = [SubExpName,'(',num2str(index),')'];
+    SubExpInStore{index} = SubExpIn;
+    [SymExpOut,SubExpOut]=subexpr(SymExpIn,SubExpIn);
+        for k = 1:index
+            if SubExpOut == SubExpInStore{k}
+                f_complete = 1;
+            end
+        end
+    if f_complete || index > 100
+        SymExpOut = SymExpIn;
+    else
+        SubExpArray(index,1) = SubExpOut;
+        SymExpIn = SymExpOut;
+    end
+end

src/lib/ecl/EKF/matlab/EKF_replay/Common/Quat2Tbn.m

@@ -0,0 +1,14 @@
+function Tbn = Quat2Tbn(quat)
+
+% Convert from quaternions defining the flight vehicles rotation to
+% the direction cosine matrix defining the rotation from  body to navigation
+% coordinates
+
+q0 = quat(1);
+q1 = quat(2);
+q2 = quat(3);
+q3 = quat(4);
+  
+Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
+     2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
+     2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];

src/lib/ecl/EKF/matlab/EKF_replay/Common/QuatDivide.m

@@ -0,0 +1,16 @@
+function q_out = QuatDivide(qin1,qin2)
+
+q0 = qin1(1);
+q1 = qin1(2);
+q2 = qin1(3);
+q3 = qin1(4);
+
+r0 = qin2(1);
+r1 = qin2(2);
+r2 = qin2(3);
+r3 = qin2(4);
+
+q_out(1,1) = (qin2(1)*qin1(1) + qin2(2)*qin1(2) + qin2(3)*qin1(3) + qin2(4)*qin1(4));
+q_out(2,1) = (r0*q1 - r1*q0 - r2*q3 + r3*q2);
+q_out(3,1) = (r0*q2 + r1*q3 - r2*q0 - r3*q1);
+q_out(4,1) = (r0*q3 - r1*q2 + r2*q1 - r3*q0);

src/lib/ecl/EKF/matlab/EKF_replay/Common/QuatMult.m

@@ -0,0 +1,5 @@
+function quatOut = QuatMult(quatA,quatB)
+% Calculate the following quaternion product quatA * quatB using the 
+% standard identity 
+
+quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))];

src/lib/ecl/EKF/matlab/EKF_replay/Common/QuatToEul.m

@@ -0,0 +1,9 @@
+% Convert from a quaternion to a 321 Euler rotation sequence in radians
+
+function Euler = QuatToEul(quat)
+
+Euler = zeros(3,1);
+
+Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)),  quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4));
+Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
+Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)),  quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4));

src/lib/ecl/EKF/matlab/EKF_replay/Common/RotToQuat.m

@@ -0,0 +1,10 @@
+% convert froma rotation vector in radians to a quaternion
+function quaternion = RotToQuat(rotVec)
+
+vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
+
+if vecLength < 1e-6
+  quaternion = [1;0;0;0];
+else
+  quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
+end

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_apm_data.m

@@ -0,0 +1,103 @@
+%% convert baro data
+clear baro_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(BARO)
+    if (BARO(source_index,2) ~= last_time)
+        baro_data.time_us(output_index,1) = BARO(source_index,2);
+        baro_data.height(output_index) = BARO(source_index,3);
+        last_time = BARO(source_index,2);
+        output_index = output_index + 1;
+    end
+end
+save baro_data.mat baro_data;
+
+%% extract IMU delta angles and velocity data
+clear imu_data;
+imu_data.time_us = IMT(:,2);
+imu_data.gyro_dt = IMT(:,5);
+imu_data.del_ang = IMT(:,6:8);
+imu_data.accel_dt = IMT(:,4);
+imu_data.del_vel = IMT(:,9:11);
+save imu_data.mat imu_data;
+
+%% convert magnetometer data
+clear mag_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(MAG)
+    mag_timestamp = MAG(source_index,2);
+    if (mag_timestamp ~= last_time)
+        mag_data.time_us(output_index,1) = mag_timestamp;
+        mag_data.field_ga(output_index,:) = 0.001*[MAG(source_index,3),MAG(source_index,4),MAG(source_index,5)];
+        last_time = mag_timestamp;
+        output_index = output_index + 1;
+    end
+end
+save mag_data.mat mag_data;
+
+%% save GPS daa
+clear gps_data;
+
+maxindex = min(length(GPS),length(GPA));
+
+gps_data.time_us = GPS(1:maxindex,2);
+gps_data.pos_error = GPA(1:maxindex,4);
+gps_data.spd_error = GPA(1:maxindex,6);
+gps_data.hgt_error = GPA(1:maxindex,5);
+
+% set reference point used to set NED origin when GPS accuracy is sufficient
+gps_data.start_index = max(find(gps_data.pos_error < 5.0, 1 ),find(gps_data.spd_error < 1.0, 1 ));
+gps_data.refLLH = [GPS(gps_data.start_index,8);GPS(gps_data.start_index,9);GPS(gps_data.start_index,10)];
+
+% convert GPS data to NED
+deg2rad = pi/180;
+for index = 1:1:maxindex
+    if (GPS(index,3) >= 3)
+        gps_data.pos_ned(index,:) = LLH2NED([GPS(index,8);GPS(index,9);GPS(index,10)],gps_data.refLLH);
+        gps_data.vel_ned(index,:) = [GPS(index,11).*cos(deg2rad*GPS(index,12)),GPS(index,11).*sin(deg2rad*GPS(index,12)),GPS(index,13)];
+    else
+        gps_data.pos_ned(index,:) = [0,0,0];
+        gps_data.vel_ned(index,:) = [0,0,0];
+    end
+end
+
+save gps_data.mat gps_data;
+
+%% save range finder data
+clear rng_data;
+if (exist('RFND','var'))
+    rng_data.time_us = RFND(:,2);
+    rng_data.dist = RFND(:,3);
+    save rng_data.mat rng_data;
+end
+
+%% save optical flow data
+clear flow_data;
+if (exist('OF','var'))
+    flow_data.time_us = OF(:,2);
+    flow_data.qual = OF(:,3)/255; % scale quality from 0 to 1
+    flow_data.flowX = OF(:,4); % optical flow rate about the X body axis (rad/sec)
+    flow_data.flowY = OF(:,5); % optical flow rate about the Y body axis (rad/sec)
+    flow_data.bodyX = OF(:,6); % angular rate about the X body axis (rad/sec)
+    flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec)
+    save flow_data.mat flow_data;
+end
+
+%% save visual odometry data
+clear viso_data;
+if (exist('VISO','var'))
+    viso_data.time_us = VISO(:,2);
+    viso_data.dt = VISO(:,3); % time period the measurement was sampled across (sec)
+    viso_data.dAngX = VISO(:,4); % delta angle about the X body axis (rad)
+    viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis (rad)
+    viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis (rad)
+    viso_data.dPosX = VISO(:,7); % delta position along the X body axis (m)
+    viso_data.dPosY = VISO(:,8); % delta position along the Y body axis (m)
+    viso_data.dPosZ = VISO(:,9); % delta position along the Z body axis (m)
+    viso_data.qual = VISO(:,10)/100; % quality from 0 - 1
+    save viso_data.mat viso_data;
+end
+
+%% save data and clear workspace
+clearvars -except baro_data imu_data mag_data gps_data rng_data flow_data viso_data;

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_px4_actuators_csv_data.m

@@ -0,0 +1,34 @@
+%% convert actuator control data
+clear actctrl_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_actctrl)
+    actctrl_timestamp = timestamp_actctrl(source_index);
+    if (actctrl_timestamp ~= last_time)
+        actctrl_data.time_us(output_index,1) = actctrl_timestamp;
+        actctrl_data.M_XYZ(output_index,:) = [control0(source_index),control1(source_index),control2(source_index)];
+        actctrl_data.F_t(output_index,:) = control3(source_index);
+        last_time = actctrl_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% convert actuator output data
+clear actout_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_actout)
+    actout_timestamp = timestamp_actout(source_index);
+    if (actout_timestamp ~= last_time)
+        actout_data.time_us(output_index,1) = actout_timestamp;
+        actout_data.pwm(output_index,:) = [output0(source_index),output1(source_index),output2(source_index),output3(source_index)];
+        last_time = actout_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% save data
+% DO NOT clear the workspace (yet)
+
+save actctrl_data.mat actctrl_data;
+save actout_data.mat actout_data;

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m

@@ -0,0 +1,18 @@
+%% convert baro data
+clear rng_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp)
+    rng_timestamp = timestamp(source_index);
+    if (rng_timestamp ~= last_time)
+        last_time = rng_timestamp;
+        rng_data.time_us(output_index,1) = rng_timestamp;
+        rng_data.dist(output_index,1) = current_distance(source_index);
+        output_index = output_index + 1;
+    end
+end
+
+%% save data and clear workspace
+clearvars -except rng_data;
+
+save rng_data.mat rng_data;

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_px4_groundtruth_csv_data.m

@@ -0,0 +1,53 @@
+%% convert attitude data
+clear attitude_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_att)
+    att_timestamp = timestamp_att(source_index);
+    if (att_timestamp ~= last_time)
+        attitude_data.time_us(output_index,1) = att_timestamp;
+        attitude_data.quat(output_index,:) = [q0(source_index),q1(source_index),q2(source_index),q3(source_index)];
+        attitude_data.del_ang(output_index,:) = [rollspeed(source_index),pitchspeed(source_index),yawspeed(source_index)];
+        last_time = att_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% convert global position data
+clear globalpos_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_gpos)
+    gpos_timestamp = timestamp_gpos(source_index);
+    if (gpos_timestamp ~= last_time)
+        globalpos_data.time_us(output_index,1) = gpos_timestamp;
+        globalpos_data.position_NED(output_index,:) = [gpos_lat(source_index),gpos_lon(source_index),gpos_alt(source_index)];
+        globalpos_data.velocity_NED(output_index,:) = [gpos_vel_n(source_index),gpos_vel_e(source_index),gpos_vel_d(source_index)];
+        last_time = gpos_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% convert local position data
+clear localpos_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_lpos)
+    lpos_timestamp = timestamp_lpos(source_index);
+    if (lpos_timestamp ~= last_time)
+        localpos_data.time_us(output_index,1) = lpos_timestamp;
+        localpos_data.ref_pos(output_index,:) = [lpos_ref_lat(source_index),lpos_ref_lon(source_index)];
+        localpos_data.position_XYZ(output_index,:) = [lpos_x(source_index),lpos_y(source_index),lpos_z(source_index)];
+        localpos_data.velocity_XYZ(output_index,:) = [lpos_vz(source_index),lpos_vy(source_index),lpos_vz(source_index)];
+        localpos_data.yaw(output_index,1) = lpos_yaw(source_index);
+        last_time = lpos_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% save data
+% DO NOT clear the workspace (yet)
+
+save attitude_data.mat attitude_data;
+save localpos_data.mat localpos_data;
+save globalpos_data.mat globalpos_data;

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m

@@ -0,0 +1,23 @@
+%% convert baro data
+clear flow_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp)
+    flow_timestamp = timestamp(source_index);
+    if ((flow_timestamp ~= last_time) && (integration_timespan(source_index) > 0))
+        last_time = flow_timestamp;
+        flow_data.time_us(output_index,1) = flow_timestamp;
+        flow_data.qual(output_index,1) = quality(source_index)/255; % scale quality from 0 to 1
+        dt_inv = 1e6 / integration_timespan(source_index);
+        flow_data.flowX(output_index,1) = dt_inv * pixel_flow_x_integral(source_index); % optical flow rate about the X body axis (rad/sec)
+        flow_data.flowY(output_index,1) = dt_inv * pixel_flow_y_integral(source_index); % optical flow rate about the Y body axis (rad/sec)
+        flow_data.bodyX(output_index,1) = dt_inv * gyro_x_rate_integral(source_index); % angular rate about the X body axis (rad/sec)
+        flow_data.bodyY(output_index,1) = dt_inv * gyro_y_rate_integral(source_index); % angular rate about the Y body axis (rad/sec)
+        output_index = output_index + 1;
+    end
+end
+
+%% save data and clear workspace
+clearvars -except flow_data;
+
+save flow_data.mat flow_data;

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m

@@ -0,0 +1,47 @@
+%% convert baro data
+clear baro_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_baro)
+    baro_timestamp = timestamp_baro(source_index);
+    if (baro_timestamp ~= last_time)
+        baro_data.time_us(output_index,1) = baro_timestamp;
+        baro_data.height(output_index) = baro_alt_meter(source_index);
+        last_time = baro_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% convert IMU data to delta angles and velocities
+% Note: these quatntis were converted from delta angles and velocites using
+% the integral_dt values in the PX4 sensor module so we only need to
+% multiply by integral_dt to convert back
+clear imu_data;
+n_samples = length(timestamp);
+imu_data.time_us = timestamp + accelerometer_timestamp_relative;
+imu_data.gyro_dt = gyro_integral_dt ./ 1e6;
+imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt];
+
+imu_data.accel_dt = accelerometer_integral_dt ./ 1e6;
+imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt];
+
+%% convert magnetometer data
+clear mag_data;
+last_time = 0;
+output_index = 1;
+for source_index = 1:length(timestamp_mag)
+    mag_timestamp = timestamp_mag(source_index);
+    if (mag_timestamp ~= last_time)
+        mag_data.time_us(output_index,1) = mag_timestamp;
+        mag_data.field_ga(output_index,:) = [magnetometer_ga0(source_index),magnetometer_ga1(source_index),magnetometer_ga2(source_index)];
+        last_time = mag_timestamp;
+        output_index = output_index + 1;
+    end
+end
+
+%% save data
+% DO NOT clear the workspace (yet)
+
+save baro_data.mat baro_data;
+save imu_data.mat imu_data;
+save mag_data.mat mag_data;

src/lib/ecl/EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m

@@ -0,0 +1,32 @@
+%% convert GPS data
+clear gps_data;
+gps_data.time_us = timestamp + timestamp_time_relative;
+gps_data.pos_error = eph;
+gps_data.spd_error = s_variance_m_s;
+gps_data.hgt_error = epv;
+
+% set reference point used to set NED origin when GPS accuracy is sufficient
+gps_data.start_index = max(min(find(gps_data.pos_error < 5.0)),min(find(gps_data.spd_error < 1.0)));
+if isempty(gps_data.start_index)
+    gps_data.start_index = 1;
+    gps_data.refLLH = [1e-7*median(lat);1e-7*median(lon);0.001*median(alt)];
+else
+    gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)];
+end
+
+% convert GPS data to NED
+for index = 1:length(timestamp)
+    if (fix_type(index) >= 3)
+        gps_data.pos_ned(index,:) = LLH2NED([1e-7*lat(index);1e-7*lon(index);0.001*alt(index)],gps_data.refLLH);
+        gps_data.vel_ned(index,:) = [vel_n_m_s(index),vel_e_m_s(index),vel_d_m_s(index)];
+    else
+        gps_data.pos_ned(index,:) = [0,0,0];
+        gps_data.vel_ned(index,:) = [0,0,0];
+    end
+end
+
+
+%% save data
+% DO NOT clear the workspace (yet)
+
+save gps_data.mat gps_data;

src/lib/ecl/EKF/matlab/EKF_replay/Filter/AlignHeading.m

@@ -0,0 +1,23 @@
+function quat = AlignHeading( ...
+    quat, ... % quaternion state vector
+    magMea, ... % body frame magnetic flux measurements 
+    declination)  % Estimated magnetic field delination at current location (rad)
+
+% Get the Euler angles and set yaw to zero
+euler = QuatToEul(quat);
+euler(3) = 0.0;
+
+% calculate the rotation matrix from body to earth frame
+quat_zero_yaw = EulToQuat(euler);
+Tbn_zero_yaw = Quat2Tbn(quat_zero_yaw);
+
+% Rotate the magnetic field measurement into earth frame
+magMeasNED = Tbn_zero_yaw*magMea;
+
+% Use the projection onto the horizontal to calculate the yaw angle
+euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1));
+
+% convert to a quaternion
+quat = EulToQuat(euler);
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/ConstrainStates.m

@@ -0,0 +1,23 @@
+function [states]  = ConstrainStates(states,dt_imu_avg)
+
+% constrain gyro bias states
+limit = 5.0*pi/180*dt_imu_avg;
+for i=11:13
+    if (states(i) > limit)
+        states(i) = limit;
+    elseif (states(i) < -limit)
+        states(i) = -limit;
+    end
+end
+
+% constrain accel bias states
+limit = 0.5*dt_imu_avg;
+for i=14:16
+    if (states(i) > limit)
+        states(i) = limit;
+    elseif (states(i) < -limit)
+        states(i) = -limit;
+    end
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m

@@ -0,0 +1,56 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation,... % NE position innovations (m)
+    varInnov] ... % NE position innovation variance (m^2)
+    = FuseBaroHeight( ...
+    states, ... % predicted states from the INS
+    P, ... % predicted covariance
+    measHgt, ... % NE position measurements (m)
+    gateSize, ... % Size of the innovation consistency check gate (std-dev)
+    R_OBS) % position observation variance (m)^2
+
+H = zeros(1,24);
+
+% position states start at index 8
+stateIndex = 10;
+
+% Calculate the vertical position height innovation (posD is opposite
+% sign to height)
+innovation = states(stateIndex) + measHgt;
+
+% Calculate the observation Jacobian
+H(stateIndex) = 1;
+
+varInnov = (H*P*transpose(H) + R_OBS);
+
+% Apply an innovation consistency check
+if (innovation^2 / (gateSize^2 * varInnov)) > 1.0
+    return;
+end
+
+% Calculate Kalman gains and update states and covariances
+
+% Calculate the Kalman gains
+K = (P*transpose(H))/varInnov;
+
+% Calculate state corrections
+xk = K * innovation;
+
+% Apply the state corrections
+states = states - xk;
+
+% Update the covariance
+P = P - K*H*P;
+
+% Force symmetry on the covariance matrix to prevent ill-conditioning
+P = 0.5*(P + transpose(P));
+
+% ensure diagonals are positive
+for i=1:24
+    if P(i,i) < 0
+        P(i,i) = 0;
+    end
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m

@@ -0,0 +1,82 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation, ... % XY optical flow innovations - rad/sec
+    varInnov] ... % XY optical flow innovation variances (rad/sec)^2
+    = FuseBodyVel( ...
+    states, ... % predicted states
+    P, ... % predicted covariance
+    relVelBodyMea, ... % XYZ velocity measured by the camera (m/sec)
+    obsVar, ... % velocity variances - (m/sec)^2
+    gateSize) % innovation gate size (SD)
+
+q0 = states(1);
+q1 = states(2);
+q2 = states(3);
+q3 = states(4);
+vn = states(5);
+ve = states(6);
+vd = states(7);
+
+innovation = zeros(1,2);
+varInnov = zeros(1,2);
+H = zeros(2,24);
+
+% Calculate predicted velocity measured in body frame axes
+Tbn = Quat2Tbn(states(1:4));
+relVelBodyPred = transpose(Tbn)*[vn;ve;vd];
+
+% calculate the observation jacobian, innovation variance and innovation
+for obsIndex = 1:3
+    
+    % Calculate corrections using X component
+    if (obsIndex == 1)
+        H(1,:) = calcH_VELX(q0,q1,q2,q3,vd,ve,vn);
+    elseif (obsIndex == 2)
+        H(2,:) = calcH_VELY(q0,q1,q2,q3,vd,ve,vn);
+    elseif (obsIndex == 3)
+        H(3,:) = calcH_VELZ(q0,q1,q2,q3,vd,ve,vn);
+    end
+    varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + obsVar);
+    innovation(obsIndex) = relVelBodyPred(obsIndex) - relVelBodyMea(obsIndex);
+end
+
+% check innovations for consistency and exit if they fail the test
+for obsIndex = 1:3
+    if (innovation(obsIndex)^2 / (varInnov(obsIndex) * gateSize^2) > 1.0);
+        return;
+    end
+end
+
+% calculate the kalman gains and perform the state and covariance update
+% using sequential fusion
+for obsIndex = 1:3
+
+    Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
+    
+    % correct the state vector
+    states = states - Kfusion * innovation(obsIndex);
+    
+    % normalise the updated quaternion states
+    quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
+    if (quatMag > 1e-12)
+        states(1:4) = states(1:4) / quatMag;
+    end
+    
+    % correct the covariance P = P - K*H*P
+    P = P - Kfusion*H(obsIndex,:)*P;
+    
+    % Force symmetry on the covariance matrix to prevent ill-conditioning
+    % of the matrix which would cause the filter to blow-up
+    P = 0.5*(P + transpose(P));
+    
+    % ensure diagonals are positive
+    for i=1:24
+        if P(i,i) < 0
+            P(i,i) = 0;
+        end
+    end
+    
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m

@@ -0,0 +1,47 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P] ... %
+    = FuseMagDeclination( ...
+    states, ... % predicted states
+    P, ... % predicted covariance
+    measDec) % magnetic field declination - azimuth angle measured from true north (rad)
+
+magN = states(17);
+magE = states(18);
+
+R_MAG = 0.5^2;
+
+H = calcH_MAGD(magE,magN);
+varInnov = (H*P*transpose(H) + R_MAG);
+Kfusion = (P*transpose(H))/varInnov;
+
+% Calculate the predicted magnetic declination
+predDec = atan2(magE,magN);
+
+% Calculate the measurement innovation
+innovation = predDec - measDec;
+
+if (innovation > pi)
+    innovation = innovation - 2*pi;
+elseif (innovation < -pi)
+    innovation = innovation + 2*pi;
+end
+
+% correct the state vector
+states = states - Kfusion * innovation;
+
+% correct the covariance P = P - K*H*P
+P = P - Kfusion*H*P;
+
+% Force symmetry on the covariance matrix to prevent ill-conditioning
+% of the matrix which would cause the filter to blow-up
+P = 0.5*(P + transpose(P));
+
+% ensure diagonals are positive
+for i=1:24
+    if P(i,i) < 0
+        P(i,i) = 0;
+    end
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseMagHeading.m

@@ -0,0 +1,71 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation, ... % Declination innovation - rad
+    varInnov] ... %
+    = FuseMagHeading( ...
+    states, ... % predicted states
+    P, ... % predicted covariance
+    magData, ... % XYZ body frame magnetic flux measurements - gauss
+    measDec, ... % magnetic field declination - azimuth angle measured from true north (rad)
+    innovGate, ... % innovation gate size (SD)
+    R_MAG) % magnetic heading measurement variance - rad^2
+
+q0 = states(1);
+q1 = states(2);
+q2 = states(3);
+q3 = states(4);
+
+magX = magData(1);
+magY = magData(2);
+magZ = magData(3);
+
+H = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3);
+varInnov = (H*P*transpose(H) + R_MAG);
+Kfusion = (P*transpose(H))/varInnov;
+
+% Calculate the predicted magnetic declination
+Tbn = Quat2Tbn(states(1:4));
+magMeasNED = Tbn*[magX;magY;magZ];
+predDec = atan2(magMeasNED(2),magMeasNED(1));
+
+% Calculate the measurement innovation
+innovation = predDec - measDec;
+
+if (innovation > pi)
+    innovation = innovation - 2*pi;
+elseif (innovation < -pi)
+    innovation = innovation + 2*pi;
+end
+
+% Apply a innovation consistency check
+if (innovation^2 / (innovGate^2 * varInnov)) > 1.0
+    innovation = NaN;
+    varInnov = NaN;
+    return;
+end
+
+% correct the state vector
+states = states - Kfusion * innovation;
+
+% normalise the updated quaternion states
+quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
+if (quatMag > 1e-12)
+    states(1:4) = states(1:4) / quatMag;
+end
+
+% correct the covariance P = P - K*H*P
+P = P - Kfusion*H*P;
+
+% Force symmetry on the covariance matrix to prevent ill-conditioning
+% of the matrix which would cause the filter to blow-up
+P = 0.5*(P + transpose(P));
+
+% ensure diagonals are positive
+for i=1:24
+    if P(i,i) < 0
+        P(i,i) = 0;
+    end
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m

@@ -0,0 +1,87 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation, ... % Declination innovation - rad
+    varInnov] ... %
+    = FuseMagnetometer( ...
+    states, ... % predicted states
+    P, ... % predicted covariance
+    magMea, ... % body frame magnetic flux measurements
+    testRatio, ... % Size of magnetometer innovation in standard deviations before measurements are rejected
+    R_MAG) % magnetometer measurement variance - gauss^2
+
+q0 = states(1);
+q1 = states(2);
+q2 = states(3);
+q3 = states(4);
+
+magXbias = states(20);
+magYbias = states(21);
+magZbias = states(22);
+
+magN = states(17);
+magE = states(18);
+magD = states(19);
+
+innovation = zeros(1,3);
+varInnov = zeros(1,3);
+H = zeros(3,24);
+
+% Calculate the predicted magnetometer measurement
+Tbn = Quat2Tbn(states(1:4));
+magPred = transpose(Tbn)*[magN;magE;magD] + [magXbias;magYbias;magZbias];
+
+% calculate the observation jacobian, innovation variance and innovation
+for obsIndex = 1:3
+    
+    % Calculate corrections using X component
+    if (obsIndex == 1)
+        H(1,:) = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3);
+    elseif (obsIndex == 2)
+        H(2,:) = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3);
+    elseif (obsIndex == 3)
+        H(3,:) = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3);
+    end
+    varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_MAG);
+    innovation(obsIndex) = magPred(obsIndex) - magMea(obsIndex);
+end
+
+% check innovations for consistency and exit if they fail the test
+for obsIndex = 1:3
+    if (innovation(obsIndex)^2 / (varInnov(obsIndex) * testRatio^2) > 1.0);
+        return;
+    end
+end
+
+% calculate the kalman gains and perform the state and covariance update
+% using sequential fusion
+for obsIndex = 1:3
+
+    Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
+    
+    % correct the state vector
+    states = states - Kfusion * innovation(obsIndex);
+    
+    % normalise the updated quaternion states
+    quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
+    if (quatMag > 1e-12)
+        states(1:4) = states(1:4) / quatMag;
+    end
+    
+    % correct the covariance P = P - K*H*P
+    P = P - Kfusion*H(obsIndex,:)*P;
+    
+    % Force symmetry on the covariance matrix to prevent ill-conditioning
+    % of the matrix which would cause the filter to blow-up
+    P = 0.5*(P + transpose(P));
+    
+    % ensure diagonals are positive
+    for i=1:24
+        if P(i,i) < 0
+            P(i,i) = 0;
+        end
+    end
+    
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m

@@ -0,0 +1,88 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation, ... % XY optical flow innovations - rad/sec
+    varInnov] ... % XY optical flow innovation variances (rad/sec)^2
+    = FuseOpticalFlow( ...
+    states, ... % predicted states
+    P, ... % predicted covariance
+    flowRate, ... % XY axis optical flow rate (rad/sec)
+    bodyRate, ... % XY axis body rate (rad/sec)
+    range,  ... % range from lens to ground measured along the centre of the optical flow sensor field of view
+    flowObsVar, ... % flow observation variance - (rad/sec)^2
+    gateSize) % innovation gate size (SD)
+
+q0 = states(1);
+q1 = states(2);
+q2 = states(3);
+q3 = states(4);
+vn = states(5);
+ve = states(6);
+vd = states(7);
+
+innovation = zeros(1,2);
+varInnov = zeros(1,2);
+H = zeros(2,24);
+
+% Calculate predicted angular LOS rates about body frame axes
+Tbn = Quat2Tbn(states(1:4));
+relVelBody = transpose(Tbn)*[vn;ve;vd];
+losRatePred(1) = +relVelBody(2)/range;
+losRatePred(2) = -relVelBody(1)/range;
+
+% Calculate measured LOS angular rates using body motion corrected flow
+% measurements
+losRateMea = - flowRate + bodyRate;
+
+% calculate the observation jacobian, innovation variance and innovation
+for obsIndex = 1:2
+    
+    % Calculate corrections using X component
+    if (obsIndex == 1)
+        H(1,:) = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn);
+    elseif (obsIndex == 2)
+        H(2,:) = calcH_LOSY(q0,q1,q2,q3,range,vd,ve,vn);
+    end
+    varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + flowObsVar);
+    innovation(obsIndex) = losRatePred(obsIndex) - losRateMea(obsIndex);
+end
+
+% check innovations for consistency and exit if they fail the test
+for obsIndex = 1:2
+    if (innovation(obsIndex)^2 / (varInnov(obsIndex) * gateSize^2) > 1.0);
+        return;
+    end
+end
+
+% calculate the kalman gains and perform the state and covariance update
+% using sequential fusion
+for obsIndex = 1:2
+
+    Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
+    
+    % correct the state vector
+    states = states - Kfusion * innovation(obsIndex);
+    
+    % normalise the updated quaternion states
+    quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
+    if (quatMag > 1e-12)
+        states(1:4) = states(1:4) / quatMag;
+    end
+    
+    % correct the covariance P = P - K*H*P
+    P = P - Kfusion*H(obsIndex,:)*P;
+    
+    % Force symmetry on the covariance matrix to prevent ill-conditioning
+    % of the matrix which would cause the filter to blow-up
+    P = 0.5*(P + transpose(P));
+    
+    % ensure diagonals are positive
+    for i=1:24
+        if P(i,i) < 0
+            P(i,i) = 0;
+        end
+    end
+    
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FusePosition.m

@@ -0,0 +1,68 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation,... % NE position innovations (m)
+    varInnov] ... % NE position innovation variance (m^2)
+    = FusePosition( ...
+    states, ... % predicted states from the INS
+    P, ... % predicted covariance
+    measPos, ... % NE position measurements (m)
+    gateSize, ... % Size of the innovation consistency check gate (std-dev)
+    R_OBS) % position observation variance (m)^2
+
+innovation = zeros(1,2);
+varInnov = zeros(1,2);
+H = zeros(2,24);
+
+for obsIndex = 1:2
+    
+    % velocity states start at index 8
+    stateIndex = 7 + obsIndex;
+
+    % Calculate the velocity measurement innovation
+    innovation(obsIndex) = states(stateIndex) - measPos(obsIndex);
+    
+    % Calculate the observation Jacobian
+    H(obsIndex,stateIndex) = 1;
+    
+    varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
+    
+end
+
+% Apply an innovation consistency check
+for obsIndex = 1:2
+    
+    if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
+        return;
+    end
+    
+end
+
+% Calculate Kalman gains and update states and covariances
+for obsIndex = 1:2
+    
+    % Calculate the Kalman gains
+    K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
+    
+    % Calculate state corrections
+    xk = K * innovation(obsIndex);
+    
+    % Apply the state corrections
+    states = states - xk;
+    
+    % Update the covariance
+    P = P - K*H(obsIndex,:)*P;
+    
+    % Force symmetry on the covariance matrix to prevent ill-conditioning
+    P = 0.5*(P + transpose(P));
+    
+    % ensure diagonals are positive
+    for i=1:24
+        if P(i,i) < 0
+            P(i,i) = 0;
+        end
+    end
+    
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/FuseVelocity.m

@@ -0,0 +1,68 @@
+function [...
+    states, ... % state vector after fusion of measurements
+    P, ... % state covariance matrix after fusion of corrections
+    innovation,... % NED velocity innovations (m/s)
+    varInnov] ... % NED velocity innovation variance ((m/s)^2)
+    = FuseVelocity( ...
+    states, ... % predicted states from the INS
+    P, ... % predicted covariance
+    measVel, ... % NED velocity measurements (m/s)
+    gateSize, ... % Size of the innovation consistency check gate (std-dev)
+    R_OBS) % velocity observation variance (m/s)^2
+
+innovation = zeros(1,3);
+varInnov = zeros(1,3);
+H = zeros(3,24);
+
+for obsIndex = 1:3
+    
+    % velocity states start at index 5
+    stateIndex = 4 + obsIndex;
+
+    % Calculate the velocity measurement innovation
+    innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
+    
+    % Calculate the observation Jacobian
+    H(obsIndex,stateIndex) = 1;
+    
+    varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
+    
+end
+
+% Apply an innovation consistency check
+for obsIndex = 1:3
+    
+    if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
+        return;
+    end
+    
+end
+
+% Calculate Kalman gains and update states and covariances
+for obsIndex = 1:3
+    
+    % Calculate the Kalman gains
+    K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
+    
+    % Calculate state corrections
+    xk = K * innovation(obsIndex);
+    
+    % Apply the state corrections
+    states = states - xk;
+    
+    % Update the covariance
+    P = P - K*H(obsIndex,:)*P;
+    
+    % Force symmetry on the covariance matrix to prevent ill-conditioning
+    P = 0.5*(P + transpose(P));
+    
+    % ensure diagonals are positive
+    for i=1:24
+        if P(i,i) < 0
+            P(i,i) = 0;
+        end
+    end
+    
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m

@@ -0,0 +1,211 @@
+%% define symbolic variables and constants
+clear all;
+reset(symengine);
+syms dax day daz real % IMU delta angle measurements in body axes - rad
+syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
+syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
+syms vn ve vd real % NED velocity - m/sec
+syms pn pe pd real % NED position - m
+syms dax_b day_b daz_b real % delta angle bias - rad
+syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
+syms dt real % IMU time step - sec
+syms gravity real % gravity  - m/sec^2
+syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
+syms vwn vwe real; % NE wind velocity - m/sec
+syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss
+syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
+syms R_MAG real  % variance for magnetic flux measurements - milligauss^2
+
+%% define the state prediction equations
+
+% define the measured Delta angle and delta velocity vectors
+dAngMeas = [dax; day; daz];
+dVelMeas = [dvx; dvy; dvz];
+
+% define the IMU bias errors and scale factor
+dAngBias = [dax_b; day_b; daz_b];
+dVelBias = [dvx_b; dvy_b; dvz_b];
+
+% define the quaternion rotation vector for the state estimate
+quat = [q0;q1;q2;q3];
+% derive the truth body to nav direction cosine matrix
+Tbn = Quat2Tbn(quat);
+
+% define the truth delta angle
+% ignore coning compensation as these effects are negligible in terms of 
+% covariance growth for our application and grade of sensor
+dAngTruth = dAngMeas - dAngBias;
+
+% Define the truth delta velocity -ignore sculling and transport rate
+% corrections as these negligible are in terms of covariance growth for our
+% application and grade of sensor
+dVelTruth = dVelMeas - dVelBias;
+
+% define the attitude update equations
+% use a first order expansion of rotation to calculate the quaternion increment
+% acceptable for propagation of covariances
+deltaQuat = [1;
+    0.5*dAngTruth(1);
+    0.5*dAngTruth(2);
+    0.5*dAngTruth(3);
+    ];
+quatNew = QuatMult(quat,deltaQuat);
+
+% define the velocity update equations
+% ignore coriolis terms for linearisation purposes
+vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
+
+% define the position update equations
+pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
+
+% define the IMU error update equations
+dAngBiasNew = dAngBias;
+dVelBiasNew = dVelBias;
+
+% define the wind velocity update equations
+vwnNew = vwn;
+vweNew = vwe;
+
+% define the earth magnetic field update equations
+magNnew = magN;
+magEnew = magE;
+magDnew = magD;
+
+% define the body magnetic field update equations
+magXnew = magX;
+magYnew = magY;
+magZnew = magZ;
+
+% Define the state vector & number of states
+stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
+nStates=numel(stateVector);
+
+% Define vector of process equations
+stateVectorNew = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
+
+%% derive the state transition and state error matrix
+
+% Define the control (disturbance) vector. Error growth in the inertial
+% solution is assumed to be driven by 'noise' in the delta angles and
+% velocities, after bias effects have been removed. This is OK because we
+% have sensor bias accounted for in the state equations.
+distVector = [daxVar;dayVar;dazVar;dvxVar;dvyVar;dvzVar];
+
+% derive the control(disturbance) influence matrix
+G = jacobian(stateVectorNew, [dAngMeas;dVelMeas]);
+
+% derive the state error matrix
+distMatrix = diag(distVector);
+Q = G*distMatrix*transpose(G);
+f = matlabFunction(Q,'file','calcQ24.m');
+
+% derive the state transition matrix
+F = jacobian(stateVectorNew, stateVector);
+f = matlabFunction(F,'file','calcF24.m');
+
+%% derive equations for fusion of magnetometer measurements
+% rotate earth field into body axes
+magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ];
+
+magMeasX = magMeas(1);
+H_MAGX = jacobian(magMeasX,stateVector); % measurement Jacobian
+f = matlabFunction(H_MAGX,'file','calcH_MAGX.m');
+
+magMeasY = magMeas(2);
+H_MAGY = jacobian(magMeasY,stateVector); % measurement Jacobian
+f = matlabFunction(H_MAGY,'file','calcH_MAGY.m');
+
+magMeasZ = magMeas(3);
+H_MAGZ = jacobian(magMeasZ,stateVector); % measurement Jacobian
+f = matlabFunction(H_MAGZ,'file','calcH_MAGZ.m');
+
+%% derive equations for fusion of synthetic deviation measurement
+% used to keep correct heading when operating without absolute position or
+% velocity measurements - eg when using optical flow
+
+% rotate magnetic field into earth axes
+magMeasNED = [magN;magE;magD];
+
+% the predicted measurement is the angle wrt magnetic north of the horizontal
+% component of the measured field
+angMeas = atan(magMeasNED(2)/magMeasNED(1));
+H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
+H_MAGD = simplify(H_MAGD);
+
+f = matlabFunction(H_MAGD,'file','calcH_MAGD.m');
+
+%% derive equations for fusion of a single magneic compass  heading measurement
+
+% rotate body measured field into earth axes
+magMeasNED = Tbn*[magX;magY;magZ]; 
+
+% the predicted measurement is the angle wrt true north of the horizontal
+% component of the measured field
+angMeas = atan(magMeasNED(2)/magMeasNED(1));
+H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian
+f = matlabFunction(H_MAG,'file','calcH_HDG.m');
+
+%% derive equations for sequential fusion of optical flow measurements
+
+% range is defined as distance from camera focal point to centre of sensor fov
+syms range real;
+
+% calculate relative velocity in body frame
+relVelBody = transpose(Tbn)*[vn;ve;vd];
+
+% divide by range to get predicted angular LOS rates relative to X and Y
+% axes. Note these are body angular rate motion compensated optical flow rates
+losRateX = +relVelBody(2)/range;
+losRateY = -relVelBody(1)/range;
+
+% calculate the observation Jacobian for the X axis
+H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian
+H_LOSX = simplify(H_LOSX);
+f = matlabFunction(H_LOSX,'file','calcH_LOSX.m');
+
+% calculate the observation Jacobian for the Y axis
+H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian
+H_LOSY = simplify(H_LOSY);
+f = matlabFunction(H_LOSY,'file','calcH_LOSY.m');
+
+%% derive equations for sequential fusion of body frame velocity measurements
+
+% body frame velocity observations
+syms velX velY velZ real;
+
+% velocity observation variance
+syms R_VEL real;
+
+% calculate relative velocity in body frame
+relVelBody = transpose(Tbn)*[vn;ve;vd];
+
+% calculate the observation Jacobian for the X axis
+H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
+H_VELX = simplify(H_VELX);
+f = matlabFunction(H_VELX,'file','calcH_VELX.m');
+
+% calculate the observation Jacobian for the Y axis
+H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
+H_VELY = simplify(H_VELY);
+f = matlabFunction(H_VELY,'file','calcH_VELY.m');
+
+% calculate the observation Jacobian for the Z axis
+H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
+H_VELZ = simplify(H_VELZ);
+f = matlabFunction(H_VELZ,'file','calcH_VELZ.m');
+
+%% calculate error transfer matrix for declination error estimate
+declination = atan(magE/magN);
+T_MAG = jacobian(declination,[magN,magE]);
+
+f = matlabFunction(T_MAG,'file','transfer_matrix.m');
+
+%% calculate Quaternion to Euler angle error transfer matrix
+% quat = [q0;q1;q2;q3];
+% syms roll pitch yaw 'real';
+roll = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)) , (quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4)));
+pitch = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
+yaw = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)) , (quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4)));
+euler = [roll;pitch;yaw];
+error_transfer_matrix = jacobian(euler,quat);
+matlabFunction(error_transfer_matrix,'file','quat_to_euler_error_transfer_matrix.m');

src/lib/ecl/EKF/matlab/EKF_replay/Filter/InitCovariance.m

@@ -0,0 +1,36 @@
+function covariance = InitCovariance(param,dt,gps_alignment,gps_data)
+
+% Define quaternion state errors
+Sigma_quat = param.alignment.quatErr * [1;1;1;1];
+
+% Define velocity state errors
+if (gps_alignment == 1)
+    Sigma_velocity = gps_data.spd_error(gps_data.start_index) * [1;1;1];
+else
+    Sigma_velocity = [param.alignment.velErrNE;param.alignment.velErrNE;param.alignment.velErrD];
+end
+
+% Define position state errors
+if (gps_alignment == 1)
+    Sigma_position = gps_data.pos_error(gps_data.start_index) * [1;1;0] + [0;0;param.alignment.hgtErr];
+else
+    Sigma_position = [param.alignment.posErrNE;param.alignment.posErrNE;param.alignment.hgtErr];
+end
+
+% Define delta angle bias state errors
+Sigma_dAngBias = param.alignment.delAngBiasErr*dt*[1;1;1];
+
+% Define delta velocity bias state errors
+Sigma_dVelBias = param.alignment.delVelBiasErr*dt*[1;1;1];
+
+% Define magnetic field state errors
+Sigma_magNED = [param.alignment.magErrNED;param.alignment.magErrNED;param.alignment.magErrNED]; % 1 Sigma uncertainty in initial NED mag field
+Sigma_magXYZ = [param.alignment.magErrXYZ;param.alignment.magErrXYZ;param.alignment.magErrXYZ]; % 1 Sigma uncertainty in initial XYZ mag sensor offset
+
+% Define wind velocity state errors
+Sigma_wind = param.alignment.windErrNE * [1;1];
+
+% Convert to variances and write to covariance matrix diagonals
+covariance = diag([Sigma_quat;Sigma_velocity;Sigma_position;Sigma_dAngBias;Sigma_dVelBias;Sigma_magNED;Sigma_magXYZ;Sigma_wind].^2);
+end
+

src/lib/ecl/EKF/matlab/EKF_replay/Filter/InitStates.m

@@ -0,0 +1,66 @@
+function [states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data)
+
+% constants
+deg2rad = pi/180;
+
+% initialise the state vector and quaternion
+states = zeros(24,1);
+quat = [1;0;0;0];
+
+if (param.control.waitForGps == 1)
+    % find IMU start index that coresponds to first valid GPS data
+    imu_start_index = (find(imu_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 50);
+    imu_start_index = max(imu_start_index,1);
+else
+    imu_start_index = 1;
+end
+
+% average first 100 accel readings to reduce effect of vibration
+initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
+initAccel(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
+initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
+
+% align tilt using gravity vector (If the velocity is changing this will
+% induce errors)
+quat = AlignTilt(quat,initAccel);
+states(1:4) = quat;
+
+% add a roll, pitch, yaw mislignment
+quat_align_err = EulToQuat([param.control.rollAlignErr,param.control.pitchAlignErr,param.control.yawAlignErr]);
+quat = QuatMult(quat,quat_align_err);
+
+% find magnetometer start index
+mag_start_index = (find(mag_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 5);
+mag_start_index = max(mag_start_index,1);
+
+% mean to reduce effect of noise in data
+magBody(1,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,1));
+magBody(2,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,2));
+magBody(3,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,3));
+
+% align heading and initialise the NED magnetic field states
+quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad);
+states(1:4) = quat;
+
+% initialise the NED magnetic field states
+Tbn = Quat2Tbn(quat);
+states(17:19) = Tbn*magBody;
+
+if (param.control.waitForGps == 1)
+    % initialise velocity and position using gps
+    states(5:7) = gps_data.vel_ned(gps_data.start_index,:);
+    states(8:9) = gps_data.pos_ned(gps_data.start_index,1:2);
+else
+    % initialise to be stationary at the origin
+    states(5:7) = zeros(1,3);
+    states(8:9) = zeros(1,2);
+end
+
+% find baro start index
+baro_start_index = (find(baro_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 10);
+baro_start_index = max(baro_start_index,1);
+
+% average baro data and initialise the vertical position
+states(10) = -mean(baro_data.height(baro_start_index:baro_start_index+20));
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/PlotData.m

@@ -0,0 +1,511 @@
+function PlotData(output,folder,runIdentifier)
+rad2deg = 180/pi;
+if ~exist(folder,'dir')
+    mkdir(folder);
+end
+plotDimensions = [0 0 210*3 297*3];
+
+%% plot Euler angle estimates
+figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+h=gcf;
+set(h,'PaperOrientation','portrait');
+set(h,'PaperUnits','normalized');
+set(h,'PaperPosition', [0 0 1 1]);
+
+margin = 5;
+
+subplot(3,1,1);
+plot(output.time_lapsed,[output.euler_angles(:,1)*rad2deg,output.euler_angles(:,1)*rad2deg-2*sqrt(output.euler_variances(:,1)*rad2deg),output.euler_angles(:,1)*rad2deg+2*sqrt(output.euler_variances(:,1)*rad2deg)]);
+minVal = rad2deg*min(output.euler_angles(:,1))-margin;
+maxVal = rad2deg*max(output.euler_angles(:,1))+margin;
+ylim([minVal maxVal]);
+grid on;
+titleText=strcat({'Euler Angle Estimates'},runIdentifier);
+title(titleText);
+ylabel('Roll (deg)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,2);
+plot(output.time_lapsed,[output.euler_angles(:,2)*rad2deg,output.euler_angles(:,2)*rad2deg-2*sqrt(output.euler_variances(:,2)*rad2deg),output.euler_angles(:,2)*rad2deg+2*sqrt(output.euler_variances(:,2)*rad2deg)]);
+minVal = rad2deg*min(output.euler_angles(:,2))-margin;
+maxVal = rad2deg*max(output.euler_angles(:,2))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('Pitch (deg)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,3);
+plot(output.time_lapsed,[output.euler_angles(:,3)*rad2deg,output.euler_angles(:,3)*rad2deg-2*sqrt(output.euler_variances(:,3)*rad2deg),output.euler_angles(:,3)*rad2deg+2*sqrt(output.euler_variances(:,3)*rad2deg)]);
+minVal = rad2deg*min(output.euler_angles(:,3))-margin;
+maxVal = rad2deg*max(output.euler_angles(:,3))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('Yaw (deg)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+fileName='euler_angle_estimates.png';
+fullFileName = fullfile(folder, fileName);
+saveas(h,fullFileName);
+
+%% plot NED velocity estimates
+figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+h=gcf;
+set(h,'PaperOrientation','portrait');
+set(h,'PaperUnits','normalized');
+set(h,'PaperPosition', [0 0 1 1]);
+
+subplot(3,1,1);
+plot(output.time_lapsed,[output.velocity_NED(:,1),output.velocity_NED(:,1)+2*sqrt(output.state_variances(:,5)),output.velocity_NED(:,1)-2*sqrt(output.state_variances(:,5))]);
+grid on;
+titleText=strcat({'NED Velocity Estimates'},runIdentifier);
+title(titleText);
+ylabel('North (m/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,2);
+plot(output.time_lapsed,[output.velocity_NED(:,2),output.velocity_NED(:,2)+2*sqrt(output.state_variances(:,6)),output.velocity_NED(:,2)-2*sqrt(output.state_variances(:,6))]);
+grid on;
+ylabel('East (m/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,3);
+plot(output.time_lapsed,[output.velocity_NED(:,3),output.velocity_NED(:,3)+2*sqrt(output.state_variances(:,7)),output.velocity_NED(:,3)-2*sqrt(output.state_variances(:,7))]);
+grid on;
+ylabel('Down (m/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+fileName='velocity_estimates.png';
+fullFileName = fullfile(folder, fileName);
+saveas(h,fullFileName);
+
+%% plot NED position estimates
+figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+h=gcf;
+set(h,'PaperOrientation','portrait');
+set(h,'PaperUnits','normalized');
+set(h,'PaperPosition', [0 0 1 1]);
+
+subplot(3,1,1);
+plot(output.time_lapsed,[output.position_NED(:,1),output.position_NED(:,1)+2*sqrt(output.state_variances(:,8)),output.position_NED(:,1)-2*sqrt(output.state_variances(:,8))]);
+grid on;
+titleText=strcat({'NED Position Estimates'},runIdentifier);
+title(titleText);
+ylabel('North (m)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,2);
+plot(output.time_lapsed,[output.position_NED(:,2),output.position_NED(:,2)+2*sqrt(output.state_variances(:,9)),output.position_NED(:,2)-2*sqrt(output.state_variances(:,9))]);
+grid on;
+ylabel('East (m)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,3);
+plot(output.time_lapsed,[output.position_NED(:,3),output.position_NED(:,3)+2*sqrt(output.state_variances(:,10)),output.position_NED(:,3)-2*sqrt(output.state_variances(:,10))]);
+grid on;
+ylabel('Down (m)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+fileName='position_estimates.png';
+fullFileName = fullfile(folder, fileName);
+saveas(h,fullFileName);
+
+%% plot IMU gyro bias estimates
+figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+h=gcf;
+set(h,'PaperOrientation','portrait');
+set(h,'PaperUnits','normalized');
+set(h,'PaperPosition', [0 0 1 1]);
+
+margin = 0.1;
+
+subplot(3,1,1);
+plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,1),output.gyro_bias(:,1)+2*sqrt(output.state_variances(:,11)),output.gyro_bias(:,1)-2*sqrt(output.state_variances(:,11))]*rad2deg);%%output.gyro_bias(:,1)*rad2deg);
+minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,1))-margin;
+maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,1))+margin;
+ylim([minVal maxVal]);
+grid on;
+titleText=strcat({'IMU Gyro Bias Estimates'},runIdentifier);
+title(titleText);
+ylabel('X gyro (deg/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,2);
+plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,2),output.gyro_bias(:,2)+2*sqrt(output.state_variances(:,12)),output.gyro_bias(:,2)-2*sqrt(output.state_variances(:,12))]*rad2deg);
+minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,2))-margin;
+maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,2))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('Y gyro (deg/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,3);
+plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,3),output.gyro_bias(:,3)+2*sqrt(output.state_variances(:,13)),output.gyro_bias(:,3)-2*sqrt(output.state_variances(:,13))]*rad2deg);
+minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,3))-margin;
+maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,3))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('Z gyro (deg/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+fileName='imu_gyro_bias_estimates.png';
+fullFileName = fullfile(folder, fileName);
+saveas(h,fullFileName);
+
+%% plot IMU accel bias estimates
+figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+h=gcf;
+set(h,'PaperOrientation','portrait');
+set(h,'PaperUnits','normalized');
+set(h,'PaperPosition', [0 0 1 1]);
+
+margin = 0.1;
+
+subplot(3,1,1);
+plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,1),output.accel_bias(:,1)+2*sqrt(output.state_variances(:,14)),output.accel_bias(:,1)-2*sqrt(output.state_variances(:,14))]);
+titleText=strcat({'IMU Accel Bias Estimates'},runIdentifier);
+title(titleText);
+minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin;
+maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('X accel (m/s/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,2);
+plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,2),output.accel_bias(:,2)+2*sqrt(output.state_variances(:,15)),output.accel_bias(:,2)-2*sqrt(output.state_variances(:,15))]);
+minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin;
+maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('Y accel (m/s/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+subplot(3,1,3);
+plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,3),output.accel_bias(:,3)+2*sqrt(output.state_variances(:,16)),output.accel_bias(:,3)-2*sqrt(output.state_variances(:,16))]);
+minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin;
+maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin;
+ylim([minVal maxVal]);
+grid on;
+ylabel('Z accel (m/s/s)');
+xlabel('time (sec)');
+legend('estimate','upper 95% bound','lower 95% bound');
+
+fileName='imu_accel_bias_estimates.png';
+fullFileName = fullfile(folder, fileName);
+saveas(h,fullFileName);
+
+%% plot magnetometer bias estimates
+if (output.magFuseMethod <= 1)
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    
+    subplot(3,1,1);
+    plot(output.time_lapsed',[output.mag_XYZ(:,1),output.mag_XYZ(:,1)+2*sqrt(output.state_variances(:,20)),output.mag_XYZ(:,1)-2*sqrt(output.state_variances(:,20))]);
+    grid on;
+    titleText=strcat({'Magnetometer Bias Estimates'},runIdentifier);
+    title(titleText);
+    ylabel('X bias (gauss)');
+    xlabel('time (sec)');
+    legend('estimate','upper 95% bound','lower 95% bound');
+    
+    subplot(3,1,2);
+    plot(output.time_lapsed',[output.mag_XYZ(:,2),output.mag_XYZ(:,2)+2*sqrt(output.state_variances(:,21)),output.mag_XYZ(:,2)-2*sqrt(output.state_variances(:,21))]);
+    grid on;
+    ylabel('Y bias (gauss)');
+    xlabel('time (sec)');
+    legend('estimate','upper 95% bound','lower 95% bound');
+    
+    subplot(3,1,3);
+    plot(output.time_lapsed',[output.mag_XYZ(:,3),output.mag_XYZ(:,3)+2*sqrt(output.state_variances(:,22)),output.mag_XYZ(:,3)-2*sqrt(output.state_variances(:,22))]);
+    grid on;
+    ylabel('Z bias (gauss)');
+    xlabel('time (sec)');
+    legend('estimate','upper 95% bound','lower 95% bound');
+    
+    fileName='body_field_estimates.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+end
+
+%% plot earth field estimates
+if (output.magFuseMethod <= 1)
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    
+    margin = 0.1;
+    
+    subplot(4,1,1);
+    plot(output.time_lapsed',[output.mag_NED(:,1),output.mag_NED(:,1)+2*sqrt(output.state_variances(:,17)),output.mag_NED(:,1)-2*sqrt(output.state_variances(:,17))]);
+    minVal = min(output.mag_NED(:,1))-margin;
+    maxVal = max(output.mag_NED(:,1))+margin;
+    ylim([minVal maxVal]);
+    grid on;
+    titleText=strcat({'Earth Magnetic Field Estimates'},runIdentifier);
+    title(titleText);
+    ylabel('North (gauss)');
+    xlabel('time (sec)');
+    legend('estimate','upper 95% bound','lower 95% bound');
+    
+    subplot(4,1,2);
+    plot(output.time_lapsed',[output.mag_NED(:,2),output.mag_NED(:,2)+2*sqrt(output.state_variances(:,18)),output.mag_NED(:,2)-2*sqrt(output.state_variances(:,18))]);
+    minVal = min(output.mag_NED(:,2))-margin;
+    maxVal = max(output.mag_NED(:,2))+margin;
+    ylim([minVal maxVal]);
+    grid on;
+    ylabel('East (gauss)');
+    xlabel('time (sec)');
+    legend('estimate','upper 95% bound','lower 95% bound');
+    
+    subplot(4,1,3);
+    plot(output.time_lapsed',[output.mag_NED(:,3),output.mag_NED(:,3)+2*sqrt(output.state_variances(:,19)),output.mag_NED(:,3)-2*sqrt(output.state_variances(:,19))]);
+    grid on;
+    minVal = min(output.mag_NED(:,3))-margin;
+    maxVal = max(output.mag_NED(:,3))+margin;
+    ylim([minVal maxVal]);
+    ylabel('Down (gauss)');
+    xlabel('time (sec)');
+    legend('estimate','upper 95% bound','lower 95% bound');
+    
+    subplot(4,1,4);
+    plot(output.time_lapsed',rad2deg*atan2(output.mag_NED(:,2),output.mag_NED(:,1)));
+    grid on;
+    titleText=strcat({'Magnetic Declination Estimate'},runIdentifier);
+    title(titleText);
+    ylabel('declination (deg)');
+    xlabel('time (sec)');
+    
+    fileName='earth_field_estimates.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+end
+
+%% plot velocity innovations
+if isfield(output.innovations,'vel_innov')
+    
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    
+    subplot(3,1,1);
+    plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]);
+    grid on;
+    titleText=strcat({'Velocity Innovations and Variances'},runIdentifier);
+    title(titleText);
+    ylabel('North (m/s)');
+    xlabel('time (sec)');
+    legend('innovation','variance sqrt','variance sqrt');
+    
+    subplot(3,1,2);
+    plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]);
+    grid on;
+    ylabel('East (m/s)');
+    xlabel('time (sec)');
+    legend('innovation','variance sqrt','variance sqrt');
+    
+    subplot(3,1,3);
+    plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]);
+    grid on;
+    ylabel('Down (m/s)');
+    xlabel('time (sec)');
+    legend('innovation','variance sqrt','variance sqrt');
+    
+    fileName='velocity_fusion.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+end
+
+%% plot position innovations
+if isfield(output.innovations,'posInnov')
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    
+    subplot(3,1,1);
+    plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]);
+    grid on;
+    titleText=strcat({'Position Innovations and Variances'},runIdentifier);
+    title(titleText);
+    ylabel('North (m)');
+    xlabel('time (sec)');
+    legend('innovation','variance sqrt','variance sqrt');
+    
+    subplot(3,1,2);
+    plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]);
+    grid on;
+    ylabel('East (m)');
+    xlabel('time (sec)');
+    legend('innovation','variance sqrt','variance sqrt');
+    
+    subplot(3,1,3);
+    plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]);
+    grid on;
+    ylabel('Up (m)');
+    xlabel('time (sec)');
+    legend('innovation','variance sqrt','variance sqrt');
+    
+    fileName='position_fusion.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+end
+
+%% plot magnetometer innovations
+if isfield(output.innovations,'magInnov')
+    
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    subplot(4,1,1);
+    plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,1)';sqrt(output.innovations.magInnovVar(:,1))';-sqrt(output.innovations.magInnovVar(:,1))']);
+    ylim([-0.15 0.15]);
+    grid on;
+    title(strcat({'Magnetometer Innovations and Variances'},runIdentifier));
+    ylabel('X (gauss)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    subplot(4,1,2);
+    plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,2)';sqrt(output.innovations.magInnovVar(:,2))';-sqrt(output.innovations.magInnovVar(:,2))']);
+    ylim([-0.15 0.15]);
+    grid on;
+    ylabel('Y (gauss)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    subplot(4,1,3);
+    plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,3)';sqrt(output.innovations.magInnovVar(:,3))';-sqrt(output.innovations.magInnovVar(:,3))']);
+    ylim([-0.15 0.15]);
+    grid on;
+    ylabel('Z (gauss)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    subplot(4,1,4);
+    plot(output.innovations.mag_time_lapsed,output.innovations.magLength);
+    ylim([0 0.7]);
+    grid on;
+    title(strcat({'Magnetic Flux'},runIdentifier));
+    ylabel('Flux (Gauss)');
+    xlabel('time (sec)');
+    fileName='magnetometer_fusion.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+    
+end
+
+%% plot magnetic yaw innovations
+if isfield(output.innovations,'hdgInnov')
+    
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    
+    subplot(2,1,1);
+    plot(output.innovations.mag_time_lapsed,[output.innovations.hdgInnov*rad2deg;sqrt(output.innovations.hdgInnovVar)*rad2deg;-sqrt(output.innovations.hdgInnovVar)*rad2deg]);
+    ylim([-30 30]);
+    grid on;
+    title(strcat({'Magnetic Heading Innovations and Variances'},runIdentifier));
+    ylabel('yaw innovation (deg)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    subplot(2,1,2);
+    plot(output.innovations.mag_time_lapsed,output.innovations.magLength);
+    ylim([0 0.7]);
+    grid on;
+    title(strcat({'Magnetic Flux'},runIdentifier));
+    ylabel('Flux (Gauss)');
+    xlabel('time (sec)');
+    fileName='magnetometer_fusion.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+    
+end
+
+%% plot optical flow innovations
+if isfield(output.innovations,'flowInnov')
+    
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    subplot(2,1,1);
+    plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,1)';sqrt(output.innovations.flowInnovVar(:,1))';-sqrt(output.innovations.flowInnovVar(:,1))']);
+    ylim([-1.0 1.0]);
+    grid on;
+    title(strcat({'Optical Flow Innovations and Variances'},runIdentifier));
+    ylabel('X (rad/sec)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    subplot(2,1,2);
+    plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,2)';sqrt(output.innovations.flowInnovVar(:,2))';-sqrt(output.innovations.flowInnovVar(:,2))']);
+    ylim([-1.0 1.0]);
+    grid on;
+    ylabel('Y (rad/sec)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    fileName='optical_flow_fusion.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+    
+end
+%% plot ZED camera innovations
+if isfield(output.innovations,'bodyVelInnov')
+    
+    figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
+    h=gcf;
+    set(h,'PaperOrientation','portrait');
+    set(h,'PaperUnits','normalized');
+    set(h,'PaperPosition', [0 0 1 1]);
+    
+    subplot(3,1,1);
+    plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,1)';sqrt(output.innovations.bodyVelInnovVar(:,1))';-sqrt(output.innovations.bodyVelInnovVar(:,1))']);
+    grid on;
+    title(strcat({'ZED Camera Innovations and Variances'},runIdentifier));
+    ylabel('X (m/sec)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    
+    subplot(3,1,2);
+    plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,2)';sqrt(output.innovations.bodyVelInnovVar(:,2))';-sqrt(output.innovations.bodyVelInnovVar(:,2))']);
+    grid on;
+    ylabel('Y (m/sec)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    
+    subplot(3,1,3);
+    plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,3)';sqrt(output.innovations.bodyVelInnovVar(:,3))';-sqrt(output.innovations.bodyVelInnovVar(:,3))']);
+    grid on;
+    ylabel('Z (m/sec)');
+    xlabel('time (sec)');
+    legend('innovation','innovation variance sqrt','innovation variance sqrt');
+    
+    fileName='zed_camera_fusion.png';
+    fullFileName = fullfile(folder, fileName);
+    saveas(h,fullFileName);
+    
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/PredictCovariance.m

@@ -0,0 +1,66 @@
+function P  = PredictCovariance(deltaAngle, ...
+    deltaVelocity, ...
+    states,...
+    P, ...  % Previous state covariance matrix
+    dt, ... % IMU and prediction time step
+    param) % tuning parameters
+    
+% Set filter state process noise other than IMU errors, which are already
+% built into the derived covariance prediction equations.
+% This process noise determines the rate of estimation of IMU bias errors
+dAngBiasSigma = dt*dt*param.prediction.dAngBiasPnoise;
+dVelBiasSigma = dt*dt*param.prediction.dVelBiasPnoise;
+magSigmaNED = dt*param.prediction.magPnoiseNED;
+magSigmaXYZ = dt*param.prediction.magPnoiseXYZ;
+processNoiseVariance = [zeros(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1], magSigmaNED*[1 1 1], magSigmaXYZ*[1 1 1], [0 0]].^2;
+
+% Specify the noise variance on the IMU delta angles and delta velocities
+daxVar = (dt*param.prediction.angRateNoise)^2;
+dayVar = daxVar;
+dazVar = daxVar;
+dvxVar = (dt*param.prediction.accelNoise)^2;
+dvyVar = dvxVar;
+dvzVar = dvxVar;
+
+dvx = deltaVelocity(1);
+dvy = deltaVelocity(2);
+dvz = deltaVelocity(3);
+dax = deltaAngle(1);
+day = deltaAngle(2);
+daz = deltaAngle(3);
+
+q0 = states(1);
+q1 = states(2);
+q2 = states(3);
+q3 = states(4);
+
+dax_b = states(11);
+day_b = states(12);
+daz_b = states(13);
+
+dvx_b = states(14);
+dvy_b = states(15);
+dvz_b = states(16);
+
+% Predicted covariance
+F = calcF24(dax,dax_b,day,day_b,daz,daz_b,dt,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3);
+Q = calcQ24(daxVar,dayVar,dazVar,dvxVar,dvyVar,dvzVar,q0,q1,q2,q3);
+P = F*P*transpose(F) + Q;
+
+% Add the general process noise variance
+for i = 1:24
+    P(i,i) = P(i,i) + processNoiseVariance(i);
+end
+
+% Force symmetry on the covariance matrix to prevent ill-conditioning
+% of the matrix which would cause the filter to blow-up
+P = 0.5*(P + transpose(P));
+
+% ensure diagonals are positive
+for i=1:24
+    if P(i,i) < 0
+        P(i,i) = 0;
+    end
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/PredictStates.m

@@ -0,0 +1,85 @@
+function [states, correctedDelAng, correctedDelVel]  = PredictStates( ...
+    states, ... % previous state vector (4x1 quaternion, 3x1 velocity, 3x1 position, 3x1 delAng bias, 3x1 delVel bias)
+    delAng, ... % IMU delta angle measurements, 3x1 (rad)
+    delVel, ... % IMU delta velocity measurements 3x1 (m/s)
+    dt, ... % accumulation time of the IMU measurement (sec)
+    gravity, ... % acceleration due to gravity (m/s/s)
+    latitude) % WGS-84 latitude (rad)
+
+% define persistent variables for previous delta angle and velocity which
+% are required for sculling and coning error corrections
+persistent prevDelAng;
+if isempty(prevDelAng)
+    prevDelAng = delAng;
+end
+
+persistent prevDelVel;
+if isempty(prevDelVel)
+    prevDelVel = delVel;
+end
+
+persistent Tbn_prev;
+if isempty(Tbn_prev)
+    Tbn_prev = Quat2Tbn(states(1:4));
+end
+
+% Remove sensor bias errors
+delAng = delAng - states(11:13);
+delVel = delVel - states(14:16);
+
+% Correct delta velocity for rotation and skulling
+% Derived from Eqn 25 of:
+% "Computational Elements For Strapdown Systems"
+% Savage, P.G.
+% Strapdown Associates
+% 2015, WBN-14010
+% correctedDelVel= delVel + ...
+%     0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) +  1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng));
+correctedDelVel= delVel;
+
+% Calculate earth delta angle spin vector
+delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt;
+delAngEarth_NED(2,1) = 0.0;
+delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt;
+
+% Apply corrections for coning errors and earth spin rate
+% Coning correction from :
+% "A new strapdown attitude algorithm", 
+% R. B. MILLER, 
+% Journal of Guidance, Control, and Dynamics
+% July, Vol. 6, No. 4, pp. 287-291, Eqn 11 
+% correctedDelAng   = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
+correctedDelAng   = delAng - transpose(Tbn_prev)*delAngEarth_NED;
+
+% Save current measurements
+prevDelAng = delAng;
+prevDelVel = delVel;
+
+% Convert the rotation vector to its equivalent quaternion
+deltaQuat = RotToQuat(correctedDelAng);
+
+% Update the quaternions by rotating from the previous attitude through
+% the delta angle rotation quaternion
+states(1:4) = QuatMult(states(1:4),deltaQuat);
+
+% Normalise the quaternions
+states(1:4) = NormQuat(states(1:4));
+
+% Calculate the body to nav cosine matrix
+Tbn = Quat2Tbn(states(1:4));
+Tbn_prev = Tbn;
+
+% transform body delta velocities to delta velocities in the nav frame
+delVelNav = Tbn * correctedDelVel + [0;0;gravity]*dt;
+
+% take a copy of the previous velocity
+prevVel = states(5:7);
+
+% Sum delta velocities to get the velocity
+states(5:7) = states(5:7) + delVelNav(1:3);
+
+% integrate the velocity vrctor to get the position using trapezoidal
+% integration
+states(8:10) = states(8:10) + 0.5 * dt * (prevVel + states(5:7));
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/RunFilter.m

@@ -0,0 +1,347 @@
+function output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,varargin)
+
+% compulsory inputs
+
+% param : parameters defined  by SetParameterDefaults.m
+% imu_data : IMU delta angle and velocity data in body frame
+% mag_data : corrected magnetometer field measurements in body frame
+% baro_data : barometric height measurements
+% gps_data : GPS NED pos vel measurements in local earth frame
+
+% optional inputs
+
+% rng _data : measurements for a Z body axis aligned range finder
+% flow_data : XY axis optical flow angular rate measurements in body frame
+% viso_data : ZED camera visula odometry measurements
+
+nVarargs = length(varargin);
+if nVarargs >= 2
+    flowDataPresent = ~isempty(varargin{1}) && ~isempty(varargin{2});
+    rng_data = varargin{1};
+    flow_data = varargin{2};
+    if flowDataPresent
+        fprintf('Using optical Flow Data\n',nVarargs);
+    end
+else
+    flowDataPresent = 0;
+end
+
+if nVarargs >= 3
+    visoDataPresent = ~isempty(varargin{3});
+    viso_data = varargin{3};
+    if visoDataPresent
+        fprintf('Using ZED camera odometry data\n',nVarargs);
+    end
+else
+    visoDataPresent = 0;
+end
+
+
+%% Set initial conditions
+
+% constants
+deg2rad = pi/180;
+gravity = 9.80665; % initial value of gravity - will be updated when WGS-84 position is known
+
+% initialise the state vector
+[states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data);
+
+dt_imu_avg = 0.5 * (median(imu_data.gyro_dt) + median(imu_data.accel_dt));
+indexStop = length(imu_data.time_us) - imu_start_index;
+indexStart = 1;
+
+% create static structs for output data
+output = struct('time_lapsed',[]',...
+    'euler_angles',[],...
+    'velocity_NED',[],...
+    'position_NED',[],...
+    'gyro_bias',[],...
+    'accel_bias',[],...
+    'mag_NED',[],...
+    'mag_XYZ',[],...
+    'wind_NE',[],...
+    'dt',0,...
+    'state_variances',[],...
+    'innovations',[],...
+    'magFuseMethod',[]);
+
+% initialise the state covariance matrix
+covariance = InitCovariance(param,dt_imu_avg,1,gps_data);
+
+%% Main Loop
+
+% control flags
+gps_use_started = boolean(false);
+gps_use_blocked = boolean(false);
+viso_use_blocked = boolean(false);
+flow_use_blocked = boolean(false);
+
+% array access index variables
+imuIndex = imu_start_index;
+last_gps_index = 0;
+gps_fuse_index = 0;
+last_baro_index = 0;
+baro_fuse_index = 0;
+last_mag_index = 0;
+mag_fuse_index = 0;
+last_flow_index = 0;
+flow_fuse_index = 0;
+last_viso_index = 0;
+viso_fuse_index = 0;
+last_range_index = 0;
+
+% covariance prediction variables
+delAngCov = [0;0;0]; % delta angle vector used by the covariance prediction (rad)
+delVelCov = [0;0;0]; % delta velocity vector used by the covariance prediction (m/sec)
+dtCov = 0; % time step used by the covariance prediction (sec)
+dtCovInt = 0; % accumulated time step of covariance predictions (sec)
+covIndex = 0; % covariance prediction frame counter
+
+output.magFuseMethod = param.fusion.magFuseMethod;
+range = 0.1;
+
+% variables used to control dead-reckoning timeout
+last_drift_constrain_time = - param.control.velDriftTimeLim;
+last_synthetic_velocity_fusion_time = 0;
+last_valid_range_time = - param.fusion.rngTimeout;
+
+for index = indexStart:indexStop
+    
+    % read IMU measurements
+    local_time=imu_data.time_us(imuIndex)*1e-6;
+    delta_angle(:,1) = imu_data.del_ang(imuIndex,:);
+    delta_velocity(:,1) = imu_data.del_vel(imuIndex,:);
+    dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex));
+    imuIndex = imuIndex+1;
+    
+    % predict states
+    [states, delAngCorrected, delVelCorrected]  = PredictStates(states,delta_angle,delta_velocity,imu_data.accel_dt(imuIndex),gravity,gps_data.refLLH(1,1)*deg2rad);
+    
+    % constrain states
+    [states]  = ConstrainStates(states,dt_imu_avg);
+    
+    dtCov = dtCov + dt_imu;
+    delAngCov = delAngCov + delAngCorrected;
+    delVelCov = delVelCov + delVelCorrected;
+    if (dtCov > 0.01)
+        % predict covariance
+        covariance  = PredictCovariance(delAngCov,delVelCov,states,covariance,dtCov,param);
+        delAngCov = [0;0;0];
+        delVelCov = [0;0;0];
+        dtCovInt = dtCovInt + dtCov;
+        dtCov = 0;
+        covIndex = covIndex + 1;
+        
+        % output state data
+        output.time_lapsed(covIndex) = local_time;
+        output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')';
+        output.velocity_NED(covIndex,:) = states(5:7)';
+        output.position_NED(covIndex,:) = states(8:10)';
+        output.gyro_bias(covIndex,:) = states(11:13)';
+        output.accel_bias(covIndex,:) = states(14:16)';
+        output.mag_NED(covIndex,:) = states(17:19);
+        output.mag_XYZ(covIndex,:) = states(20:22);
+        output.wind_NE(covIndex,:) = states(23:24);
+        
+        % output covariance data
+        for i=1:24
+            output.state_variances(covIndex,i) = covariance(i,i);
+        end
+        
+        % output equivalent euler angle variances
+        error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));
+        euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);
+        for i=1:3
+            output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);
+        end
+        
+        % Get most recent GPS data that had fallen behind the fusion time horizon
+        latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
+        
+        if ~isempty(latest_gps_index)
+            % Check if GPS use is being blocked by the user
+            if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
+                gps_use_started = false;
+                gps_use_blocked = true;
+            else
+                gps_use_blocked = false;
+            end
+            
+            % If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS
+            if (~gps_use_started && ~gps_use_blocked)
+                if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim))
+                    states(5:7) = gps_data.vel_ned(latest_gps_index,:);
+                    states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
+                    gps_use_started = true;
+                    last_drift_constrain_time = local_time;
+                end
+            end
+            
+            % Fuse GPS data when available if GPS use has started
+            if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
+                last_gps_index = latest_gps_index;
+                gps_fuse_index = gps_fuse_index + 1;
+                last_drift_constrain_time = local_time;
+                
+                % fuse NED GPS velocity
+                [states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index));
+                
+                % data logging
+                output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
+                output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
+                output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
+                
+                % fuse NE GPS position
+                [states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index));
+                
+                % data logging
+                output.innovations.pos_time_lapsed(gps_fuse_index) = local_time;
+                output.innovations.posInnov(gps_fuse_index,:) = posInnov';
+                output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
+            else
+                % Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging
+                if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim)
+                    if ((local_time - last_synthetic_velocity_fusion_time) > 0.2)
+                        [states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim);
+                        last_synthetic_velocity_fusion_time = local_time;
+                    end
+                end
+            end
+        end
+        
+        % Fuse new Baro data that has fallen beind the fusion time horizon
+        latest_baro_index = find((baro_data.time_us - 1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
+        if (latest_baro_index > last_baro_index)
+            last_baro_index = latest_baro_index;
+            baro_fuse_index = baro_fuse_index + 1;
+            
+            % fuse baro height
+            [states,covariance,hgtInnov,hgtInnovVar] = FuseBaroHeight(states,covariance,baro_data.height(latest_baro_index),param.fusion.baroHgtGate,param.fusion.baroHgtNoise);
+            
+            % data logging
+            output.innovations.hgt_time_lapsed(baro_fuse_index) = local_time;
+            output.innovations.hgtInnov(baro_fuse_index) = hgtInnov;
+            output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar;
+        end
+        
+        % Fuse new mag data that has fallen behind the fusion time horizon
+        latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
+        if (latest_mag_index > last_mag_index)
+            last_mag_index = latest_mag_index;
+            mag_fuse_index = mag_fuse_index + 1;
+            
+            % output magnetic field length to help with diagnostics
+            output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:)));
+            
+            % fuse magnetometer data
+            if (param.fusion.magFuseMethod == 0 || param.fusion.magFuseMethod == 1)
+                [states,covariance,magInnov,magInnovVar] = FuseMagnetometer(states,covariance,mag_data.field_ga(latest_mag_index,:),param.fusion.magFieldGate, param.fusion.magFieldError^2);
+                
+                % data logging
+                output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
+                output.innovations.magInnov(mag_fuse_index,:) = magInnov;
+                output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar;
+                
+                if (param.fusion.magFuseMethod == 1)
+                    % fuse in the local declination value
+                    [states, covariance] = FuseMagDeclination(states, covariance, param.fusion.magDeclDeg*deg2rad);
+                    
+                end
+                
+            elseif (param.fusion.magFuseMethod == 2)
+                % fuse magnetometer data as a single magnetic heading measurement
+                [states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2);
+                
+                % log data
+                output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
+                output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
+                output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;
+                
+            end
+            
+        end
+        
+        % Check if optical flow use is being blocked by the user
+        if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime))
+            flow_use_blocked = true;
+        else
+            flow_use_blocked = false;
+        end
+        
+        % Attempt to use optical flow and range finder data if available and not blocked
+        if (flowDataPresent && ~flow_use_blocked)
+            
+            % Get latest range finder data and gate to remove dropouts
+            last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
+            if (rng_data.dist(last_range_index) < param.fusion.rngValidMax)
+                range = max(rng_data.dist(last_range_index) , param.fusion.rngValidMin);
+                last_valid_range_time = local_time;
+            end
+            
+            % Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement
+            latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
+            
+            if (~isempty(latest_flow_index) && (latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout))
+                last_flow_index = latest_flow_index;
+                flow_fuse_index = flow_fuse_index + 1;
+                last_drift_constrain_time = local_time;
+                
+                % fuse flow data
+                flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)];
+                bodyRate = [flow_data.bodyX(latest_flow_index);flow_data.bodyY(latest_flow_index)];
+                [states,covariance,flowInnov,flowInnovVar] = FuseOpticalFlow(states, covariance, flowRate, bodyRate, range, param.fusion.flowRateError^2, param.fusion.flowGate);
+                
+                % data logging
+                output.innovations.flow_time_lapsed(flow_fuse_index) = local_time;
+                output.innovations.flowInnov(flow_fuse_index,:) = flowInnov;
+                output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar;
+                
+            end
+            
+        end
+        
+        % Check if optical flow use is being blocked by the user
+        if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime))
+            viso_use_blocked = true;
+        else
+            viso_use_blocked = false;
+        end
+        
+        % attempt to use ZED camera visual odmetry data if available and not blocked
+        if (visoDataPresent && ~viso_use_blocked)
+            
+            % Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon
+            latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
+            if (latest_viso_index > last_viso_index)
+                last_viso_index = latest_viso_index;
+                viso_fuse_index = viso_fuse_index + 1;
+                last_drift_constrain_time = local_time;
+                
+                % convert delta position measurements to velocity
+                relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index);
+                
+                % convert quality metric to equivalent observation error
+                % (this is a guess)
+                quality = viso_data.qual(latest_viso_index);
+                bodyVelError = param.fusion.bodyVelErrorMin * quality + param.fusion.bodyVelErrorMax * (1 - quality);
+                
+                % fuse measurements
+                [states,covariance,bodyVelInnov,bodyVelInnovVar] = FuseBodyVel(states, covariance, relVelBodyMea, bodyVelError^2, param.fusion.bodyVelGate);
+                
+                % data logging
+                output.innovations.bodyVel_time_lapsed(viso_fuse_index) = local_time;
+                output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov;
+                output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar;
+                
+            end
+            
+        end
+        
+    end
+    
+    % update average delta time estimate
+    output.dt = dtCovInt / covIndex;
+    
+end
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m

@@ -0,0 +1,69 @@
+%% Filter Control
+param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass
+param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s)
+param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m)
+param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec)
+param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec)
+param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec)
+param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec)
+param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec)
+param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec)
+param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec)
+param.control.rollAlignErr = 0.0; % initial roll misalignment (rad)
+param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad)
+param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad)
+
+%% GPS fusion
+param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
+param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
+param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
+param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec)
+
+%% Baro fusion
+param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec)
+param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
+param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
+
+%% Magnetometer measurement fusion
+param.fusion.magTimeDelay = 0.0; % magnetometer time delay relative to IMU (sec)
+param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
+param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
+param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
+param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD
+param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD
+param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg
+
+%% Optical flow measurement fusion
+param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec)
+param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec)
+param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec)
+param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
+param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be  no less than this (m)
+param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
+param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec)
+
+%% Visual odometry body frame velocity measurement fusion
+param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
+param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
+param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec)
+param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
+
+%% State prediction error growth
+param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
+param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
+param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
+param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3)
+param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec)
+param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2)
+
+%% Initial Uncertainty
+param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec)
+param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec)
+param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec)
+param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec)
+param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2)
+param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion.
+param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss)
+param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss)
+param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m)
+param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec)

src/lib/ecl/EKF/matlab/EKF_replay/Filter/SetParameters.m

@@ -0,0 +1,69 @@
+%% Filter Control
+param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass
+param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s)
+param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m)
+param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec)
+param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec)
+param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec)
+param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec)
+param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec)
+param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec)
+param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec)
+param.control.rollAlignErr = 0.0; % initial roll misalignment (rad)
+param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad)
+param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad)
+
+%% GPS fusion
+param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
+param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
+param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
+param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec)
+
+%% Baro fusion
+param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec)
+param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
+param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
+
+%% Magnetometer measurement fusion
+param.fusion.magTimeDelay = 0.0; % magnetometer time delay relative to IMU (sec)
+param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
+param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
+param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
+param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD
+param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD
+param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg
+
+%% Optical flow measurement fusion
+param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec)
+param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec)
+param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec)
+param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
+param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be  no less than this (m)
+param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
+param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec)
+
+%% Visual odometry body frame velocity measurement fusion
+param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
+param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
+param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec)
+param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
+
+%% State prediction error growth
+param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
+param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
+param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
+param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3)
+param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec)
+param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2)
+
+%% Initial Uncertainty
+param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec)
+param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec)
+param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec)
+param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec)
+param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2)
+param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion.
+param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss)
+param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss)
+param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m)
+param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec)

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcF24.m

@@ -0,0 +1,45 @@
+function F = calcF24(dax,dax_b,day,day_b,daz,daz_b,dt,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3)
+%CALCF24
+%    F = CALCF24(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DT,DVX,DVX_B,DVY,DVY_B,DVZ,DVZ_B,Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:12
+
+t2 = dax_b.*(1.0./2.0);
+t3 = daz_b.*(1.0./2.0);
+t4 = day_b.*(1.0./2.0);
+t8 = day.*(1.0./2.0);
+t5 = t4-t8;
+t6 = q3.*(1.0./2.0);
+t7 = q2.*(1.0./2.0);
+t9 = daz.*(1.0./2.0);
+t10 = dax.*(1.0./2.0);
+t11 = -t2+t10;
+t12 = q1.*(1.0./2.0);
+t13 = -t3+t9;
+t14 = -t4+t8;
+t15 = dvx-dvx_b;
+t16 = dvy-dvy_b;
+t17 = dvz-dvz_b;
+t18 = q1.*t17.*2.0;
+t19 = q1.*t16.*2.0;
+t20 = q0.*t17.*2.0;
+t21 = q1.*t15.*2.0;
+t22 = q2.*t16.*2.0;
+t23 = q3.*t17.*2.0;
+t24 = t21+t22+t23;
+t25 = q0.*t15.*2.0;
+t26 = q2.*t17.*2.0;
+t37 = q3.*t16.*2.0;
+t27 = t25+t26-t37;
+t28 = q0.*q3.*2.0;
+t29 = q0.^2;
+t30 = q1.^2;
+t31 = q2.^2;
+t32 = q3.^2;
+t33 = q2.*t15.*2.0;
+t34 = q3.*t15.*2.0;
+t35 = q0.*t16.*2.0;
+t36 = -t18+t34+t35;
+t38 = q0.*q1.*2.0;
+F = reshape([1.0,t11,t14,t13,t27,t36,t19+t20-t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dax.*(-1.0./2.0)+t2,1.0,t3-t9,t14,t24,-t19-t20+t33,t36,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5,t13,1.0,t2-t10,t19+t20-q2.*t15.*2.0,t24,-t25-t26+t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,daz.*(-1.0./2.0)+t3,t5,t11,1.0,t18-q0.*t16.*2.0-q3.*t15.*2.0,t27,t24,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t12,q0.*(-1.0./2.0),-t6,t7,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t6,q0.*(-1.0./2.0),-t12,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t6,-t7,t12,q0.*(-1.0./2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t29-t30+t31+t32,-t28-q1.*q2.*2.0,q0.*q2.*2.0-q1.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t28-q1.*q2.*2.0,-t29+t30-t31+t32,-t38-q2.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q2.*-2.0-q1.*q3.*2.0,t38-q2.*q3.*2.0,-t29+t30+t31-t32,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0],[24, 24]);

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_HDG.m

@@ -0,0 +1,51 @@
+function H_MAG = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3)
+%CALCH_HDG
+%    H_MAG = CALCH_HDG(MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:14
+
+t2 = q0.^2;
+t3 = q1.^2;
+t4 = q2.^2;
+t5 = q3.^2;
+t6 = q0.*q3.*2.0;
+t10 = q1.*q2.*2.0;
+t17 = t2-t3+t4-t5;
+t18 = magY.*t17;
+t19 = t6+t10;
+t20 = magX.*t19;
+t21 = q0.*q1.*2.0;
+t22 = q2.*q3.*2.0;
+t23 = t21-t22;
+t24 = magZ.*t23;
+t7 = t18+t20-t24;
+t8 = t2+t3-t4-t5;
+t9 = magX.*t8;
+t11 = q0.*q2.*2.0;
+t12 = q1.*q3.*2.0;
+t13 = t11+t12;
+t14 = magZ.*t13;
+t15 = t6-t10;
+t25 = magY.*t15;
+t16 = t9+t14-t25;
+t26 = 1.0./t16.^2;
+t27 = t7.^2;
+t28 = 1.0./t16;
+t29 = t26.*t27;
+t30 = t29+1.0;
+t31 = 1.0./t30;
+t32 = magX.*q1.*2.0;
+t33 = magY.*q2.*2.0;
+t34 = magZ.*q3.*2.0;
+t35 = t32+t33+t34;
+t36 = magY.*q1.*2.0;
+t37 = magZ.*q0.*2.0;
+t38 = t36+t37-magX.*q2.*2.0;
+t39 = magX.*q0.*2.0;
+t40 = magZ.*q2.*2.0;
+t41 = t39+t40-magY.*q3.*2.0;
+t42 = magY.*q0.*2.0;
+t43 = magX.*q3.*2.0;
+t44 = t42+t43-magZ.*q1.*2.0;
+H_MAG = [(t28.*t44-t7.*t26.*t41)./(t27.*1.0./(t9+t14-magY.*(t6-q1.*q2.*2.0)).^2+1.0),-t31.*(t28.*t38+t7.*t26.*t35),t31.*(t28.*t35-t7.*t26.*t38),t31.*(t28.*t41+t7.*t26.*t44),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t31.*(t19.*t28-t7.*t8.*t26),t31.*(t17.*t28+t7.*t15.*t26),-t31.*(t23.*t28+t7.*t13.*t26),0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_LOSX.m

@@ -0,0 +1,9 @@
+function H_LOSX = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn)
+%CALCH_LOSX
+%    H_LOSX = CALCH_LOSX(Q0,Q1,Q2,Q3,RANGE,VD,VE,VN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:14
+
+t2 = 1.0./range;
+H_LOSX = [t2.*(q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0),t2.*(q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0),t2.*(q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0),-t2.*(q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0),-t2.*(q0.*q3.*2.0-q1.*q2.*2.0),t2.*(q0.^2-q1.^2+q2.^2-q3.^2),t2.*(q0.*q1.*2.0+q2.*q3.*2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_LOSY.m

@@ -0,0 +1,9 @@
+function H_LOSY = calcH_LOSY(q0,q1,q2,q3,range,vd,ve,vn)
+%CALCH_LOSY
+%    H_LOSY = CALCH_LOSY(Q0,Q1,Q2,Q3,RANGE,VD,VE,VN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:15
+
+t2 = 1.0./range;
+H_LOSY = [-t2.*(q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0),-t2.*(q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0),t2.*(q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0),-t2.*(q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0),-t2.*(q0.^2+q1.^2-q2.^2-q3.^2),-t2.*(q0.*q3.*2.0+q1.*q2.*2.0),t2.*(q0.*q2.*2.0-q1.*q3.*2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_MAGD.m

@@ -0,0 +1,12 @@
+function H_MAGD = calcH_MAGD(magE,magN)
+%CALCH_MAGD
+%    H_MAGD = CALCH_MAGD(MAGE,MAGN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:13
+
+t2 = magE.^2;
+t3 = magN.^2;
+t4 = t2+t3;
+t5 = 1.0./t4;
+H_MAGD = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-magE.*t5,magN.*t5,0.0,0.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_MAGX.m

@@ -0,0 +1,8 @@
+function H_MAGX = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3)
+%CALCH_MAGX
+%    H_MAGX = CALCH_MAGX(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:12
+
+H_MAGX = [magD.*q2.*-2.0+magE.*q3.*2.0+magN.*q0.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,magD.*q0.*-2.0+magE.*q1.*2.0-magN.*q2.*2.0,magD.*q1.*2.0+magE.*q0.*2.0-magN.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.^2+q1.^2-q2.^2-q3.^2,q0.*q3.*2.0+q1.*q2.*2.0,q0.*q2.*-2.0+q1.*q3.*2.0,1.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_MAGY.m

@@ -0,0 +1,8 @@
+function H_MAGY = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3)
+%CALCH_MAGY
+%    H_MAGY = CALCH_MAGY(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:13
+
+H_MAGY = [magD.*q1.*2.0+magE.*q0.*2.0-magN.*q3.*2.0,magD.*q0.*2.0-magE.*q1.*2.0+magN.*q2.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,magD.*q2.*2.0-magE.*q3.*2.0-magN.*q0.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q3.*-2.0+q1.*q2.*2.0,q0.^2-q1.^2+q2.^2-q3.^2,q0.*q1.*2.0+q2.*q3.*2.0,0.0,1.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m

@@ -0,0 +1,8 @@
+function H_MAGZ = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3)
+%CALCH_MAGZ
+%    H_MAGZ = CALCH_MAGZ(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:13
+
+H_MAGZ = [magD.*q0.*2.0-magE.*q1.*2.0+magN.*q2.*2.0,magD.*q1.*-2.0-magE.*q0.*2.0+magN.*q3.*2.0,magD.*q2.*-2.0+magE.*q3.*2.0+magN.*q0.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q2.*2.0+q1.*q3.*2.0,q0.*q1.*-2.0+q2.*q3.*2.0,q0.^2-q1.^2-q2.^2+q3.^2,0.0,0.0,1.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_VELX.m

@@ -0,0 +1,8 @@
+function H_VELX = calcH_VELX(q0,q1,q2,q3,vd,ve,vn)
+%CALCH_VELX
+%    H_VELX = CALCH_VELX(Q0,Q1,Q2,Q3,VD,VE,VN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:15
+
+H_VELX = [q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q0.*vd.*-2.0+q1.*ve.*2.0-q2.*vn.*2.0,q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0,q0.^2+q1.^2-q2.^2-q3.^2,q0.*q3.*2.0+q1.*q2.*2.0,q0.*q2.*-2.0+q1.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_VELY.m

@@ -0,0 +1,8 @@
+function H_VELY = calcH_VELY(q0,q1,q2,q3,vd,ve,vn)
+%CALCH_VELY
+%    H_VELY = CALCH_VELY(Q0,Q1,Q2,Q3,VD,VE,VN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:15
+
+H_VELY = [q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0,q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q2.*vd.*2.0-q3.*ve.*2.0-q0.*vn.*2.0,q0.*q3.*-2.0+q1.*q2.*2.0,q0.^2-q1.^2+q2.^2-q3.^2,q0.*q1.*2.0+q2.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcH_VELZ.m

@@ -0,0 +1,8 @@
+function H_VELZ = calcH_VELZ(q0,q1,q2,q3,vd,ve,vn)
+%CALCH_VELZ
+%    H_VELZ = CALCH_VELZ(Q0,Q1,Q2,Q3,VD,VE,VN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:16
+
+H_VELZ = [q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0,q1.*vd.*-2.0-q0.*ve.*2.0+q3.*vn.*2.0,q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q0.*q2.*2.0+q1.*q3.*2.0,q0.*q1.*-2.0+q2.*q3.*2.0,q0.^2-q1.^2-q2.^2+q3.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];

src/lib/ecl/EKF/matlab/EKF_replay/Filter/calcQ24.m

@@ -0,0 +1,45 @@
+function Q = calcQ24(daxVar,dayVar,dazVar,dvxVar,dvyVar,dvzVar,q0,q1,q2,q3)
+%CALCQ24
+%    Q = CALCQ24(DAXVAR,DAYVAR,DAZVAR,DVXVAR,DVYVAR,DVZVAR,Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:11
+
+t2 = dayVar.*q2.*q3.*(1.0./4.0);
+t3 = t2-daxVar.*q0.*q1.*(1.0./4.0)-dazVar.*q2.*q3.*(1.0./4.0);
+t4 = q3.^2;
+t5 = q2.^2;
+t6 = dazVar.*q1.*q3.*(1.0./4.0);
+t7 = t6-daxVar.*q1.*q3.*(1.0./4.0)-dayVar.*q0.*q2.*(1.0./4.0);
+t8 = daxVar.*q0.*q3.*(1.0./4.0);
+t9 = t8-dayVar.*q0.*q3.*(1.0./4.0)-dazVar.*q1.*q2.*(1.0./4.0);
+t10 = q0.^2;
+t11 = q1.^2;
+t12 = daxVar.*q1.*q2.*(1.0./4.0);
+t13 = t12-dayVar.*q1.*q2.*(1.0./4.0)-dazVar.*q0.*q3.*(1.0./4.0);
+t14 = dazVar.*q0.*q2.*(1.0./4.0);
+t15 = t14-daxVar.*q0.*q2.*(1.0./4.0)-dayVar.*q1.*q3.*(1.0./4.0);
+t16 = dayVar.*q0.*q1.*(1.0./4.0);
+t17 = t16-daxVar.*q2.*q3.*(1.0./4.0)-dazVar.*q0.*q1.*(1.0./4.0);
+t21 = q0.*q3.*2.0;
+t22 = q1.*q2.*2.0;
+t18 = t21-t22;
+t23 = q0.*q2.*2.0;
+t24 = q1.*q3.*2.0;
+t19 = t23+t24;
+t20 = t4+t5-t10-t11;
+t25 = q0.*q1.*2.0;
+t26 = t21+t22;
+t32 = t4-t5-t10+t11;
+t27 = dvyVar.*t18.*t32;
+t28 = q2.*q3.*2.0;
+t29 = t25-t28;
+t30 = t4-t5-t10+t11;
+t31 = t25+t28;
+t33 = t4-t5+t10-t11;
+t34 = t23-t24;
+t35 = dvxVar.*t34.*(t4+t5-t10-t11);
+t36 = dvzVar.*t19.*t33;
+t37 = t35+t36-dvyVar.*t18.*t31;
+t38 = -dvxVar.*t26.*t34-dvyVar.*t31.*t32-dvzVar.*t29.*t33;
+Q = reshape([daxVar.*t11.*(1.0./4.0)+dayVar.*t5.*(1.0./4.0)+dazVar.*t4.*(1.0./4.0),t3,t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3,daxVar.*t10.*(1.0./4.0)+dayVar.*t4.*(1.0./4.0)+dazVar.*t5.*(1.0./4.0),t9,t15,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t9,daxVar.*t4.*(1.0./4.0)+dayVar.*t10.*(1.0./4.0)+dazVar.*t11.*(1.0./4.0),t17,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t13,t15,t17,daxVar.*t5.*(1.0./4.0)+dayVar.*t11.*(1.0./4.0)+dazVar.*t10.*(1.0./4.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxVar.*t20.^2+dvyVar.*t18.^2+dvzVar.*t19.^2,t27-dvxVar.*t20.*t26-dvzVar.*t19.*t29,t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t27-dvzVar.*t19.*(t25-q2.*q3.*2.0)-dvxVar.*t20.*t26,dvxVar.*t26.^2+dvyVar.*t30.^2+dvzVar.*t29.^2,t38,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t37,t38,dvxVar.*t34.^2+dvyVar.*t31.^2+dvzVar.*t33.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[24, 24]);

src/lib/ecl/EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m

@@ -0,0 +1,50 @@
+clear all;
+close all;
+
+% add required paths
+addpath('../Common');
+
+% load test data
+load '../TestData/PX4/baro_data.mat';
+load '../TestData/PX4/gps_data.mat';
+load '../TestData/PX4/imu_data.mat';
+load '../TestData/PX4/mag_data.mat';
+
+% set parameters to default values
+run('SetParameterDefaults.m');
+
+% Loop through range of GPS time delays and capture the RMS velocity
+% innovation and corresponding delay each time
+for index = 1:1:21
+    index
+    param.fusion.gpsTimeDelay = 0.01*(index - 1);
+    output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
+    gps_delay(index) = param.fusion.gpsTimeDelay;
+    gps_vel_innov_rms(index) = sqrt(sum(output.innovations.vel_innov(:,1).^2) + sum(output.innovations.vel_innov(:,2).^2))/sqrt(length(output.innovations.vel_innov));
+end
+
+% plot the innvation level vs time delay
+figure;
+plot(gps_delay,gps_vel_innov_rms);
+grid on;
+xlabel('GPS delay (sec)');
+ylabel('RMS velocity innovation (m/s)');
+
+% find the smallest value and re-run the replay
+param.fusion.gpsTimeDelay = gps_delay(gps_delay == min(gps_delay))
+
+% run the filter replay
+output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
+
+% generate and save output plots
+runIdentifier = ' : PX4 data replay ';
+folder = strcat('../OutputPlots/PX4');
+PlotData(output,folder,runIdentifier);
+
+% save output data
+folder = '../OutputData/PX4';
+fileName = '../OutputData/PX4/ekf_replay_output.mat';
+if ~exist(folder,'dir')
+    mkdir(folder);
+end
+save(fileName,'output');

src/lib/ecl/EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m

@@ -0,0 +1,38 @@
+function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3)
+%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX
+%    ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    13-Jul-2017 08:41:41
+
+t8 = q0.*q1.*2.0;
+t9 = q2.*q3.*2.0;
+t2 = t8+t9;
+t4 = q0.^2;
+t5 = q1.^2;
+t6 = q2.^2;
+t7 = q3.^2;
+t3 = t4-t5-t6+t7;
+t10 = t3.^2;
+t11 = t2.^2;
+t12 = t10+t11;
+t13 = 1.0./t12;
+t14 = 1.0./t3;
+t15 = 1.0./t3.^2;
+t17 = q0.*q2.*2.0;
+t18 = q1.*q3.*2.0;
+t16 = t17-t18;
+t19 = t16.^2;
+t20 = -t19+1.0;
+t21 = 1.0./sqrt(t20);
+t24 = q0.*q3.*2.0;
+t25 = q1.*q2.*2.0;
+t22 = t24+t25;
+t23 = t4+t5-t6-t7;
+t26 = t23.^2;
+t27 = t22.^2;
+t28 = t26+t27;
+t29 = 1.0./t28;
+t30 = 1.0./t23;
+t31 = 1.0./t23.^2;
+error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]);

src/lib/ecl/EKF/matlab/EKF_replay/Filter/replay_apm_data.m

@@ -0,0 +1,46 @@
+clear all;
+close all;
+
+% add required paths
+addpath('../Common');
+
+% load compulsory data
+load '../TestData/APM/baro_data.mat';
+load '../TestData/APM/gps_data.mat';
+load '../TestData/APM/imu_data.mat';
+load '../TestData/APM/mag_data.mat';
+
+% load optional data required for optical flow replay
+if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file')
+    load '../TestData/APM/rng_data.mat';
+    load '../TestData/APM/flow_data.mat';
+else 
+    rng_data = [];
+    flow_data = [];
+end
+
+% load optional data required for ZED camera replay
+if exist('../TestData/APM/viso_data.mat','file')
+    load '../TestData/APM/viso_data.mat';
+else
+    viso_data = [];
+end
+
+% load default parameters
+run('SetParameters.m');
+
+% run the filter replay 
+output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
+
+% generate and save output plots
+runIdentifier = ' : APM data replay ';
+folder = strcat('../OutputPlots/APM');
+PlotData(output,folder,runIdentifier);
+
+% save output data
+folder = '../OutputData/APM';
+fileName = '../OutputData/APM/ekf_replay_output.mat';
+if ~exist(folder,'dir')
+    mkdir(folder);
+end
+save(fileName,'output');

src/lib/ecl/EKF/matlab/EKF_replay/Filter/replay_px4_data.m

@@ -0,0 +1,30 @@
+clear all;
+close all;
+
+% add required paths
+addpath('../Common');
+
+% load test data
+load '../TestData/PX4/baro_data.mat';
+load '../TestData/PX4/gps_data.mat';
+load '../TestData/PX4/imu_data.mat';
+load '../TestData/PX4/mag_data.mat';
+
+% set parameters to default values
+run('SetParameters.m');
+
+% run the filter replay
+output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
+
+% generate and save output plots
+runIdentifier = ' : PX4 data replay ';
+folder = strcat('../OutputPlots/PX4');
+PlotData(output,folder,runIdentifier);
+
+% save output data
+folder = '../OutputData/PX4';
+fileName = '../OutputData/PX4/ekf_replay_output.mat';
+if ~exist(folder,'dir')
+    mkdir(folder);
+end
+save(fileName,'output');

src/lib/ecl/EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m

@@ -0,0 +1,32 @@
+clear all;
+close all;
+
+% add required paths
+addpath('../Common');
+
+% load test data
+load '../TestData/PX4_optical_flow/baro_data.mat';
+load '../TestData/PX4_optical_flow/gps_data.mat';
+load '../TestData/PX4_optical_flow/imu_data.mat';
+load '../TestData/PX4_optical_flow/mag_data.mat';
+load '../TestData/PX4_optical_flow/rng_data.mat';
+load '../TestData/PX4_optical_flow/flow_data.mat';
+
+% set parameters to default values
+run('SetParameterDefaults.m');
+
+% run the filter replay
+output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data);
+
+% generate and save output plots
+runIdentifier = ' : PX4 optical flow replay ';
+folder = strcat('../OutputPlots/PX4_optical_flow');
+PlotData(output,folder,runIdentifier);
+
+% save output data
+folder = '../OutputData/PX4_optical_flow';
+fileName = '../OutputData/PX4_optical_flow/ekf_replay_output.mat';
+if ~exist(folder,'dir')
+    mkdir(folder);
+end
+save(fileName,'output');

src/lib/ecl/EKF/matlab/EKF_replay/Filter/transfer_matrix.m

@@ -0,0 +1,13 @@
+function T_MAG = transfer_matrix(magE,magN)
+%TRANSFER_MATRIX
+%    T_MAG = TRANSFER_MATRIX(MAGE,MAGN)
+
+%    This function was generated by the Symbolic Math Toolbox version 6.2.
+%    29-May-2017 00:16:16
+
+t2 = 1.0./magN.^2;
+t3 = magE.^2;
+t4 = t2.*t3;
+t5 = t4+1.0;
+t6 = 1.0./t5;
+T_MAG = [-magE.*t2.*t6,t6./magN];

src/lib/ecl/EKF/matlab/EKF_replay/SensorCalibration/allan.m

@@ -0,0 +1,19 @@
+function [T,sigma] = allan(omega,fs,pts)
+[N,M]  = size(omega);                 % figure out how big the output data set is
+n      = 2.^(0:floor(log2(N/2)))';    % determine largest bin size
+maxN = n(end);
+endLogInc = log10(maxN);
+m = unique(ceil(logspace(0,endLogInc,pts)))';  % create log spaced vector average factor
+t0     = 1/fs;
+T      = m*t0;
+theta  = cumsum(omega)/fs;
+sigma2 = zeros(length(T),M);
+for i=1:length(m)
+                                               % t0 = sample interval
+% T = length of time for each cluster
+% integration of samples over time to obtain output angle ?
+                                % array of dimensions (cluster periods) X (#variables)
+                                % loop over the various cluster sizes
+                              % implements the summation in the AV equation
+sigma2 = sigma2./repmat((2*T.^2.*(N-2*m)),1,M);
+sigma  = sqrt(sigma2)

src/lib/ecl/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m

@@ -0,0 +1,189 @@
+% test ellipsoid sphere fitting algorithms
+%
+% http://www.st.com/content/ccc/resource/technical/document/design_tip/group0/a2/98/f5/d4/9c/48/4a/d1/DM00286302/files/DM00286302.pdf/jcr:content/translations/en.DM00286302.pdf
+%
+
+%% load log data
+clear all;
+close all;
+
+% uncomment these lines if using legacy .px4log format
+%load sysvector.mat;
+%mag_meas = [sysvector.IMU_MagX';sysvector.IMU_MagY';sysvector.IMU_MagZ'];
+
+% uncomment these lines if using data imported from ulog format
+%load sysdata.mat;
+%mag_meas = [magnetometer_ga0';magnetometer_ga1';magnetometer_ga2'];
+
+% thin data points to use data every 5 deg
+delta_angle_lim = 5* pi/180;
+counter = 1;
+
+angle = 0;
+last_angle = 0;
+for i = 2:length(data1.IMU.Tsec)
+    ang_rate = 0.5 * (sqrt(data1.IMU.GyroX(i)^2 + data1.IMU.GyroY(i)^2 + data1.IMU.GyroZ(i)^2) + ...
+        sqrt(data1.IMU.GyroX(i-1)^2 + data1.IMU.GyroY(i-1)^2 + data1.IMU.GyroZ(i-1)^2));
+    dt = data1.IMU.Tsec(i) - data1.IMU.Tsec(i-1);
+    angle = angle + ang_rate * dt;
+    if ((angle - last_angle) > delta_angle_lim)
+        mag_meas(:,counter) = [data1.IMU.MagX(i);data1.IMU.MagY(i);data1.IMU.MagZ(i)];
+        counter = counter + 1;
+        last_angle = angle;
+    end
+end
+
+angle = 0;
+last_angle = 0;
+for i = 2:length(data2.IMU.Tsec)
+    ang_rate = 0.5 * (sqrt(data2.IMU.GyroX(i)^2 + data2.IMU.GyroY(i)^2 + data2.IMU.GyroZ(i)^2) + ...
+        sqrt(data2.IMU.GyroX(i-1)^2 + data2.IMU.GyroY(i-1)^2 + data2.IMU.GyroZ(i-1)^2));
+    dt = data2.IMU.Tsec(i) - data2.IMU.Tsec(i-1);
+    angle = angle + ang_rate * dt;
+    if ((angle - last_angle) > delta_angle_lim)
+        mag_meas(:,counter) = [data2.IMU.MagX(i);data2.IMU.MagY(i);data2.IMU.MagZ(i)];
+        counter = counter + 1;
+        last_angle = angle;
+    end
+
+end
+
+angle = 0;
+last_angle = 0;
+for i = 2:length(data3.IMU.Tsec)
+    ang_rate = 0.5 * (sqrt(data3.IMU.GyroX(i)^2 + data3.IMU.GyroY(i)^2 + data3.IMU.GyroZ(i)^2) + ...
+        sqrt(data3.IMU.GyroX(i-1)^2 + data3.IMU.GyroY(i-1)^2 + data3.IMU.GyroZ(i-1)^2));
+    dt = data3.IMU.Tsec(i) - data3.IMU.Tsec(i-1);
+    angle = angle + ang_rate * dt;
+    if ((angle - last_angle) > delta_angle_lim)
+        mag_meas(:,counter) = [data3.IMU.MagX(i);data3.IMU.MagY(i);data3.IMU.MagZ(i)];
+        counter = counter + 1;
+        last_angle = angle;
+    end
+
+end
+
+
+%% fit a sphere and determine the fit quality
+[offset,gain,rotation]=ellipsoid_fit(mag_meas',5);
+
+% correct the data
+mag_corrected_5 = zeros(size(mag_meas));
+rotation_correction = inv(rotation); % we apply the inverse of the original rotation
+scale_correction = 1./gain;
+scale_correction = scale_correction ./ mean(scale_correction);
+mag_strength = zeros(length(mag_meas),1);
+for i = 1:length(mag_meas)
+    % subtract the offsets
+    mag_corrected_5(:,i) = mag_meas(:,i) - offset;
+    
+    % correct the rotation
+    mag_corrected_5(:,i) = rotation_correction * mag_corrected_5(:,i);
+    
+    % correct the scale factor
+    mag_corrected_5(:,i) = mag_corrected_5(:,i) .* scale_correction;
+    
+    % calculate the residual
+    mag_strength(i) = sqrt(dot(mag_corrected_5(:,i),mag_corrected_5(:,i)));
+    
+end
+
+% calculate the fit residual for fit option 5
+fit_residual_5 = mag_strength - mean(mag_strength);
+
+%% fit a un-rotated ellipsoid and determine the fit quality
+[offset,gain,rotation]=ellipsoid_fit(mag_meas',1);
+
+% correct the data
+mag_corrected_1 = zeros(size(mag_meas));
+rotation_correction = inv(rotation); % we apply the inverse of the original rotation
+scale_correction = 1./gain;
+scale_correction = scale_correction ./ mean(scale_correction);
+mag_strength = zeros(length(mag_meas),1);
+angle_change_1 = zeros(length(mag_meas),1);
+for i = 1:length(mag_meas)
+    % subtract the offsets
+    mag_corrected_1(:,i) = mag_meas(:,i) - offset;
+    
+    % correct the rotation
+    mag_corrected_1(:,i) = rotation_correction * mag_corrected_1(:,i);
+    
+    % correct the scale factor
+    mag_corrected_1(:,i) = mag_corrected_1(:,i) .* scale_correction;
+    
+    % calculate the residual
+    mag_strength(i) = sqrt(dot(mag_corrected_1(:,i),mag_corrected_1(:,i)));
+    
+    % calculate the angular change due to the fit
+    angle_change_1(i) = atan2(norm(cross(mag_corrected_1(:,i),mag_meas(:,i))),dot(mag_corrected_1(:,i),mag_meas(:,i)));
+    
+end
+
+% calculate the fit residual for fit option 1
+fit_residual_1 = mag_strength - mean(mag_strength);
+
+%% fit a rotated ellipsoid and check the fit quality
+[offset,gain,rotation]=ellipsoid_fit(mag_meas',0);
+
+% correct the data
+mag_corrected_0 = zeros(size(mag_meas));
+rotation_correction = inv(rotation); % we apply the inverse of the original rotation
+scale_correction = 1./gain;
+scale_correction = scale_correction ./ mean(scale_correction);
+mag_strength = zeros(length(mag_meas),1);
+angle_change_0 = zeros(length(mag_meas),1);
+for i = 1:length(mag_meas)
+    % subtract the offsets
+    mag_corrected_0(:,i) = mag_meas(:,i) - offset;
+    
+    % correct the rotation
+    mag_corrected_0(:,i) = rotation_correction * mag_corrected_0(:,i);
+    
+    % correct the scale factor
+    mag_corrected_0(:,i) = mag_corrected_0(:,i) .* scale_correction;
+    
+    % calculate the residual
+    mag_strength(i) = sqrt(dot(mag_corrected_0(:,i),mag_corrected_0(:,i)));
+    
+    % calculate the angular change due to the fit
+    angle_change_0(i) = atan2(norm(cross(mag_corrected_0(:,i),mag_meas(:,i))),dot(mag_corrected_0(:,i),mag_meas(:,i)));
+    
+end
+
+% calculate the fit residual for fit option 0
+fit_residual_0 = mag_strength - mean(mag_strength);
+
+%% calculate the residual for uncorrected data
+for i = 1:length(mag_meas)
+    mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i)));    
+end
+uncorrected_residual = mag_strength - mean(mag_strength);
+
+%% plot the fit residuals
+plot(uncorrected_residual,'k+');
+hold on;
+plot(fit_residual_5,'r+');
+plot(fit_residual_1,'b+');
+plot(fit_residual_0,'g+');
+hold off;
+grid on;
+title('mag fit comparison');
+xlabel('measurement index');
+ylabel('fit residual (Gauss)');
+legend('uncorrected','sphere','non-rotated ellipse','rotated ellipse');
+
+%% plot the data points in 3D
+figure;
+plot3(mag_meas(1,:),mag_meas(2,:),mag_meas(3,:),' .');hold on;plot3(mag_corrected_1(1,:),mag_corrected_1(2,:),mag_corrected_1(3,:),'r.');
+hold off;grid on;axis equal;
+xlabel('x (Gauss)');
+xlabel('y (Gauss)');
+xlabel('z (Gauss)');
+legend('uncorrected','unrotated ellipse');
+
+%% calculate and plot the angular error
+figure;
+plot(angle_change_1*(180/pi),'b+');
+title('angle change after un-rotated ellipse fit');
+xlabel('measurement index');
+ylabel('angle change magnitude (deg)');

src/lib/ecl/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m

@@ -0,0 +1,61 @@
+function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin) 
+
+% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points 
+
+% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z) 
+
+% optional flag f, default to 0 (fitting of rotated ellipsoid) 
+
+x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end; 
+
+if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid) 
+
+elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid) 
+
+elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y 
+
+elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z 
+
+elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z 
+
+elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere) 
+
+end; 
+
+v = (D'*D)\(D'*ones(length(x),1)); % least square fitting 
+
+if f==0, % rotated ellipsoid 
+
+ A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ]; 
+
+ ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid 
+
+ Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0) 
+
+ [rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain) 
+
+ gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid 
+
+else % non-rotated ellipsoid 
+
+ if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; 
+
+ elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; 
+
+ elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; 
+
+ elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; 
+
+ elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere 
+
+ end; 
+
+ ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid 
+
+ rotM=eye(3); % eigenvectors (rotation), identity = no rotation 
+
+ g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3)); 
+
+ gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale) 
+
+end; 

src/lib/ecl/EKF/matlab/EKF_replay/px4_replay_import.m

@@ -0,0 +1,387 @@
+%% PX4 replay: import sensors CSV
+% the following variables must be set beforehand!
+if ~exist('sensors_file','var')
+    error('sensors_file missing');
+end
+if ~exist('air_data_file','var')
+    error('air_data_file missing');
+end
+if ~exist('magnetometer_file','var')
+    error('magnetometer_file missing');
+end
+if ~exist('gps_file','var')
+    error('gps_file missing');
+end
+
+if ~exist('attitude_file','var')
+    disp('INFO no attitude_file set, all ground truth data will be skipped');
+end
+if ~exist('localpos_file','var')
+    disp('INFO no localpos_file set, all ground truth data will be skipped');
+end
+if ~exist('globalpos_file','var')
+    disp('INFO no globalpos_file set, all ground truth data will be skipped');
+end
+
+if ~exist('actctrl_file','var')
+    disp('INFO no actctrl_file set, all actuator data will be skipped');
+end
+if ~exist('actout_file','var')
+    disp('INFO no actout_file set, all actuator data will be skipped');
+end
+
+%% ------                 SECTION 1: IMU, Baro, Mag                 ------
+
+%% Import IMU data from text file
+opts = delimitedTextImportOptions("NumVariables", 10);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "gyro_rad0", "gyro_rad1", "gyro_rad2", "gyro_integral_dt", "accelerometer_timestamp_relative", "accelerometer_m_s20", "accelerometer_m_s21", "accelerometer_m_s22", "accelerometer_integral_dt"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(sensors_file, opts);
+
+% Convert to output type
+timestamp = tbl.timestamp;
+gyro_rad0 = tbl.gyro_rad0;
+gyro_rad1 = tbl.gyro_rad1;
+gyro_rad2 = tbl.gyro_rad2;
+gyro_integral_dt = tbl.gyro_integral_dt;
+accelerometer_timestamp_relative = tbl.accelerometer_timestamp_relative;
+accelerometer_m_s20 = tbl.accelerometer_m_s20;
+accelerometer_m_s21 = tbl.accelerometer_m_s21;
+accelerometer_m_s22 = tbl.accelerometer_m_s22;
+accelerometer_integral_dt = tbl.accelerometer_integral_dt;
+
+clear opts tbl
+
+%% Import Baro data from text file
+opts = delimitedTextImportOptions("NumVariables", 5);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp_baro", "baro_alt_meter", "baro_temp_celcius", "baro_pressure_pa", "rho"];
+opts.VariableTypes = ["double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(air_data_file, opts);
+
+% Convert to output type
+timestamp_baro = tbl.timestamp_baro;
+baro_alt_meter = tbl.baro_alt_meter;
+baro_temp_celcius = tbl.baro_temp_celcius;
+baro_pressure_pa = tbl.baro_pressure_pa;
+rho = tbl.rho;
+
+clear opts tbl
+
+%% Import Mag data from text file
+opts = delimitedTextImportOptions("NumVariables", 4);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp_mag", "magnetometer_ga0", "magnetometer_ga1", "magnetometer_ga2"];
+opts.VariableTypes = ["double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(magnetometer_file, opts);
+
+% Convert to output type
+timestamp_mag = tbl.timestamp_mag;
+magnetometer_ga0 = tbl.magnetometer_ga0;
+magnetometer_ga1 = tbl.magnetometer_ga1;
+magnetometer_ga2 = tbl.magnetometer_ga2;
+
+clear opts tbl
+
+%% Run conversion script for IMU, Baro and Mag
+cd Common/;
+convert_px4_sensor_combined_csv_data;
+cd ../;
+
+
+%% ------                       SECTION 2: GPS                       ------
+
+%% Import data from text file
+opts = delimitedTextImportOptions("NumVariables", 25);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "time_utc_usec", "lat", "lon", "alt", "alt_ellipsoid", "s_variance_m_s", "c_variance_rad", "eph", "epv", "hdop", "vdop", "noise_per_ms", "jamming_indicator", "vel_m_s", "vel_n_m_s", "vel_e_m_s", "vel_d_m_s", "cog_rad", "timestamp_time_relative", "heading", "heading_offset", "fix_type", "vel_ned_valid", "satellites_used"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(gps_file, opts);
+
+% Convert to output type
+timestamp = tbl.timestamp;
+time_utc_usec = tbl.time_utc_usec;
+lat = tbl.lat;
+lon = tbl.lon;
+alt = tbl.alt;
+alt_ellipsoid = tbl.alt_ellipsoid;
+s_variance_m_s = tbl.s_variance_m_s;
+c_variance_rad = tbl.c_variance_rad;
+eph = tbl.eph;
+epv = tbl.epv;
+hdop = tbl.hdop;
+vdop = tbl.vdop;
+noise_per_ms = tbl.noise_per_ms;
+jamming_indicator = tbl.jamming_indicator;
+vel_m_s = tbl.vel_m_s;
+vel_n_m_s = tbl.vel_n_m_s;
+vel_e_m_s = tbl.vel_e_m_s;
+vel_d_m_s = tbl.vel_d_m_s;
+cog_rad = tbl.cog_rad;
+timestamp_time_relative = tbl.timestamp_time_relative;
+heading = tbl.heading;
+heading_offset = tbl.heading_offset;
+fix_type = tbl.fix_type;
+vel_ned_valid = tbl.vel_ned_valid;
+satellites_used = tbl.satellites_used;
+
+clear opts tbl
+
+
+%% Run conversion script for GPS
+cd Common/;
+convert_px4_vehicle_gps_position_csv;
+cd ../;
+
+
+%% ------     SECTION 3: Ground Truth Data (STIL only, optional)     ------
+
+if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var')
+    
+%- Import Attitude data from text file
+opts = delimitedTextImportOptions("NumVariables", 13);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "rollspeed", "pitchspeed", "yawspeed", "q0", "q1", "q2", "q3", "delta_q_reset0", "delta_q_reset1", "delta_q_reset2", "delta_q_reset3", "quat_reset_counter"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(attitude_file, opts);
+
+% Convert to output type
+timestamp_att = tbl.timestamp;
+rollspeed = tbl.rollspeed;
+pitchspeed = tbl.pitchspeed;
+yawspeed = tbl.yawspeed;
+q0 = tbl.q0;
+q1 = tbl.q1;
+q2 = tbl.q2;
+q3 = tbl.q3;
+% delta_q_reset0 = tbl.delta_q_reset0;
+% delta_q_reset1 = tbl.delta_q_reset1;
+% delta_q_reset2 = tbl.delta_q_reset2;
+% delta_q_reset3 = tbl.delta_q_reset3;
+% quat_reset_counter = tbl.quat_reset_counter;
+
+clear opts tbl
+
+
+%- Import Global Position data from text file
+opts = delimitedTextImportOptions("NumVariables", 17);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "lat", "lon", "alt", "alt_ellipsoid", "delta_alt", "vel_n", "vel_e", "vel_d", "yaw", "eph", "epv", "terrain_alt", "lat_lon_reset_counter", "alt_reset_counter", "terrain_alt_valid", "dead_reckoning"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(globalpos_file, opts);
+
+% Convert to output type
+timestamp_gpos = tbl.timestamp;
+gpos_lat = tbl.lat;
+gpos_lon = tbl.lon;
+gpos_alt = tbl.alt;
+% gpos_alt_ellipsoid = tbl.alt_ellipsoid;
+% gpos_delta_alt = tbl.delta_alt;
+gpos_vel_n = tbl.vel_n;
+gpos_vel_e = tbl.vel_e;
+gpos_vel_d = tbl.vel_d;
+% gpos_yaw = tbl.yaw;
+% gpos_eph = tbl.eph;
+% gpos_epv = tbl.epv;
+% gpos_terrain_alt = tbl.terrain_alt;
+% gpos_lat_lon_reset_counter = tbl.lat_lon_reset_counter;
+% gpos_alt_reset_counter = tbl.alt_reset_counter;
+% gpos_terrain_alt_valid = tbl.terrain_alt_valid;
+% gpos_dead_reckoning = tbl.dead_reckoning;
+
+% Clear temporary variables
+clear opts tbl
+
+
+%- Import Local Position data from text file
+opts = delimitedTextImportOptions("NumVariables", 43);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "ref_timestamp", "ref_lat", "ref_lon", "x", "y", "z", "delta_xy0", "delta_xy1", "delta_z", "vx", "vy", "vz", "z_deriv", "delta_vxy0", "delta_vxy1", "delta_vz", "ax", "ay", "az", "yaw", "ref_alt", "dist_bottom", "dist_bottom_rate", "eph", "epv", "evh", "evv", "vxy_max", "vz_max", "hagl_min", "hagl_max", "xy_valid", "z_valid", "v_xy_valid", "v_z_valid", "xy_reset_counter", "z_reset_counter", "vxy_reset_counter", "vz_reset_counter", "xy_global", "z_global", "dist_bottom_valid"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(localpos_file, opts);
+
+% Convert to output type
+timestamp_lpos = tbl.timestamp;
+% lpos_ref_timestamp = tbl.ref_timestamp;
+lpos_ref_lat = tbl.ref_lat;
+lpos_ref_lon = tbl.ref_lon;
+lpos_x = tbl.x;
+lpos_y = tbl.y;
+lpos_z = tbl.z;
+% lpos_delta_xy0 = tbl.delta_xy0;
+% lpos_delta_xy1 = tbl.delta_xy1;
+% lpos_delta_z = tbl.delta_z;
+lpos_vx = tbl.vx;
+lpos_vy = tbl.vy;
+lpos_vz = tbl.vz;
+% lpos_z_deriv = tbl.z_deriv;
+% lpos_delta_vxy0 = tbl.delta_vxy0;
+% lpos_delta_vxy1 = tbl.delta_vxy1;
+% lpos_delta_vz = tbl.delta_vz;
+% lpos_ax = tbl.ax;
+% lpos_ay = tbl.ay;
+% lpos_az = tbl.az;
+lpos_yaw = tbl.yaw;
+% ref_alt = tbl.ref_alt;
+% dist_bottom = tbl.dist_bottom;
+% dist_bottom_rate = tbl.dist_bottom_rate;
+% eph = tbl.eph;
+% epv = tbl.epv;
+% evh = tbl.evh;
+% evv = tbl.evv;
+% vxy_max = tbl.vxy_max;
+% vz_max = tbl.vz_max;
+% hagl_min = tbl.hagl_min;
+% hagl_max = tbl.hagl_max;
+% xy_valid = tbl.xy_valid;
+% z_valid = tbl.z_valid;
+% v_xy_valid = tbl.v_xy_valid;
+% v_z_valid = tbl.v_z_valid;
+% xy_reset_counter = tbl.xy_reset_counter;
+% z_reset_counter = tbl.z_reset_counter;
+% vxy_reset_counter = tbl.vxy_reset_counter;
+% vz_reset_counter = tbl.vz_reset_counter;
+% xy_global = tbl.xy_global;
+% z_global = tbl.z_global;
+% dist_bottom_valid = tbl.dist_bottom_valid;
+
+% Clear temporary variables
+clear opts tbl
+
+
+%- Run conversion script for GPS
+cd Common/;
+convert_px4_groundtruth_csv_data;
+cd ../;
+
+end
+
+
+%% ------          SECTION 4: Actuator Controls (optional)          ------
+
+if exist('actctrl_file','var') && exist('actout_file','var')
+
+%- Import Actuator Control data from text file
+opts = delimitedTextImportOptions("NumVariables", 10);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "timestamp_sample", "control0", "control1", "control2", "control3", "control4", "control5", "control6", "control7"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(actctrl_file, opts);
+
+% Convert to output type
+timestamp_actctrl = tbl.timestamp;
+%timestamp_sample = tbl.timestamp_sample;
+control0 = tbl.control0;
+control1 = tbl.control1;
+control2 = tbl.control2;
+control3 = tbl.control3;
+% control4 = tbl.control4;
+% control5 = tbl.control5;
+% control6 = tbl.control6;
+% control7 = tbl.control7;
+
+% Clear temporary variables
+clear opts tbl
+
+
+%- Import Actuator Output data from text file
+opts = delimitedTextImportOptions("NumVariables", 18);
+opts.DataLines = [2, Inf];
+opts.Delimiter = ",";
+
+% Specify column names and types
+opts.VariableNames = ["timestamp", "noutputs", "output0", "output1", "output2", "output3", "output4", "output5", "output6", "output7", "output8", "output9", "output10", "output11", "output12", "output13", "output14", "output15"];
+opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+opts.ExtraColumnsRule = "ignore";
+opts.EmptyLineRule = "read";
+
+% Import the data
+tbl = readtable(actout_file, opts);
+
+% Convert to output type
+timestamp_actout = tbl.timestamp;
+% noutputs = tbl.noutputs;
+output0 = tbl.output0;
+output1 = tbl.output1;
+output2 = tbl.output2;
+output3 = tbl.output3;
+% output4 = tbl.output4;
+% output5 = tbl.output5;
+% output6 = tbl.output6;
+% output7 = tbl.output7;
+% output8 = tbl.output8;
+% output9 = tbl.output9;
+% output10 = tbl.output10;
+% output11 = tbl.output11;
+% output12 = tbl.output12;
+% output13 = tbl.output13;
+% output14 = tbl.output14;
+% output15 = tbl.output15;
+
+% Clear temporary variables
+clear opts tbl
+
+
+%- Run conversion script for GPS
+cd Common/;
+convert_px4_actuators_csv_data;
+cd ../;
+
+end

src/lib/ecl/EKF/matlab/EKF_replay/readme.txt

@@ -0,0 +1,108 @@
+Instructions for running the EKF replay
+
+1) Ensure the ‘EKF_replay’ directory is in a location you have full read and write access
+
+
+2) Create a ‘TestData’ sub-directory under the ‘EKF_replay’ directory
+
+A sample dataset can be downloaded here: https://drive.google.com/file/d/0By4v2BuLAaCfSW9fWl9aSWNGbGs/view?usp=sharing
+
+
+3a) If replaying APM data:
+
+Collect data with LOG_REPLAY = 1 and LOG_DISARMED = 1.
+Convert data to a .mat file using the MissionPlanner ‘Create Matlab File’ option under the DataFlash Logs tab.
+Convert .mat file to the required data format using the convert_apm_data.m script file. This will generate the following data files:
+
+imu_data.mat
+baro_data.mat
+gps_data.mat
+mag_data.mat
+
+and optionally
+
+rng_data.mat
+flow_data.mat
+viso_data.mat
+
+Note: If the rangefinder, optical flow or ZED camera odometer data are not present in the log, then the corresponding sections in the convert_apm_data.m script file will need to be commented out.
+
+Copy the generated .mat files into the /EKF_replay/TestData/APM directory.
+
+
+3b) If replaying PX4 data:
+
+Collect data with EK2_REC_RPL = 1
+Convert the .ulg log file to .csv files using the PX4/pyulog python script https://github.com/PX4/pyulog/blob/master/pyulog/ulog2csv.py
+
+Make this directory your current MARLAB working directory and fill these variables with the paths to the CSV files:
+- sensors_file: path to *_sensor_combined_0.csv
+- air_data_file: path to *vehicle_air_data_0.csv
+- magnetometer_file: path to *_vehicle_magetometer_0.csv
+- gps_file: path to *_vehicle_gps_position_0.csv
+
+If a simulation was used, ground truth data can be converted as well:
+- attitude_file: path to *_vehicle_attitude_groundtruth_0.csv
+- localpos_file: path to *_vehicle_local_position_groundtruth_0.csv
+- globalpos_file: path to *_vehicle_global_position_groundtruth_0.csv
+
+The actuator controls can also be converted:
+- actctrl_file: path to *_actuator_controls_0_0.csv
+- actout_file: path to *_actuator_outputs_0.csv
+
+When executing .../EKF_replay/px4_replay_import.m, the CSV files will be automatically loaded and converted using these scripts:
+- .../EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
+  * uses imported data from:
+    + sensors_file
+    + air_data_file
+    + magnetometer_file
+  * generates:
+    + imu_data.mat
+    + baro_data.mat
+    + mag_data.mat
+- .../EKF_replay/Common/convert_px4_vehicle_gps_position_csv
+  * uses imported data from:
+    + gps_file
+  * generates:
+    + gps_data.mat
+- .../EKF_replay/Common/convert_px4_groundtruth_csv_data
+  * uses imported data from:
+    + attitude_file
+    + localpos_file
+    + globalpos_file
+  * generates:
+    + attitude_data.mat
+    + localpos_data.mat
+    + globalpos_data.mat
+- .../EKF_replay/Common/convert_px4_actuators_csv_data
+  * uses imported data from:
+    + actctrl_file
+    + actout_file
+  * generates:
+    + actctrl_data.mat
+    + actout_data.mat
+
+If you have an optical flow and range finder sensor fitted, they must be imported and converted manually:
+
+Import the .csv file containing the optical_flow_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_optical_flow_csv_data.m.
+Import the .csv file containing the distance_sensor_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m.
+This will generate the following data files:
+
+flow_data.mat
+rng_data.mat
+
+
+Finally copy the generated .mat files into the /EKF_replay/TestData/PX4 directory.
+
+
+4) Make ‘…/EKF_replay/Filter’ the working directory.
+
+
+5) Execute ‘SetParameterDefaults’ at the command prompt to load the default filter tuning parameter struct ‘param’ into the workspace. The defaults have been set to provide robust estimation across the entire data set, not optimised for accuracy.
+
+
+6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file. 
+
+Output plots are saved as .png files in the ‘…/EKF_replay/OutputPlots/‘ directory.
+
+Output data is written to the Matlab workspace in the ‘output’ struct.