consume PX4 ecl submodule and preserve all history

2026-05-21 21:55:34 +08:00 · 2021-07-14 07:00:52 -04:00
parent 4a436f1ac9 18dcec43d3
commit 7f2acb6d59
239 changed files with 82326 additions and 22 deletions
@@ -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
@@ -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
@@ -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
@@ -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
@@ -41,6 +41,5 @@ px4_add_module(
 	DEPENDS
 		drivers_magnetometer
 		ecl_geo
-		git_ecl
 		px4_work_queue
 )
@@ -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")

@@ -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
+...
+
@@ -0,0 +1,5 @@
+# EditorConfig https://EditorConfig.org
+root = true
+
+[{Makefile,CMakeLists.txt}]
+indent_style = tab
@@ -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
@@ -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
@@ -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{};
+};
@@ -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()
+
@@ -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
+
@@ -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)
@@ -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;
+};
@@ -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};
+};
@@ -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
@@ -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();
+	}
+}
@@ -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
@@ -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));
+		}
+	}
+}
@@ -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};
+
+};
@@ -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);
+}
@@ -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;
+}
@@ -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;
+}
@@ -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};
+};
@@ -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;
+}
@@ -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
@@ -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;
@@ -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
@@ -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;
+
+   
@@ -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
@@ -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;
@@ -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
@@ -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];
@@ -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);
@@ -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))];
@@ -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));
--- a/Show More
+++ b/Show More