fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 10:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

cmake standalone build

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent eec71d1a10
commit e5952fadaf
173 changed files with 588 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/CMakeLists.txt
+11 -47
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 ECL Development Team. All rights reserved.
# 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
@@ -31,27 +31,7 @@
#
############################################################################
cmake_minimum_required(VERSION 2.8)
project(ECL CXX)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
set(MATRIX_DIR ${CMAKE_SOURCE_DIR}/../matrix)
if(NOT EXISTS "${MATRIX_DIR}/.git")
message(SEND_ERROR "The git submodules are not available. Please run git submodule update --init --recursive")
endif()
include_directories(
..
${MATRIX_DIR}
)
set(SRCS
../geo/geo.cpp
../geo_lookup/geo_mag_declination.cpp
../mathlib/mathlib.cpp
add_library(ecl_EKF
airspeed_fusion.cpp
control.cpp
covariance.cpp
@@ -65,28 +45,12 @@ set(SRCS
sideslip_fusion.cpp
terrain_estimator.cpp
vel_pos_fusion.cpp
)
)
add_definitions(-DPOSIX_SHARED)
add_compile_options(
-pedantic
-std=c++11
add_dependencies(ecl_EKF prebuild_targets)
-Wall
-Werror
-Wno-deprecated-declarations
-Wno-enum-compare
-Wno-unused-local-typedefs
-Wno-unused-parameter
)
# clang tolerate unknown gcc options
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
add_compile_options(-Wno-unknown-warning-option)
endif()
add_library(ecl SHARED ${SRCS})
target_include_directories(ecl_EKF PUBLIC ${ECL_SOURCE_DIR})
target_link_libraries(ecl_EKF PRIVATE ecl_geo_lookup)
# Python bindings & tests
# Use cmake -DPythonTests=1 ../EKF && make pytest
@@ -120,27 +84,27 @@ if(PythonTests)
swig_link_libraries(ecl ${PYTHON_LIBRARIES} ecl)
add_custom_target(pytest
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --verbose
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/tests/pytest/ekf_test --verbose
)
add_dependencies(pytest ${SWIG_MODULE_ecl_REAL_NAME})
add_custom_target(pytest-quick
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --quick --verbose
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/tests/pytest/ekf_test --quick --verbose
)
add_dependencies(pytest-quick ${SWIG_MODULE_ecl_REAL_NAME})
add_custom_target(pytest-benchmark
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --benchmark
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/tests/pytest/ekf_test --benchmark
)
add_dependencies(pytest-benchmark ${SWIG_MODULE_ecl_REAL_NAME})
add_custom_target(pytest-plots
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_SOURCE_DIR}/tests/pytest/ekf_test --plots
env PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/tests/pytest/ekf_test --plots
)
add_dependencies(pytest-plots ${SWIG_MODULE_ecl_REAL_NAME})
add_custom_target(pytest-lint
COMMAND cd ${CMAKE_SOURCE_DIR}/tests/pytest/ && ${CMAKE_SOURCE_DIR}/tests/pytest/lint
COMMAND cd ${CMAKE_CURRENT_SOURCE_DIR}/tests/pytest/ && ${CMAKE_CURRENT_SOURCE_DIR}/tests/pytest/lint
)
add_dependencies(pytest-lint ${SWIG_MODULE_ecl_REAL_NAME})
endif()
EKF/FindEigen3.cmake
-90
View File
@@ -1,90 +0,0 @@
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
#
# This module reads hints about search locations from
# the following enviroment variables:
#
# EIGEN3_ROOT
# EIGEN3_ROOT_DIR
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
HINTS
ENV EIGEN3_ROOT
ENV EIGEN3_ROOT_DIR
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)
EKF/common.h
+1 -1
View File
@@ -40,7 +40,7 @@
*
*/
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
namespace estimator
{
EKF/estimator_interface.h
+1 -1
View File
@@ -43,7 +43,7 @@
#include "RingBuffer.h"
#include <geo/geo.h>
#include <matrix/matrix/math.hpp>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
using namespace estimator;
EKF/matlab/EKF_replay/Common/AlignTilt.m
+20
View File
@@ -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
EKF/matlab/EKF_replay/Common/ConvertToC.m
+220
View File
@@ -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;
EKF/matlab/EKF_replay/Common/ConvertToM.m
+46
View File
@@ -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
EKF/matlab/EKF_replay/Common/EulToQuat.m
+23
View File
@@ -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;
EKF/matlab/EKF_replay/Common/LLH2NED.m
+12
View File
@@ -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
EKF/matlab/EKF_replay/Common/NormQuat.m
+5
View File
@@ -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;
EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m
+29
View File
@@ -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
EKF/matlab/EKF_replay/Common/Quat2Tbn.m
+14
View File
@@ -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];
EKF/matlab/EKF_replay/Common/QuatDivide.m
+16
View File
@@ -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);
EKF/matlab/EKF_replay/Common/QuatMult.m
+5
View File
@@ -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))];
EKF/matlab/EKF_replay/Common/QuatToEul.m
+9
View File
@@ -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));
EKF/matlab/EKF_replay/Common/RotToQuat.m
+10
View File
@@ -0,0 +1,10 @@
% convert froma rotation vector in radians to a quaternion
function quaternion = RotToQuat(rotVec)
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
if vecLength < 1e-6
quaternion = [1;0;0;0];
else
quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
end
EKF/matlab/EKF_replay/Common/convert_apm_data.m
Executable
+103
View File
@@ -0,0 +1,103 @@
%% convert baro data
clear baro_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(BARO)
if (BARO(source_index,2) ~= last_time)
baro_data.time_us(output_index,1) = BARO(source_index,2);
baro_data.height(output_index) = BARO(source_index,3);
last_time = BARO(source_index,2);
output_index = output_index + 1;
end
end
save baro_data.mat baro_data;
%% extract IMU delta angles and velocity data
clear imu_data;
imu_data.time_us = IMT(:,2);
imu_data.gyro_dt = IMT(:,5);
imu_data.del_ang = IMT(:,6:8);
imu_data.accel_dt = IMT(:,4);
imu_data.del_vel = IMT(:,9:11);
save imu_data.mat imu_data;
%% convert magnetomer data
clear mag_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(MAG)
mag_timestamp = MAG(source_index,2);
if (mag_timestamp ~= last_time)
mag_data.time_us(output_index,1) = mag_timestamp;
mag_data.field_ga(output_index,:) = 0.001*[MAG(source_index,3),MAG(source_index,4),MAG(source_index,5)];
last_time = mag_timestamp;
output_index = output_index + 1;
end
end
save mag_data.mat mag_data;
%% save GPS daa
clear gps_data;
maxindex = min(length(GPS),length(GPA));
gps_data.time_us = GPS(1:maxindex,2);
gps_data.pos_error = GPA(1:maxindex,4);
gps_data.spd_error = GPA(1:maxindex,6);
gps_data.hgt_error = GPA(1:maxindex,5);
% set reference point used to set NED origin when GPS accuracy is sufficient
gps_data.start_index = max(find(gps_data.pos_error < 5.0, 1 ),find(gps_data.spd_error < 1.0, 1 ));
gps_data.refLLH = [GPS(gps_data.start_index,8);GPS(gps_data.start_index,9);GPS(gps_data.start_index,10)];
% convert GPS data to NED
deg2rad = pi/180;
for index = 1:1:maxindex
if (GPS(index,3) >= 3)
gps_data.pos_ned(index,:) = LLH2NED([GPS(index,8);GPS(index,9);GPS(index,10)],gps_data.refLLH);
gps_data.vel_ned(index,:) = [GPS(index,11).*cos(deg2rad*GPS(index,12)),GPS(index,11).*sin(deg2rad*GPS(index,12)),GPS(index,13)];
else
gps_data.pos_ned(index,:) = [0,0,0];
gps_data.vel_ned(index,:) = [0,0,0];
end
end
save gps_data.mat gps_data;
%% save range finder data
clear rng_data;
if (exist('RFND','var'))
rng_data.time_us = RFND(:,2);
rng_data.dist = RFND(:,3);
save rng_data.mat rng_data;
end
%% save optical flow data
clear flow_data;
if (exist('OF','var'))
flow_data.time_us = OF(:,2);
flow_data.qual = OF(:,3)/255; % scale quality from 0 to 1
flow_data.flowX = OF(:,4); % optical flow rate about the X body axis (rad/sec)
flow_data.flowY = OF(:,5); % optical flow rate about the Y body axis (rad/sec)
flow_data.bodyX = OF(:,6); % angular rate about the X body axis (rad/sec)
flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec)
save flow_data.mat flow_data;
end
%% save visual odometry data
clear viso_data;
if (exist('VISO','var'))
viso_data.time_us = VISO(:,2);
viso_data.dt = VISO(:,3); % time period the measurement was sampled across (sec)
viso_data.dAngX = VISO(:,4); % delta angle about the X body axis (rad)
viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis (rad)
viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis (rad)
viso_data.dPosX = VISO(:,7); % delta position along the X body axis (m)
viso_data.dPosY = VISO(:,8); % delta position along the Y body axis (m)
viso_data.dPosZ = VISO(:,9); % delta position along the Z body axis (m)
viso_data.qual = VISO(:,10)/100; % quality from 0 - 1
save viso_data.mat viso_data;
end
%% save data and clear workspace
clearvars -except baro_data imu_data mag_data gps_data rng_data flow_data viso_data;
EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m
Executable
+18
View File
@@ -0,0 +1,18 @@
%% convert baro data
clear rng_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(timestamp)
rng_timestamp = timestamp(source_index);
if (rng_timestamp ~= last_time)
last_time = rng_timestamp;
rng_data.time_us(output_index,1) = rng_timestamp;
rng_data.dist(output_index,1) = current_distance(source_index);
output_index = output_index + 1;
end
end
%% save data and clear workspace
clearvars -except rng_data;
save rng_data.mat rng_data;
EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m
Executable
+23
View File
@@ -0,0 +1,23 @@
%% convert baro data
clear flow_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(timestamp)
flow_timestamp = timestamp(source_index);
if ((flow_timestamp ~= last_time) && (integration_timespan(source_index) > 0))
last_time = flow_timestamp;
flow_data.time_us(output_index,1) = flow_timestamp;
flow_data.qual(output_index,1) = quality(source_index)/255; % scale quality from 0 to 1
dt_inv = 1e6 / integration_timespan(source_index);
flow_data.flowX(output_index,1) = dt_inv * pixel_flow_x_integral(source_index); % optical flow rate about the X body axis (rad/sec)
flow_data.flowY(output_index,1) = dt_inv * pixel_flow_y_integral(source_index); % optical flow rate about the Y body axis (rad/sec)
flow_data.bodyX(output_index,1) = dt_inv * gyro_x_rate_integral(source_index); % angular rate about the X body axis (rad/sec)
flow_data.bodyY(output_index,1) = dt_inv * gyro_y_rate_integral(source_index); % angular rate about the Y body axis (rad/sec)
output_index = output_index + 1;
end
end
%% save data and clear workspace
clearvars -except flow_data;
save flow_data.mat flow_data;
EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
Executable
+47
View File
@@ -0,0 +1,47 @@
%% convert baro data
clear baro_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(timestamp)
baro_timestamp = timestamp(source_index) + baro_timestamp_relative(source_index);
if (baro_timestamp ~= last_time)
baro_data.time_us(output_index,1) = baro_timestamp;
baro_data.height(output_index) = baro_alt_meter(source_index);
last_time = baro_timestamp;
output_index = output_index + 1;
end
end
%% convert IMU data to delta angles and velocities
% Note: these quatntis were converted from delta angles and velocites using
% the integral_dt values in the PX4 sensor module so we only need to
% multiply by integral_dt to convert back
clear imu_data;
n_samples = length(timestamp);
imu_data.time_us = timestamp + accelerometer_timestamp_relative;
imu_data.gyro_dt = gyro_integral_dt ./ 1e6;
imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt];
imu_data.accel_dt = accelerometer_integral_dt ./ 1e6;
imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt];
%% convert magnetomer data
clear mag_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(timestamp)
mag_timestamp = timestamp(source_index) + magnetometer_timestamp_relative(source_index);
if (mag_timestamp ~= last_time)
mag_data.time_us(output_index,1) = mag_timestamp;
mag_data.field_ga(output_index,:) = [magnetometer_ga0(source_index),magnetometer_ga1(source_index),magnetometer_ga2(source_index)];
last_time = mag_timestamp;
output_index = output_index + 1;
end
end
%% save data and clear workspace
clearvars -except baro_data imu_data mag_data gps_data;
save baro_data.mat baro_data;
save imu_data.mat imu_data;
save mag_data.mat mag_data;
EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m
Executable
+29
View File
@@ -0,0 +1,29 @@
clear gps_data;
gps_data.time_us = timestamp + timestamp_time_relative;
gps_data.pos_error = eph;
gps_data.spd_error = s_variance_m_s;
gps_data.hgt_error = epv;
% set reference point used to set NED origin when GPS accuracy is sufficient
gps_data.start_index = max(min(find(gps_data.pos_error < 5.0)),min(find(gps_data.spd_error < 1.0)));
if isempty(gps_data.start_index)
gps_data.start_index = 1;
gps_data.refLLH = [1e-7*median(lat);1e-7*median(lon);0.001*median(alt)];
else
gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)];
end
% convert GPS data to NED
for index = 1:length(timestamp)
if (fix_type(index) >= 3)
gps_data.pos_ned(index,:) = LLH2NED([1e-7*lat(index);1e-7*lon(index);0.001*alt(index)],gps_data.refLLH);
gps_data.vel_ned(index,:) = [vel_n_m_s(index),vel_e_m_s(index),vel_d_m_s(index)];
else
gps_data.pos_ned(index,:) = [0,0,0];
gps_data.vel_ned(index,:) = [0,0,0];
end
end
clearvars -except baro_data imu_data mag_data gps_data;
save gps_data.mat;
EKF/matlab/EKF_replay/Filter/AlignHeading.m
+23
View File
@@ -0,0 +1,23 @@
function quat = AlignHeading( ...
quat, ... % quaternion state vector
magMea, ... % body frame magnetic flux measurements
declination) % Estimated magnetic field delination at current location (rad)
% Get the Euler angles and set yaw to zero
euler = QuatToEul(quat);
euler(3) = 0.0;
% calculate the rotation matrix from body to earth frame
quat_zero_yaw = EulToQuat(euler);
Tbn_zero_yaw = Quat2Tbn(quat_zero_yaw);
% Rotate the magnetic field measurement into earth frame
magMeasNED = Tbn_zero_yaw*magMea;
% Use the projection onto the horizontal to calculate the yaw angle
euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1));
% convert to a quaternion
quat = EulToQuat(euler);
end
EKF/matlab/EKF_replay/Filter/ConstrainStates.m
+23
View File
@@ -0,0 +1,23 @@
function [states] = ConstrainStates(states,dt_imu_avg)
% constrain gyro bias states
limit = 5.0*pi/180*dt_imu_avg;
for i=11:13
if (states(i) > limit)
states(i) = limit;
elseif (states(i) < -limit)
states(i) = -limit;
end
end
% constrain accel bias states
limit = 0.5*dt_imu_avg;
for i=14:16
if (states(i) > limit)
states(i) = limit;
elseif (states(i) < -limit)
states(i) = -limit;
end
end
end
EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m
+56
View File
@@ -0,0 +1,56 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation,... % NE position innovations (m)
varInnov] ... % NE position innovation variance (m^2)
= FuseBaroHeight( ...
states, ... % predicted states from the INS
P, ... % predicted covariance
measHgt, ... % NE position measurements (m)
gateSize, ... % Size of the innovation consistency check gate (std-dev)
R_OBS) % position observation variance (m)^2
H = zeros(1,24);
% position states start at index 8
stateIndex = 10;
% Calculate the vertical position height innovation (posD is opposite
% sign to height)
innovation = states(stateIndex) + measHgt;
% Calculate the observation Jacobian
H(stateIndex) = 1;
varInnov = (H*P*transpose(H) + R_OBS);
% Apply an innovation consistency check
if (innovation^2 / (gateSize^2 * varInnov)) > 1.0
return;
end
% Calculate Kalman gains and update states and covariances
% Calculate the Kalman gains
K = (P*transpose(H))/varInnov;
% Calculate state corrections
xk = K * innovation;
% Apply the state corrections
states = states - xk;
% Update the covariance
P = P - K*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
EKF/matlab/EKF_replay/Filter/FuseBodyVel.m
+82
View File
@@ -0,0 +1,82 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation, ... % XY optical flow innovations - rad/sec
varInnov] ... % XY optical flow innovation variances (rad/sec)^2
= FuseBodyVel( ...
states, ... % predicted states
P, ... % predicted covariance
relVelBodyMea, ... % XYZ velocity measured by the camera (m/sec)
obsVar, ... % velocity variances - (m/sec)^2
gateSize) % innovation gate size (SD)
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
vn = states(5);
ve = states(6);
vd = states(7);
innovation = zeros(1,2);
varInnov = zeros(1,2);
H = zeros(2,24);
% Calculate predicted velocity measured in body frame axes
Tbn = Quat2Tbn(states(1:4));
relVelBodyPred = transpose(Tbn)*[vn;ve;vd];
% calculate the observation jacobian, innovation variance and innovation
for obsIndex = 1:3
% Calculate corrections using X component
if (obsIndex == 1)
H(1,:) = calcH_VELX(q0,q1,q2,q3,vd,ve,vn);
elseif (obsIndex == 2)
H(2,:) = calcH_VELY(q0,q1,q2,q3,vd,ve,vn);
elseif (obsIndex == 3)
H(3,:) = calcH_VELZ(q0,q1,q2,q3,vd,ve,vn);
end
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + obsVar);
innovation(obsIndex) = relVelBodyPred(obsIndex) - relVelBodyMea(obsIndex);
end
% check innovations for consistency and exit if they fail the test
for obsIndex = 1:3
if (innovation(obsIndex)^2 / (varInnov(obsIndex) * gateSize^2) > 1.0);
return;
end
end
% calculate the kalman gains and perform the state and covariance update
% using sequential fusion
for obsIndex = 1:3
Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
% correct the state vector
states = states - Kfusion * innovation(obsIndex);
% normalise the updated quaternion states
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
if (quatMag > 1e-12)
states(1:4) = states(1:4) / quatMag;
end
% correct the covariance P = P - K*H*P
P = P - Kfusion*H(obsIndex,:)*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
end
EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m
+47
View File
@@ -0,0 +1,47 @@
function [...
states, ... % state vector after fusion of measurements
P] ... %
= FuseMagDeclination( ...
states, ... % predicted states
P, ... % predicted covariance
measDec) % magnetic field declination - azimuth angle measured from true north (rad)
magN = states(17);
magE = states(18);
R_MAG = 0.5^2;
H = calcH_MAGD(magE,magN);
varInnov = (H*P*transpose(H) + R_MAG);
Kfusion = (P*transpose(H))/varInnov;
% Calculate the predicted magnetic declination
predDec = atan2(magE,magN);
% Calculate the measurement innovation
innovation = predDec - measDec;
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
% correct the state vector
states = states - Kfusion * innovation;
% correct the covariance P = P - K*H*P
P = P - Kfusion*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
EKF/matlab/EKF_replay/Filter/FuseMagHeading.m
+71
View File
@@ -0,0 +1,71 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation, ... % Declination innovation - rad
varInnov] ... %
= FuseMagHeading( ...
states, ... % predicted states
P, ... % predicted covariance
magData, ... % XYZ body frame magnetic flux measurements - gauss
measDec, ... % magnetic field declination - azimuth angle measured from true north (rad)
innovGate, ... % innovation gate size (SD)
R_MAG) % magnetic heading measurement variance - rad^2
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
magX = magData(1);
magY = magData(2);
magZ = magData(3);
H = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3);
varInnov = (H*P*transpose(H) + R_MAG);
Kfusion = (P*transpose(H))/varInnov;
% Calculate the predicted magnetic declination
Tbn = Quat2Tbn(states(1:4));
magMeasNED = Tbn*[magX;magY;magZ];
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Calculate the measurement innovation
innovation = predDec - measDec;
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
% Apply a innovation consistency check
if (innovation^2 / (innovGate^2 * varInnov)) > 1.0
innovation = NaN;
varInnov = NaN;
return;
end
% correct the state vector
states = states - Kfusion * innovation;
% normalise the updated quaternion states
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
if (quatMag > 1e-12)
states(1:4) = states(1:4) / quatMag;
end
% correct the covariance P = P - K*H*P
P = P - Kfusion*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m
+87
View File
@@ -0,0 +1,87 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation, ... % Declination innovation - rad
varInnov] ... %
= FuseMagnetometer( ...
states, ... % predicted states
P, ... % predicted covariance
magMea, ... % body frame magnetic flux measurements
testRatio, ... % Size of magnetometer innovation in standard deviations before measurements are rejected
R_MAG) % magnetoemter measurement variance - gauss^2
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
magXbias = states(20);
magYbias = states(21);
magZbias = states(22);
magN = states(17);
magE = states(18);
magD = states(19);
innovation = zeros(1,3);
varInnov = zeros(1,3);
H = zeros(3,24);
% Calculate the predicted magnetometer measurement
Tbn = Quat2Tbn(states(1:4));
magPred = transpose(Tbn)*[magN;magE;magD] + [magXbias;magYbias;magZbias];
% calculate the observation jacobian, innovation variance and innovation
for obsIndex = 1:3
% Calculate corrections using X component
if (obsIndex == 1)
H(1,:) = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3);
elseif (obsIndex == 2)
H(2,:) = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3);
elseif (obsIndex == 3)
H(3,:) = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3);
end
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_MAG);
innovation(obsIndex) = magPred(obsIndex) - magMea(obsIndex);
end
% check innovations for consistency and exit if they fail the test
for obsIndex = 1:3
if (innovation(obsIndex)^2 / (varInnov(obsIndex) * testRatio^2) > 1.0);
return;
end
end
% calculate the kalman gains and perform the state and covariance update
% using sequential fusion
for obsIndex = 1:3
Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
% correct the state vector
states = states - Kfusion * innovation(obsIndex);
% normalise the updated quaternion states
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
if (quatMag > 1e-12)
states(1:4) = states(1:4) / quatMag;
end
% correct the covariance P = P - K*H*P
P = P - Kfusion*H(obsIndex,:)*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
end
EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m
+88
View File
@@ -0,0 +1,88 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation, ... % XY optical flow innovations - rad/sec
varInnov] ... % XY optical flow innovation variances (rad/sec)^2
= FuseOpticalFlow( ...
states, ... % predicted states
P, ... % predicted covariance
flowRate, ... % XY axis optical flow rate (rad/sec)
bodyRate, ... % XY axis body rate (rad/sec)
range, ... % range from lens to ground measured along the centre of the optical flow sensor field of view
flowObsVar, ... % flow observation variance - (rad/sec)^2
gateSize) % innovation gate size (SD)
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
vn = states(5);
ve = states(6);
vd = states(7);
innovation = zeros(1,2);
varInnov = zeros(1,2);
H = zeros(2,24);
% Calculate predicted angular LOS rates about body frame axes
Tbn = Quat2Tbn(states(1:4));
relVelBody = transpose(Tbn)*[vn;ve;vd];
losRatePred(1) = +relVelBody(2)/range;
losRatePred(2) = -relVelBody(1)/range;
% Calculate measured LOS angular rates using body motion corrected flow
% measurements
losRateMea = - flowRate + bodyRate;
% calculate the observation jacobian, innovation variance and innovation
for obsIndex = 1:2
% Calculate corrections using X component
if (obsIndex == 1)
H(1,:) = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn);
elseif (obsIndex == 2)
H(2,:) = calcH_LOSY(q0,q1,q2,q3,range,vd,ve,vn);
end
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + flowObsVar);
innovation(obsIndex) = losRatePred(obsIndex) - losRateMea(obsIndex);
end
% check innovations for consistency and exit if they fail the test
for obsIndex = 1:2
if (innovation(obsIndex)^2 / (varInnov(obsIndex) * gateSize^2) > 1.0);
return;
end
end
% calculate the kalman gains and perform the state and covariance update
% using sequential fusion
for obsIndex = 1:2
Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
% correct the state vector
states = states - Kfusion * innovation(obsIndex);
% normalise the updated quaternion states
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
if (quatMag > 1e-12)
states(1:4) = states(1:4) / quatMag;
end
% correct the covariance P = P - K*H*P
P = P - Kfusion*H(obsIndex,:)*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
end
EKF/matlab/EKF_replay/Filter/FusePosition.m
+68
View File
@@ -0,0 +1,68 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation,... % NE position innovations (m)
varInnov] ... % NE position innovation variance (m^2)
= FusePosition( ...
states, ... % predicted states from the INS
P, ... % predicted covariance
measPos, ... % NE position measurements (m)
gateSize, ... % Size of the innovation consistency check gate (std-dev)
R_OBS) % position observation variance (m)^2
innovation = zeros(1,2);
varInnov = zeros(1,2);
H = zeros(2,24);
for obsIndex = 1:2
% velocity states start at index 8
stateIndex = 7 + obsIndex;
% Calculate the velocity measurement innovation
innovation(obsIndex) = states(stateIndex) - measPos(obsIndex);
% Calculate the observation Jacobian
H(obsIndex,stateIndex) = 1;
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
end
% Apply an innovation consistency check
for obsIndex = 1:2
if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
return;
end
end
% Calculate Kalman gains and update states and covariances
for obsIndex = 1:2
% Calculate the Kalman gains
K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
% Calculate state corrections
xk = K * innovation(obsIndex);
% Apply the state corrections
states = states - xk;
% Update the covariance
P = P - K*H(obsIndex,:)*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
end
EKF/matlab/EKF_replay/Filter/FuseVelocity.m
+68
View File
@@ -0,0 +1,68 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation,... % NED velocity innovations (m/s)
varInnov] ... % NED velocity innovation variance ((m/s)^2)
= FuseVelocity( ...
states, ... % predicted states from the INS
P, ... % predicted covariance
measVel, ... % NED velocity measurements (m/s)
gateSize, ... % Size of the innovation consistency check gate (std-dev)
R_OBS) % velocity observation variance (m/s)^2
innovation = zeros(1,3);
varInnov = zeros(1,3);
H = zeros(3,24);
for obsIndex = 1:3
% velocity states start at index 5
stateIndex = 4 + obsIndex;
% Calculate the velocity measurement innovation
innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
% Calculate the observation Jacobian
H(obsIndex,stateIndex) = 1;
varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
end
% Apply an innovation consistency check
for obsIndex = 1:3
if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
return;
end
end
% Calculate Kalman gains and update states and covariances
for obsIndex = 1:3
% Calculate the Kalman gains
K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);
% Calculate state corrections
xk = K * innovation(obsIndex);
% Apply the state corrections
states = states - xk;
% Update the covariance
P = P - K*H(obsIndex,:)*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
end
EKF/matlab/EKF_replay/Filter/GenerateEquations24.m
Executable
+211
View File
@@ -0,0 +1,211 @@
%% define symbolic variables and constants
clear all;
reset(symengine);
syms dax day daz real % IMU delta angle measurements in body axes - rad
syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
syms vn ve vd real % NED velocity - m/sec
syms pn pe pd real % NED position - m
syms dax_b day_b daz_b real % delta angle bias - rad
syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
syms dt real % IMU time step - sec
syms gravity real % gravity - m/sec^2
syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
syms vwn vwe real; % NE wind velocity - m/sec
syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss
syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
syms R_MAG real % variance for magnetic flux measurements - milligauss^2
%% define the state prediction equations
% define the measured Delta angle and delta velocity vectors
dAngMeas = [dax; day; daz];
dVelMeas = [dvx; dvy; dvz];
% define the IMU bias errors and scale factor
dAngBias = [dax_b; day_b; daz_b];
dVelBias = [dvx_b; dvy_b; dvz_b];
% define the quaternion rotation vector for the state estimate
quat = [q0;q1;q2;q3];
% derive the truth body to nav direction cosine matrix
Tbn = Quat2Tbn(quat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas - dAngBias;
% Define the truth delta velocity -ignore sculling and transport rate
% corrections as these negligible are in terms of covariance growth for our
% application and grade of sensor
dVelTruth = dVelMeas - dVelBias;
% define the attitude update equations
% use a first order expansion of rotation to calculate the quaternion increment
% acceptable for propagation of covariances
deltaQuat = [1;
0.5*dAngTruth(1);
0.5*dAngTruth(2);
0.5*dAngTruth(3);
];
quatNew = QuatMult(quat,deltaQuat);
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
% define the position update equations
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
% define the IMU error update equations
dAngBiasNew = dAngBias;
dVelBiasNew = dVelBias;
% define the wind velocity update equations
vwnNew = vwn;
vweNew = vwe;
% define the earth magnetic field update equations
magNnew = magN;
magEnew = magE;
magDnew = magD;
% define the body magnetic field update equations
magXnew = magX;
magYnew = magY;
magZnew = magZ;
% Define the state vector & number of states
stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
nStates=numel(stateVector);
% Define vector of process equations
stateVectorNew = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
%% derive the state transition and state error matrix
% Define the control (disturbance) vector. Error growth in the inertial
% solution is assumed to be driven by 'noise' in the delta angles and
% velocities, after bias effects have been removed. This is OK becasue we
% have sensor bias accounted for in the state equations.
distVector = [daxVar;dayVar;dazVar;dvxVar;dvyVar;dvzVar];
% derive the control(disturbance) influence matrix
G = jacobian(stateVectorNew, [dAngMeas;dVelMeas]);
% derive the state error matrix
distMatrix = diag(distVector);
Q = G*distMatrix*transpose(G);
f = matlabFunction(Q,'file','calcQ24.m');
% derive the state transition matrix
F = jacobian(stateVectorNew, stateVector);
f = matlabFunction(F,'file','calcF24.m');
%% derive equations for fusion of magnetometer measurements
% rotate earth field into body axes
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ];
magMeasX = magMeas(1);
H_MAGX = jacobian(magMeasX,stateVector); % measurement Jacobian
f = matlabFunction(H_MAGX,'file','calcH_MAGX.m');
magMeasY = magMeas(2);
H_MAGY = jacobian(magMeasY,stateVector); % measurement Jacobian
f = matlabFunction(H_MAGY,'file','calcH_MAGY.m');
magMeasZ = magMeas(3);
H_MAGZ = jacobian(magMeasZ,stateVector); % measurement Jacobian
f = matlabFunction(H_MAGZ,'file','calcH_MAGZ.m');
%% derive equations for fusion of synthetic deviation measurement
% used to keep correct heading when operating without absolute position or
% velocity measurements - eg when using optical flow
% rotate magnetic field into earth axes
magMeasNED = [magN;magE;magD];
% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = simplify(H_MAGD);
f = matlabFunction(H_MAGD,'file','calcH_MAGD.m');
%% derive equations for fusion of a single magneic compass heading measurement
% rotate body measured field into earth axes
magMeasNED = Tbn*[magX;magY;magZ];
% the predicted measurement is the angle wrt true north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian
f = matlabFunction(H_MAG,'file','calcH_HDG.m');
%% derive equations for sequential fusion of optical flow measurements
% range is defined as distance from camera focal point to centre of sensor fov
syms range real;
% calculate relative velocity in body frame
relVelBody = transpose(Tbn)*[vn;ve;vd];
% divide by range to get predicted angular LOS rates relative to X and Y
% axes. Note these are body angular rate motion compensated optical flow rates
losRateX = +relVelBody(2)/range;
losRateY = -relVelBody(1)/range;
% calculate the observation Jacobian for the X axis
H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian
H_LOSX = simplify(H_LOSX);
f = matlabFunction(H_LOSX,'file','calcH_LOSX.m');
% calculate the observation Jacobian for the Y axis
H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian
H_LOSY = simplify(H_LOSY);
f = matlabFunction(H_LOSY,'file','calcH_LOSY.m');
%% derive equations for sequential fusion of body frame velocity measurements
% body frame velocity observations
syms velX velY velZ real;
% velocity observation variance
syms R_VEL real;
% calculate relative velocity in body frame
relVelBody = transpose(Tbn)*[vn;ve;vd];
% calculate the observation Jacobian for the X axis
H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
H_VELX = simplify(H_VELX);
f = matlabFunction(H_VELX,'file','calcH_VELX.m');
% calculate the observation Jacobian for the Y axis
H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
H_VELY = simplify(H_VELY);
f = matlabFunction(H_VELY,'file','calcH_VELY.m');
% calculate the observation Jacobian for the Z axis
H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
H_VELZ = simplify(H_VELZ);
f = matlabFunction(H_VELZ,'file','calcH_VELZ.m');
%% calculate error transfer matrix for declination error estimate
declination = atan(magE/magN);
T_MAG = jacobian(declination,[magN,magE]);
f = matlabFunction(T_MAG,'file','transfer_matrix.m');
%% calculate Quaternion to Euler angle error transfer matrix
% quat = [q0;q1;q2;q3];
% syms roll pitch yaw 'real';
roll = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)) , (quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4)));
pitch = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
yaw = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)) , (quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4)));
euler = [roll;pitch;yaw];
error_transfer_matrix = jacobian(euler,quat);
matlabFunction(error_transfer_matrix,'file','quat_to_euler_error_transfer_matrix.m');
EKF/matlab/EKF_replay/Filter/InitCovariance.m
+36
View File
@@ -0,0 +1,36 @@
function covariance = InitCovariance(param,dt,gps_alignment,gps_data)
% Define quaternion state errors
Sigma_quat = param.alignment.quatErr * [1;1;1;1];
% Define velocity state errors
if (gps_alignment == 1)
Sigma_velocity = gps_data.spd_error(gps_data.start_index) * [1;1;1];
else
Sigma_velocity = [param.alignment.velErrNE;param.alignment.velErrNE;param.alignment.velErrD];
end
% Define position state errors
if (gps_alignment == 1)
Sigma_position = gps_data.pos_error(gps_data.start_index) * [1;1;0] + [0;0;param.alignment.hgtErr];
else
Sigma_position = [param.alignment.posErrNE;param.alignment.posErrNE;param.alignment.hgtErr];
end
% Define delta angle bias state errors
Sigma_dAngBias = param.alignment.delAngBiasErr*dt*[1;1;1];
% Define delta velocity bias state errors
Sigma_dVelBias = param.alignment.delVelBiasErr*dt*[1;1;1];
% Define magnetic field state errors
Sigma_magNED = [param.alignment.magErrNED;param.alignment.magErrNED;param.alignment.magErrNED]; % 1 Sigma uncertainty in initial NED mag field
Sigma_magXYZ = [param.alignment.magErrXYZ;param.alignment.magErrXYZ;param.alignment.magErrXYZ]; % 1 Sigma uncertainty in initial XYZ mag sensor offset
% Define wind velocity state errors
Sigma_wind = param.alignment.windErrNE * [1;1];
% Convert to variances and write to covariance matrix diagonals
covariance = diag([Sigma_quat;Sigma_velocity;Sigma_position;Sigma_dAngBias;Sigma_dVelBias;Sigma_magNED;Sigma_magXYZ;Sigma_wind].^2);
end
EKF/matlab/EKF_replay/Filter/InitStates.m
+66
View File
@@ -0,0 +1,66 @@
function [states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data)
% constants
deg2rad = pi/180;
% initialise the state vector and quaternion
states = zeros(24,1);
quat = [1;0;0;0];
if (param.control.waitForGps == 1)
% find IMU start index that coresponds to first valid GPS data
imu_start_index = (find(imu_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 50);
imu_start_index = max(imu_start_index,1);
else
imu_start_index = 1;
end
% average first 100 accel readings to reduce effect of vibration
initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
initAccel(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
% align tilt using gravity vector (If the velocity is changing this will
% induce errors)
quat = AlignTilt(quat,initAccel);
states(1:4) = quat;
% add a roll, pitch, yaw mislignment
quat_align_err = EulToQuat([param.control.rollAlignErr,param.control.pitchAlignErr,param.control.yawAlignErr]);
quat = QuatMult(quat,quat_align_err);
% find magnetometer start index
mag_start_index = (find(mag_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 5);
mag_start_index = max(mag_start_index,1);
% mean to reduce effect of noise in data
magBody(1,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,1));
magBody(2,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,2));
magBody(3,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,3));
% align heading and initialise the NED magnetic field states
quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad);
states(1:4) = quat;
% initialise the NED magnetic field states
Tbn = Quat2Tbn(quat);
states(17:19) = Tbn*magBody;
if (param.control.waitForGps == 1)
% initialise velocity and position using gps
states(5:7) = gps_data.vel_ned(gps_data.start_index,:);
states(8:9) = gps_data.pos_ned(gps_data.start_index,1:2);
else
% initialise to be stationary at the origin
states(5:7) = zeros(1,3);
states(8:9) = zeros(1,2);
end
% find baro start index
baro_start_index = (find(baro_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 10);
baro_start_index = max(baro_start_index,1);
% average baro data and initialise the vertical position
states(10) = -mean(baro_data.height(baro_start_index:baro_start_index+20));
end
EKF/matlab/EKF_replay/Filter/PlotData.m
Executable
+511
View File
@@ -0,0 +1,511 @@
function PlotData(output,folder,runIdentifier)
rad2deg = 180/pi;
if ~exist(folder,'dir')
mkdir(folder);
end
plotDimensions = [0 0 210*3 297*3];
%% plot Euler angle estimates
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
margin = 5;
subplot(3,1,1);
plot(output.time_lapsed,[output.euler_angles(:,1)*rad2deg,output.euler_angles(:,1)*rad2deg-2*sqrt(output.euler_variances(:,1)*rad2deg),output.euler_angles(:,1)*rad2deg+2*sqrt(output.euler_variances(:,1)*rad2deg)]);
minVal = rad2deg*min(output.euler_angles(:,1))-margin;
maxVal = rad2deg*max(output.euler_angles(:,1))+margin;
ylim([minVal maxVal]);
grid on;
titleText=strcat({'Euler Angle Estimates'},runIdentifier);
title(titleText);
ylabel('Roll (deg)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed,[output.euler_angles(:,2)*rad2deg,output.euler_angles(:,2)*rad2deg-2*sqrt(output.euler_variances(:,2)*rad2deg),output.euler_angles(:,2)*rad2deg+2*sqrt(output.euler_variances(:,2)*rad2deg)]);
minVal = rad2deg*min(output.euler_angles(:,2))-margin;
maxVal = rad2deg*max(output.euler_angles(:,2))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Pitch (deg)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed,[output.euler_angles(:,3)*rad2deg,output.euler_angles(:,3)*rad2deg-2*sqrt(output.euler_variances(:,3)*rad2deg),output.euler_angles(:,3)*rad2deg+2*sqrt(output.euler_variances(:,3)*rad2deg)]);
minVal = rad2deg*min(output.euler_angles(:,3))-margin;
maxVal = rad2deg*max(output.euler_angles(:,3))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Yaw (deg)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='euler_angle_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
%% plot NED velocity estimates
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(3,1,1);
plot(output.time_lapsed,[output.velocity_NED(:,1),output.velocity_NED(:,1)+2*sqrt(output.state_variances(:,5)),output.velocity_NED(:,1)-2*sqrt(output.state_variances(:,5))]);
grid on;
titleText=strcat({'NED Velocity Estimates'},runIdentifier);
title(titleText);
ylabel('North (m/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed,[output.velocity_NED(:,2),output.velocity_NED(:,2)+2*sqrt(output.state_variances(:,6)),output.velocity_NED(:,2)-2*sqrt(output.state_variances(:,6))]);
grid on;
ylabel('East (m/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed,[output.velocity_NED(:,3),output.velocity_NED(:,3)+2*sqrt(output.state_variances(:,7)),output.velocity_NED(:,3)-2*sqrt(output.state_variances(:,7))]);
grid on;
ylabel('Down (m/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='velocity_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
%% plot NED position estimates
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(3,1,1);
plot(output.time_lapsed,[output.position_NED(:,1),output.position_NED(:,1)+2*sqrt(output.state_variances(:,8)),output.position_NED(:,1)-2*sqrt(output.state_variances(:,8))]);
grid on;
titleText=strcat({'NED Position Estimates'},runIdentifier);
title(titleText);
ylabel('North (m)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed,[output.position_NED(:,2),output.position_NED(:,2)+2*sqrt(output.state_variances(:,9)),output.position_NED(:,2)-2*sqrt(output.state_variances(:,9))]);
grid on;
ylabel('East (m)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed,[output.position_NED(:,3),output.position_NED(:,3)+2*sqrt(output.state_variances(:,10)),output.position_NED(:,3)-2*sqrt(output.state_variances(:,10))]);
grid on;
ylabel('Down (m)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='position_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
%% plot IMU gyro bias estimates
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
margin = 0.1;
subplot(3,1,1);
plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,1),output.gyro_bias(:,1)+2*sqrt(output.state_variances(:,11)),output.gyro_bias(:,1)-2*sqrt(output.state_variances(:,11))]*rad2deg);%%output.gyro_bias(:,1)*rad2deg);
minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,1))-margin;
maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,1))+margin;
ylim([minVal maxVal]);
grid on;
titleText=strcat({'IMU Gyro Bias Estimates'},runIdentifier);
title(titleText);
ylabel('X gyro (deg/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,2),output.gyro_bias(:,2)+2*sqrt(output.state_variances(:,12)),output.gyro_bias(:,2)-2*sqrt(output.state_variances(:,12))]*rad2deg);
minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,2))-margin;
maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,2))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Y gyro (deg/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,3),output.gyro_bias(:,3)+2*sqrt(output.state_variances(:,13)),output.gyro_bias(:,3)-2*sqrt(output.state_variances(:,13))]*rad2deg);
minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,3))-margin;
maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,3))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Z gyro (deg/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='imu_gyro_bias_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
%% plot IMU accel bias estimates
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
margin = 0.1;
subplot(3,1,1);
plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,1),output.accel_bias(:,1)+2*sqrt(output.state_variances(:,14)),output.accel_bias(:,1)-2*sqrt(output.state_variances(:,14))]);
titleText=strcat({'IMU Accel Bias Estimates'},runIdentifier);
title(titleText);
minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin;
maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('X accel (m/s/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,2),output.accel_bias(:,2)+2*sqrt(output.state_variances(:,15)),output.accel_bias(:,2)-2*sqrt(output.state_variances(:,15))]);
minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin;
maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Y accel (m/s/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,3),output.accel_bias(:,3)+2*sqrt(output.state_variances(:,16)),output.accel_bias(:,3)-2*sqrt(output.state_variances(:,16))]);
minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin;
maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Z accel (m/s/s)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='imu_accel_bias_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
%% plot magnetometer bias estimates
if (output.magFuseMethod <= 1)
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(3,1,1);
plot(output.time_lapsed',[output.mag_XYZ(:,1),output.mag_XYZ(:,1)+2*sqrt(output.state_variances(:,20)),output.mag_XYZ(:,1)-2*sqrt(output.state_variances(:,20))]);
grid on;
titleText=strcat({'Magnetometer Bias Estimates'},runIdentifier);
title(titleText);
ylabel('X bias (gauss)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed',[output.mag_XYZ(:,2),output.mag_XYZ(:,2)+2*sqrt(output.state_variances(:,21)),output.mag_XYZ(:,2)-2*sqrt(output.state_variances(:,21))]);
grid on;
ylabel('Y bias (gauss)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed',[output.mag_XYZ(:,3),output.mag_XYZ(:,3)+2*sqrt(output.state_variances(:,22)),output.mag_XYZ(:,3)-2*sqrt(output.state_variances(:,22))]);
grid on;
ylabel('Z bias (gauss)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='body_field_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot earth field estimates
if (output.magFuseMethod <= 1)
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
margin = 0.1;
subplot(4,1,1);
plot(output.time_lapsed',[output.mag_NED(:,1),output.mag_NED(:,1)+2*sqrt(output.state_variances(:,17)),output.mag_NED(:,1)-2*sqrt(output.state_variances(:,17))]);
minVal = min(output.mag_NED(:,1))-margin;
maxVal = max(output.mag_NED(:,1))+margin;
ylim([minVal maxVal]);
grid on;
titleText=strcat({'Earth Magnetic Field Estimates'},runIdentifier);
title(titleText);
ylabel('North (gauss)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(4,1,2);
plot(output.time_lapsed',[output.mag_NED(:,2),output.mag_NED(:,2)+2*sqrt(output.state_variances(:,18)),output.mag_NED(:,2)-2*sqrt(output.state_variances(:,18))]);
minVal = min(output.mag_NED(:,2))-margin;
maxVal = max(output.mag_NED(:,2))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('East (gauss)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(4,1,3);
plot(output.time_lapsed',[output.mag_NED(:,3),output.mag_NED(:,3)+2*sqrt(output.state_variances(:,19)),output.mag_NED(:,3)-2*sqrt(output.state_variances(:,19))]);
grid on;
minVal = min(output.mag_NED(:,3))-margin;
maxVal = max(output.mag_NED(:,3))+margin;
ylim([minVal maxVal]);
ylabel('Down (gauss)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(4,1,4);
plot(output.time_lapsed',rad2deg*atan2(output.mag_NED(:,2),output.mag_NED(:,1)));
grid on;
titleText=strcat({'Magnetic Declination Estimate'},runIdentifier);
title(titleText);
ylabel('declination (deg)');
xlabel('time (sec)');
fileName='earth_field_estimates.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot velocity innovations
if isfield(output.innovations,'vel_innov')
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(3,1,1);
plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]);
grid on;
titleText=strcat({'Velocity Innovations and Variances'},runIdentifier);
title(titleText);
ylabel('North (m/s)');
xlabel('time (sec)');
legend('innovation','variance sqrt','variance sqrt');
subplot(3,1,2);
plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]);
grid on;
ylabel('East (m/s)');
xlabel('time (sec)');
legend('innovation','variance sqrt','variance sqrt');
subplot(3,1,3);
plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]);
grid on;
ylabel('Down (m/s)');
xlabel('time (sec)');
legend('innovation','variance sqrt','variance sqrt');
fileName='velocity_fusion.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot position innovations
if isfield(output.innovations,'posInnov')
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(3,1,1);
plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]);
grid on;
titleText=strcat({'Position Innovations and Variances'},runIdentifier);
title(titleText);
ylabel('North (m)');
xlabel('time (sec)');
legend('innovation','variance sqrt','variance sqrt');
subplot(3,1,2);
plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]);
grid on;
ylabel('East (m)');
xlabel('time (sec)');
legend('innovation','variance sqrt','variance sqrt');
subplot(3,1,3);
plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]);
grid on;
ylabel('Up (m)');
xlabel('time (sec)');
legend('innovation','variance sqrt','variance sqrt');
fileName='position_fusion.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot magnetometer innovations
if isfield(output.innovations,'magInnov')
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(4,1,1);
plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,1)';sqrt(output.innovations.magInnovVar(:,1))';-sqrt(output.innovations.magInnovVar(:,1))']);
ylim([-0.15 0.15]);
grid on;
title(strcat({'Magnetometer Innovations and Variances'},runIdentifier));
ylabel('X (gauss)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(4,1,2);
plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,2)';sqrt(output.innovations.magInnovVar(:,2))';-sqrt(output.innovations.magInnovVar(:,2))']);
ylim([-0.15 0.15]);
grid on;
ylabel('Y (gauss)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(4,1,3);
plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,3)';sqrt(output.innovations.magInnovVar(:,3))';-sqrt(output.innovations.magInnovVar(:,3))']);
ylim([-0.15 0.15]);
grid on;
ylabel('Z (gauss)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(4,1,4);
plot(output.innovations.mag_time_lapsed,output.innovations.magLength);
ylim([0 0.7]);
grid on;
title(strcat({'Magnetic Flux'},runIdentifier));
ylabel('Flux (Gauss)');
xlabel('time (sec)');
fileName='magnetometer_fusion.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot magnetic yaw innovations
if isfield(output.innovations,'hdgInnov')
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(2,1,1);
plot(output.innovations.mag_time_lapsed,[output.innovations.hdgInnov*rad2deg;sqrt(output.innovations.hdgInnovVar)*rad2deg;-sqrt(output.innovations.hdgInnovVar)*rad2deg]);
ylim([-30 30]);
grid on;
title(strcat({'Magnetic Heading Innovations and Variances'},runIdentifier));
ylabel('yaw innovation (deg)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(2,1,2);
plot(output.innovations.mag_time_lapsed,output.innovations.magLength);
ylim([0 0.7]);
grid on;
title(strcat({'Magnetic Flux'},runIdentifier));
ylabel('Flux (Gauss)');
xlabel('time (sec)');
fileName='magnetometer_fusion.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot optical flow innovations
if isfield(output.innovations,'flowInnov')
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(2,1,1);
plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,1)';sqrt(output.innovations.flowInnovVar(:,1))';-sqrt(output.innovations.flowInnovVar(:,1))']);
ylim([-1.0 1.0]);
grid on;
title(strcat({'Optical Flow Innovations and Variances'},runIdentifier));
ylabel('X (rad/sec)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(2,1,2);
plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,2)';sqrt(output.innovations.flowInnovVar(:,2))';-sqrt(output.innovations.flowInnovVar(:,2))']);
ylim([-1.0 1.0]);
grid on;
ylabel('Y (rad/sec)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
fileName='optical_flow_fusion.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
%% plot ZED camera innovations
if isfield(output.innovations,'bodyVelInnov')
figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait');
h=gcf;
set(h,'PaperOrientation','portrait');
set(h,'PaperUnits','normalized');
set(h,'PaperPosition', [0 0 1 1]);
subplot(3,1,1);
plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,1)';sqrt(output.innovations.bodyVelInnovVar(:,1))';-sqrt(output.innovations.bodyVelInnovVar(:,1))']);
grid on;
title(strcat({'ZED Camera Innovations and Variances'},runIdentifier));
ylabel('X (m/sec)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(3,1,2);
plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,2)';sqrt(output.innovations.bodyVelInnovVar(:,2))';-sqrt(output.innovations.bodyVelInnovVar(:,2))']);
grid on;
ylabel('Y (m/sec)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
subplot(3,1,3);
plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,3)';sqrt(output.innovations.bodyVelInnovVar(:,3))';-sqrt(output.innovations.bodyVelInnovVar(:,3))']);
grid on;
ylabel('Z (m/sec)');
xlabel('time (sec)');
legend('innovation','innovation variance sqrt','innovation variance sqrt');
fileName='zed_camera_fusion.png';
fullFileName = fullfile(folder, fileName);
saveas(h,fullFileName);
end
EKF/matlab/EKF_replay/Filter/PredictCovariance.m
+66
View File
@@ -0,0 +1,66 @@
function P = PredictCovariance(deltaAngle, ...
deltaVelocity, ...
states,...
P, ... % Previous state covariance matrix
dt, ... % IMU and prediction time step
param) % tuning parameters
% Set filter state process noise other than IMU errors, which are already
% built into the derived covariance predition equations.
% This process noise determines the rate of estimation of IMU bias errors
dAngBiasSigma = dt*dt*param.prediction.dAngBiasPnoise;
dVelBiasSigma = dt*dt*param.prediction.dVelBiasPnoise;
magSigmaNED = dt*param.prediction.magPnoiseNED;
magSigmaXYZ = dt*param.prediction.magPnoiseXYZ;
processNoiseVariance = [zeros(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1], magSigmaNED*[1 1 1], magSigmaXYZ*[1 1 1], [0 0]].^2;
% Specify the noise variance on the IMU delta angles and delta velocities
daxVar = (dt*param.prediction.angRateNoise)^2;
dayVar = daxVar;
dazVar = daxVar;
dvxVar = (dt*param.prediction.accelNoise)^2;
dvyVar = dvxVar;
dvzVar = dvxVar;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
dax_b = states(11);
day_b = states(12);
daz_b = states(13);
dvx_b = states(14);
dvy_b = states(15);
dvz_b = states(16);
% Predicted covariance
F = calcF24(dax,dax_b,day,day_b,daz,daz_b,dt,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3);
Q = calcQ24(daxVar,dayVar,dazVar,dvxVar,dvyVar,dvzVar,q0,q1,q2,q3);
P = F*P*transpose(F) + Q;
% Add the general process noise variance
for i = 1:24
P(i,i) = P(i,i) + processNoiseVariance(i);
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:24
if P(i,i) < 0
P(i,i) = 0;
end
end
end
EKF/matlab/EKF_replay/Filter/PredictStates.m
Executable
+85
View File
@@ -0,0 +1,85 @@
function [states, correctedDelAng, correctedDelVel] = PredictStates( ...
states, ... % previous state vector (4x1 quaternion, 3x1 velocity, 3x1 position, 3x1 delAng bias, 3x1 delVel bias)
delAng, ... % IMU delta angle measurements, 3x1 (rad)
delVel, ... % IMU delta velocity measurements 3x1 (m/s)
dt, ... % accumulation time of the IMU measurement (sec)
gravity, ... % acceleration due to gravity (m/s/s)
latitude) % WGS-84 latitude (rad)
% define persistent variables for previous delta angle and velocity which
% are required for sculling and coning error corrections
persistent prevDelAng;
if isempty(prevDelAng)
prevDelAng = delAng;
end
persistent prevDelVel;
if isempty(prevDelVel)
prevDelVel = delVel;
end
persistent Tbn_prev;
if isempty(Tbn_prev)
Tbn_prev = Quat2Tbn(states(1:4));
end
% Remove sensor bias errors
delAng = delAng - states(11:13);
delVel = delVel - states(14:16);
% Correct delta velocity for rotation and skulling
% Derived from Eqn 25 of:
% "Computational Elements For Strapdown Systems"
% Savage, P.G.
% Strapdown Associates
% 2015, WBN-14010
% correctedDelVel= delVel + ...
% 0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + 1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng));
correctedDelVel= delVel;
% Calculate earth delta angle spin vector
delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt;
delAngEarth_NED(2,1) = 0.0;
delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt;
% Apply corrections for coning errors and earth spin rate
% Coning correction from :
% "A new strapdown attitude algorithm",
% R. B. MILLER,
% Journal of Guidance, Control, and Dynamics
% July, Vol. 6, No. 4, pp. 287-291, Eqn 11
% correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
correctedDelAng = delAng - transpose(Tbn_prev)*delAngEarth_NED;
% Save current measurements
prevDelAng = delAng;
prevDelVel = delVel;
% Convert the rotation vector to its equivalent quaternion
deltaQuat = RotToQuat(correctedDelAng);
% Update the quaternions by rotating from the previous attitude through
% the delta angle rotation quaternion
states(1:4) = QuatMult(states(1:4),deltaQuat);
% Normalise the quaternions
states(1:4) = NormQuat(states(1:4));
% Calculate the body to nav cosine matrix
Tbn = Quat2Tbn(states(1:4));
Tbn_prev = Tbn;
% transform body delta velocities to delta velocities in the nav frame
delVelNav = Tbn * correctedDelVel + [0;0;gravity]*dt;
% take a copy of the previous velocity
prevVel = states(5:7);
% Sum delta velocities to get the velocity
states(5:7) = states(5:7) + delVelNav(1:3);
% integrate the velocity vrctor to get the position using trapezoidal
% integration
states(8:10) = states(8:10) + 0.5 * dt * (prevVel + states(5:7));
end
EKF/matlab/EKF_replay/Filter/RunFilter.m
Executable
+347
View File
@@ -0,0 +1,347 @@
function output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,varargin)
% compulsory inputs
% param : parameters defined by SetParameterDefaults.m
% imu_data : IMU delta angle and velocity data in body frame
% mag_data : corrected magnetometer field measurements in body frame
% baro_data : barometric height measurements
% gps_data : GPS NED pos vel measurements in local earth frame
% optional inputs
% rng _data : measurements for a Z body axis aligned range finder
% flow_data : XY axis optical flow angular rate measurements in body frame
% viso_data : ZED camera visula odometry measurements
nVarargs = length(varargin);
if nVarargs >= 2
flowDataPresent = ~isempty(varargin{1}) && ~isempty(varargin{2});
rng_data = varargin{1};
flow_data = varargin{2};
if flowDataPresent
fprintf('Using optical Flow Data\n',nVarargs);
end
else
flowDataPresent = 0;
end
if nVarargs >= 3
visoDataPresent = ~isempty(varargin{3});
viso_data = varargin{3};
if visoDataPresent
fprintf('Using ZED camera odometry data\n',nVarargs);
end
else
visoDataPresent = 0;
end
%% Set initial conditions
% constants
deg2rad = pi/180;
gravity = 9.80665; % initial value of gravity - will be updated when WGS-84 position is known
% initialise the state vector
[states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data);
dt_imu_avg = 0.5 * (median(imu_data.gyro_dt) + median(imu_data.accel_dt));
indexStop = length(imu_data.time_us) - imu_start_index;
indexStart = 1;
% create static structs for output data
output = struct('time_lapsed',[]',...
'euler_angles',[],...
'velocity_NED',[],...
'position_NED',[],...
'gyro_bias',[],...
'accel_bias',[],...
'mag_NED',[],...
'mag_XYZ',[],...
'wind_NE',[],...
'dt',0,...
'state_variance',[],...
'innovations',[],...
'magFuseMethod',[]);
% initialise the state covariance matrix
covariance = InitCovariance(param,dt_imu_avg,1,gps_data);
%% Main Loop
% control flags
gps_use_started = boolean(false);
gps_use_blocked = boolean(false);
viso_use_blocked = boolean(false);
flow_use_blocked = boolean(false);
% array access index variables
imuIndex = imu_start_index;
last_gps_index = 0;
gps_fuse_index = 0;
last_baro_index = 0;
baro_fuse_index = 0;
last_mag_index = 0;
mag_fuse_index = 0;
last_flow_index = 0;
flow_fuse_index = 0;
last_viso_index = 0;
viso_fuse_index = 0;
last_range_index = 0;
% covariance prediction variables
delAngCov = [0;0;0]; % delta angle vector used by the covariance prediction (rad)
delVelCov = [0;0;0]; % delta velocity vector used by the covariance prediction (m/sec)
dtCov = 0; % time step used by the covariance prediction (sec)
dtCovInt = 0; % accumulated time step of covariance predictions (sec)
covIndex = 0; % covariance prediction frame counter
output.magFuseMethod = param.fusion.magFuseMethod;
range = 0.1;
% variables used to control dead-reckoning timeout
last_drift_constrain_time = - param.control.velDriftTimeLim;
last_synthetic_velocity_fusion_time = 0;
last_valid_range_time = - param.fusion.rngTimeout;
for index = indexStart:indexStop
% read IMU measurements
local_time=imu_data.time_us(imuIndex)*1e-6;
delta_angle(:,1) = imu_data.del_ang(imuIndex,:);
delta_velocity(:,1) = imu_data.del_vel(imuIndex,:);
dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex));
imuIndex = imuIndex+1;
% predict states
[states, delAngCorrected, delVelCorrected] = PredictStates(states,delta_angle,delta_velocity,imu_data.accel_dt(imuIndex),gravity,gps_data.refLLH(1,1)*deg2rad);
% constrain states
[states] = ConstrainStates(states,dt_imu_avg);
dtCov = dtCov + dt_imu;
delAngCov = delAngCov + delAngCorrected;
delVelCov = delVelCov + delVelCorrected;
if (dtCov > 0.01)
% predict covariance
covariance = PredictCovariance(delAngCov,delVelCov,states,covariance,dtCov,param);
delAngCov = [0;0;0];
delVelCov = [0;0;0];
dtCovInt = dtCovInt + dtCov;
dtCov = 0;
covIndex = covIndex + 1;
% output state data
output.time_lapsed(covIndex) = local_time;
output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')';
output.velocity_NED(covIndex,:) = states(5:7)';
output.position_NED(covIndex,:) = states(8:10)';
output.gyro_bias(covIndex,:) = states(11:13)';
output.accel_bias(covIndex,:) = states(14:16)';
output.mag_NED(covIndex,:) = states(17:19);
output.mag_XYZ(covIndex,:) = states(20:22);
output.wind_NE(covIndex,:) = states(23:24);
% output covariance data
for i=1:24
output.state_variances(covIndex,i) = covariance(i,i);
end
% output equivalent euler angle variances
error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));
euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);
for i=1:3
output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);
end
% Get most recent GPS data that had fallen behind the fusion time horizon
latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
if ~isempty(latest_gps_index)
% Check if GPS use is being blocked by the user
if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
gps_use_started = false;
gps_use_blocked = true;
else
gps_use_blocked = false;
end
% If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS
if (~gps_use_started && ~gps_use_blocked)
if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim))
states(5:7) = gps_data.vel_ned(latest_gps_index,:);
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
gps_use_started = true;
last_drift_constrain_time = local_time;
end
end
% Fuse GPS data when available if GPS use has started
if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
last_gps_index = latest_gps_index;
gps_fuse_index = gps_fuse_index + 1;
last_drift_constrain_time = local_time;
% fuse NED GPS velocity
[states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index));
% data logging
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
% fuse NE GPS position
[states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index));
% data logging
output.innovations.pos_time_lapsed(gps_fuse_index) = local_time;
output.innovations.posInnov(gps_fuse_index,:) = posInnov';
output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
else
% Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging
if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim)
if ((local_time - last_synthetic_velocity_fusion_time) > 0.2)
[states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim);
last_synthetic_velocity_fusion_time = local_time;
end
end
end
end
% Fuse new Baro data that has fallen beind the fusion time horizon
latest_baro_index = find((baro_data.time_us - 1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
if (latest_baro_index > last_baro_index)
last_baro_index = latest_baro_index;
baro_fuse_index = baro_fuse_index + 1;
% fuse baro height
[states,covariance,hgtInnov,hgtInnovVar] = FuseBaroHeight(states,covariance,baro_data.height(latest_baro_index),param.fusion.baroHgtGate,param.fusion.baroHgtNoise);
% data logging
output.innovations.hgt_time_lapsed(baro_fuse_index) = local_time;
output.innovations.hgtInnov(baro_fuse_index) = hgtInnov;
output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar;
end
% Fuse new mag data that has fallen behind the fusion time horizon
latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
if (latest_mag_index > last_mag_index)
last_mag_index = latest_mag_index;
mag_fuse_index = mag_fuse_index + 1;
% output magnetic field length to help with diagnostics
output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:)));
% fuse magnetometer data
if (param.fusion.magFuseMethod == 0 || param.fusion.magFuseMethod == 1)
[states,covariance,magInnov,magInnovVar] = FuseMagnetometer(states,covariance,mag_data.field_ga(latest_mag_index,:),param.fusion.magFieldGate, param.fusion.magFieldError^2);
% data logging
output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
output.innovations.magInnov(mag_fuse_index,:) = magInnov;
output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar;
if (param.fusion.magFuseMethod == 1)
% fuse in the local declination value
[states, covariance] = FuseMagDeclination(states, covariance, param.fusion.magDeclDeg*deg2rad);
end
elseif (param.fusion.magFuseMethod == 2)
% fuse magnetomer data as a single magnetic heading measurement
[states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2);
% log data
output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;
end
end
% Check if optical flow use is being blocked by the user
if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime))
flow_use_blocked = true;
else
flow_use_blocked = false;
end
% Attempt to use optical flow and range finder data if available and not blocked
if (flowDataPresent && ~flow_use_blocked)
% Get latest range finder data and gate to remove dropouts
last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
if (rng_data.dist(last_range_index) < param.fusion.rngValidMax)
range = max(rng_data.dist(last_range_index) , param.fusion.rngValidMin);
last_valid_range_time = local_time;
end
% Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement
latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
if (~isempty(latest_flow_index) && (latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout))
last_flow_index = latest_flow_index;
flow_fuse_index = flow_fuse_index + 1;
last_drift_constrain_time = local_time;
% fuse flow data
flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)];
bodyRate = [flow_data.bodyX(latest_flow_index);flow_data.bodyY(latest_flow_index)];
[states,covariance,flowInnov,flowInnovVar] = FuseOpticalFlow(states, covariance, flowRate, bodyRate, range, param.fusion.flowRateError^2, param.fusion.flowGate);
% data logging
output.innovations.flow_time_lapsed(flow_fuse_index) = local_time;
output.innovations.flowInnov(flow_fuse_index,:) = flowInnov;
output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar;
end
end
% Check if optical flow use is being blocked by the user
if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime))
viso_use_blocked = true;
else
viso_use_blocked = false;
end
% attempt to use ZED camera visual odmetry data if available and not blocked
if (visoDataPresent && ~viso_use_blocked)
% Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon
latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
if (latest_viso_index > last_viso_index)
last_viso_index = latest_viso_index;
viso_fuse_index = viso_fuse_index + 1;
last_drift_constrain_time = local_time;
% convert delta positon measurements to velocity
relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index);
% convert quality metric to equivalent observation error
% (this is a guess)
quality = viso_data.qual(latest_viso_index);
bodyVelError = param.fusion.bodyVelErrorMin * quality + param.fusion.bodyVelErrorMax * (1 - quality);
% fuse measurements
[states,covariance,bodyVelInnov,bodyVelInnovVar] = FuseBodyVel(states, covariance, relVelBodyMea, bodyVelError^2, param.fusion.bodyVelGate);
% data logging
output.innovations.bodyVel_time_lapsed(viso_fuse_index) = local_time;
output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov;
output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar;
end
end
end
% update average delta time estimate
output.dt = dtCovInt / covIndex;
end
end
EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m
Executable
+69
View File
@@ -0,0 +1,69 @@
%% Filter Control
param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass
param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s)
param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m)
param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec)
param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec)
param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec)
param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec)
param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec)
param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec)
param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec)
param.control.rollAlignErr = 0.0; % initial roll misalignment (rad)
param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad)
param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad)
%% GPS fusion
param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec)
%% Baro fusion
param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec)
param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
%% Magnetometer measurement fusion
param.fusion.magTimeDelay = 0.0; % Magnetomer time delay relative to IMU (sec)
param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD
param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD
param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg
%% Optical flow measurement fusion
param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec)
param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec)
param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec)
param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m)
param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec)
%% Visual odometry body frame velocity measurement fusion
param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec)
param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
%% State prediction error growth
param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3)
param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec)
param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2)
%% Initial Uncertainty
param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec)
param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec)
param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec)
param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec)
param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2)
param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion.
param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss)
param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss)
param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m)
param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec)
EKF/matlab/EKF_replay/Filter/SetParameters.m
Executable
+69
View File
@@ -0,0 +1,69 @@
%% Filter Control
param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass
param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s)
param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m)
param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec)
param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec)
param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec)
param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec)
param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec)
param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec)
param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec)
param.control.rollAlignErr = 0.0; % initial roll misalignment (rad)
param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad)
param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad)
%% GPS fusion
param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec)
%% Baro fusion
param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec)
param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
%% Magnetometer measurement fusion
param.fusion.magTimeDelay = 0.0; % Magnetomer time delay relative to IMU (sec)
param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None)
param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss)
param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad)
param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD
param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD
param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg
%% Optical flow measurement fusion
param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec)
param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec)
param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec)
param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m)
param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec)
%% Visual odometry body frame velocity measurement fusion
param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec)
param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
%% State prediction error growth
param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3)
param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec)
param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2)
%% Initial Uncertainty
param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec)
param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec)
param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec)
param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec)
param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2)
param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion.
param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss)
param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss)
param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m)
param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec)
EKF/matlab/EKF_replay/Filter/calcF24.m
+45
View File
@@ -0,0 +1,45 @@
function F = calcF24(dax,dax_b,day,day_b,daz,daz_b,dt,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3)
%CALCF24
% F = CALCF24(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DT,DVX,DVX_B,DVY,DVY_B,DVZ,DVZ_B,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:12
t2 = dax_b.*(1.0./2.0);
t3 = daz_b.*(1.0./2.0);
t4 = day_b.*(1.0./2.0);
t8 = day.*(1.0./2.0);
t5 = t4-t8;
t6 = q3.*(1.0./2.0);
t7 = q2.*(1.0./2.0);
t9 = daz.*(1.0./2.0);
t10 = dax.*(1.0./2.0);
t11 = -t2+t10;
t12 = q1.*(1.0./2.0);
t13 = -t3+t9;
t14 = -t4+t8;
t15 = dvx-dvx_b;
t16 = dvy-dvy_b;
t17 = dvz-dvz_b;
t18 = q1.*t17.*2.0;
t19 = q1.*t16.*2.0;
t20 = q0.*t17.*2.0;
t21 = q1.*t15.*2.0;
t22 = q2.*t16.*2.0;
t23 = q3.*t17.*2.0;
t24 = t21+t22+t23;
t25 = q0.*t15.*2.0;
t26 = q2.*t17.*2.0;
t37 = q3.*t16.*2.0;
t27 = t25+t26-t37;
t28 = q0.*q3.*2.0;
t29 = q0.^2;
t30 = q1.^2;
t31 = q2.^2;
t32 = q3.^2;
t33 = q2.*t15.*2.0;
t34 = q3.*t15.*2.0;
t35 = q0.*t16.*2.0;
t36 = -t18+t34+t35;
t38 = q0.*q1.*2.0;
F = reshape([1.0,t11,t14,t13,t27,t36,t19+t20-t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dax.*(-1.0./2.0)+t2,1.0,t3-t9,t14,t24,-t19-t20+t33,t36,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5,t13,1.0,t2-t10,t19+t20-q2.*t15.*2.0,t24,-t25-t26+t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,daz.*(-1.0./2.0)+t3,t5,t11,1.0,t18-q0.*t16.*2.0-q3.*t15.*2.0,t27,t24,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t12,q0.*(-1.0./2.0),-t6,t7,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t6,q0.*(-1.0./2.0),-t12,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t6,-t7,t12,q0.*(-1.0./2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t29-t30+t31+t32,-t28-q1.*q2.*2.0,q0.*q2.*2.0-q1.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t28-q1.*q2.*2.0,-t29+t30-t31+t32,-t38-q2.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q2.*-2.0-q1.*q3.*2.0,t38-q2.*q3.*2.0,-t29+t30+t31-t32,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0],[24, 24]);
EKF/matlab/EKF_replay/Filter/calcH_HDG.m
+51
View File
@@ -0,0 +1,51 @@
function H_MAG = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3)
%CALCH_HDG
% H_MAG = CALCH_HDG(MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:14
t2 = q0.^2;
t3 = q1.^2;
t4 = q2.^2;
t5 = q3.^2;
t6 = q0.*q3.*2.0;
t10 = q1.*q2.*2.0;
t17 = t2-t3+t4-t5;
t18 = magY.*t17;
t19 = t6+t10;
t20 = magX.*t19;
t21 = q0.*q1.*2.0;
t22 = q2.*q3.*2.0;
t23 = t21-t22;
t24 = magZ.*t23;
t7 = t18+t20-t24;
t8 = t2+t3-t4-t5;
t9 = magX.*t8;
t11 = q0.*q2.*2.0;
t12 = q1.*q3.*2.0;
t13 = t11+t12;
t14 = magZ.*t13;
t15 = t6-t10;
t25 = magY.*t15;
t16 = t9+t14-t25;
t26 = 1.0./t16.^2;
t27 = t7.^2;
t28 = 1.0./t16;
t29 = t26.*t27;
t30 = t29+1.0;
t31 = 1.0./t30;
t32 = magX.*q1.*2.0;
t33 = magY.*q2.*2.0;
t34 = magZ.*q3.*2.0;
t35 = t32+t33+t34;
t36 = magY.*q1.*2.0;
t37 = magZ.*q0.*2.0;
t38 = t36+t37-magX.*q2.*2.0;
t39 = magX.*q0.*2.0;
t40 = magZ.*q2.*2.0;
t41 = t39+t40-magY.*q3.*2.0;
t42 = magY.*q0.*2.0;
t43 = magX.*q3.*2.0;
t44 = t42+t43-magZ.*q1.*2.0;
H_MAG = [(t28.*t44-t7.*t26.*t41)./(t27.*1.0./(t9+t14-magY.*(t6-q1.*q2.*2.0)).^2+1.0),-t31.*(t28.*t38+t7.*t26.*t35),t31.*(t28.*t35-t7.*t26.*t38),t31.*(t28.*t41+t7.*t26.*t44),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t31.*(t19.*t28-t7.*t8.*t26),t31.*(t17.*t28+t7.*t15.*t26),-t31.*(t23.*t28+t7.*t13.*t26),0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_LOSX.m
+9
View File
@@ -0,0 +1,9 @@
function H_LOSX = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn)
%CALCH_LOSX
% H_LOSX = CALCH_LOSX(Q0,Q1,Q2,Q3,RANGE,VD,VE,VN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:14
t2 = 1.0./range;
H_LOSX = [t2.*(q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0),t2.*(q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0),t2.*(q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0),-t2.*(q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0),-t2.*(q0.*q3.*2.0-q1.*q2.*2.0),t2.*(q0.^2-q1.^2+q2.^2-q3.^2),t2.*(q0.*q1.*2.0+q2.*q3.*2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_LOSY.m
+9
View File
@@ -0,0 +1,9 @@
function H_LOSY = calcH_LOSY(q0,q1,q2,q3,range,vd,ve,vn)
%CALCH_LOSY
% H_LOSY = CALCH_LOSY(Q0,Q1,Q2,Q3,RANGE,VD,VE,VN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:15
t2 = 1.0./range;
H_LOSY = [-t2.*(q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0),-t2.*(q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0),t2.*(q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0),-t2.*(q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0),-t2.*(q0.^2+q1.^2-q2.^2-q3.^2),-t2.*(q0.*q3.*2.0+q1.*q2.*2.0),t2.*(q0.*q2.*2.0-q1.*q3.*2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_MAGD.m
+12
View File
@@ -0,0 +1,12 @@
function H_MAGD = calcH_MAGD(magE,magN)
%CALCH_MAGD
% H_MAGD = CALCH_MAGD(MAGE,MAGN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:13
t2 = magE.^2;
t3 = magN.^2;
t4 = t2+t3;
t5 = 1.0./t4;
H_MAGD = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-magE.*t5,magN.*t5,0.0,0.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_MAGX.m
+8
View File
@@ -0,0 +1,8 @@
function H_MAGX = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3)
%CALCH_MAGX
% H_MAGX = CALCH_MAGX(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:12
H_MAGX = [magD.*q2.*-2.0+magE.*q3.*2.0+magN.*q0.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,magD.*q0.*-2.0+magE.*q1.*2.0-magN.*q2.*2.0,magD.*q1.*2.0+magE.*q0.*2.0-magN.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.^2+q1.^2-q2.^2-q3.^2,q0.*q3.*2.0+q1.*q2.*2.0,q0.*q2.*-2.0+q1.*q3.*2.0,1.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_MAGY.m
+8
View File
@@ -0,0 +1,8 @@
function H_MAGY = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3)
%CALCH_MAGY
% H_MAGY = CALCH_MAGY(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:13
H_MAGY = [magD.*q1.*2.0+magE.*q0.*2.0-magN.*q3.*2.0,magD.*q0.*2.0-magE.*q1.*2.0+magN.*q2.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,magD.*q2.*2.0-magE.*q3.*2.0-magN.*q0.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q3.*-2.0+q1.*q2.*2.0,q0.^2-q1.^2+q2.^2-q3.^2,q0.*q1.*2.0+q2.*q3.*2.0,0.0,1.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m
+8
View File
@@ -0,0 +1,8 @@
function H_MAGZ = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3)
%CALCH_MAGZ
% H_MAGZ = CALCH_MAGZ(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:13
H_MAGZ = [magD.*q0.*2.0-magE.*q1.*2.0+magN.*q2.*2.0,magD.*q1.*-2.0-magE.*q0.*2.0+magN.*q3.*2.0,magD.*q2.*-2.0+magE.*q3.*2.0+magN.*q0.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q2.*2.0+q1.*q3.*2.0,q0.*q1.*-2.0+q2.*q3.*2.0,q0.^2-q1.^2-q2.^2+q3.^2,0.0,0.0,1.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_VELX.m
+8
View File
@@ -0,0 +1,8 @@
function H_VELX = calcH_VELX(q0,q1,q2,q3,vd,ve,vn)
%CALCH_VELX
% H_VELX = CALCH_VELX(Q0,Q1,Q2,Q3,VD,VE,VN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:15
H_VELX = [q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q0.*vd.*-2.0+q1.*ve.*2.0-q2.*vn.*2.0,q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0,q0.^2+q1.^2-q2.^2-q3.^2,q0.*q3.*2.0+q1.*q2.*2.0,q0.*q2.*-2.0+q1.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_VELY.m
+8
View File
@@ -0,0 +1,8 @@
function H_VELY = calcH_VELY(q0,q1,q2,q3,vd,ve,vn)
%CALCH_VELY
% H_VELY = CALCH_VELY(Q0,Q1,Q2,Q3,VD,VE,VN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:15
H_VELY = [q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0,q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q2.*vd.*2.0-q3.*ve.*2.0-q0.*vn.*2.0,q0.*q3.*-2.0+q1.*q2.*2.0,q0.^2-q1.^2+q2.^2-q3.^2,q0.*q1.*2.0+q2.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcH_VELZ.m
+8
View File
@@ -0,0 +1,8 @@
function H_VELZ = calcH_VELZ(q0,q1,q2,q3,vd,ve,vn)
%CALCH_VELZ
% H_VELZ = CALCH_VELZ(Q0,Q1,Q2,Q3,VD,VE,VN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:16
H_VELZ = [q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0,q1.*vd.*-2.0-q0.*ve.*2.0+q3.*vn.*2.0,q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q0.*q2.*2.0+q1.*q3.*2.0,q0.*q1.*-2.0+q2.*q3.*2.0,q0.^2-q1.^2-q2.^2+q3.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
EKF/matlab/EKF_replay/Filter/calcQ24.m
+45
View File
@@ -0,0 +1,45 @@
function Q = calcQ24(daxVar,dayVar,dazVar,dvxVar,dvyVar,dvzVar,q0,q1,q2,q3)
%CALCQ24
% Q = CALCQ24(DAXVAR,DAYVAR,DAZVAR,DVXVAR,DVYVAR,DVZVAR,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:11
t2 = dayVar.*q2.*q3.*(1.0./4.0);
t3 = t2-daxVar.*q0.*q1.*(1.0./4.0)-dazVar.*q2.*q3.*(1.0./4.0);
t4 = q3.^2;
t5 = q2.^2;
t6 = dazVar.*q1.*q3.*(1.0./4.0);
t7 = t6-daxVar.*q1.*q3.*(1.0./4.0)-dayVar.*q0.*q2.*(1.0./4.0);
t8 = daxVar.*q0.*q3.*(1.0./4.0);
t9 = t8-dayVar.*q0.*q3.*(1.0./4.0)-dazVar.*q1.*q2.*(1.0./4.0);
t10 = q0.^2;
t11 = q1.^2;
t12 = daxVar.*q1.*q2.*(1.0./4.0);
t13 = t12-dayVar.*q1.*q2.*(1.0./4.0)-dazVar.*q0.*q3.*(1.0./4.0);
t14 = dazVar.*q0.*q2.*(1.0./4.0);
t15 = t14-daxVar.*q0.*q2.*(1.0./4.0)-dayVar.*q1.*q3.*(1.0./4.0);
t16 = dayVar.*q0.*q1.*(1.0./4.0);
t17 = t16-daxVar.*q2.*q3.*(1.0./4.0)-dazVar.*q0.*q1.*(1.0./4.0);
t21 = q0.*q3.*2.0;
t22 = q1.*q2.*2.0;
t18 = t21-t22;
t23 = q0.*q2.*2.0;
t24 = q1.*q3.*2.0;
t19 = t23+t24;
t20 = t4+t5-t10-t11;
t25 = q0.*q1.*2.0;
t26 = t21+t22;
t32 = t4-t5-t10+t11;
t27 = dvyVar.*t18.*t32;
t28 = q2.*q3.*2.0;
t29 = t25-t28;
t30 = t4-t5-t10+t11;
t31 = t25+t28;
t33 = t4-t5+t10-t11;
t34 = t23-t24;
t35 = dvxVar.*t34.*(t4+t5-t10-t11);
t36 = dvzVar.*t19.*t33;
t37 = t35+t36-dvyVar.*t18.*t31;
t38 = -dvxVar.*t26.*t34-dvyVar.*t31.*t32-dvzVar.*t29.*t33;
Q = reshape([daxVar.*t11.*(1.0./4.0)+dayVar.*t5.*(1.0./4.0)+dazVar.*t4.*(1.0./4.0),t3,t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3,daxVar.*t10.*(1.0./4.0)+dayVar.*t4.*(1.0./4.0)+dazVar.*t5.*(1.0./4.0),t9,t15,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t9,daxVar.*t4.*(1.0./4.0)+dayVar.*t10.*(1.0./4.0)+dazVar.*t11.*(1.0./4.0),t17,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t13,t15,t17,daxVar.*t5.*(1.0./4.0)+dayVar.*t11.*(1.0./4.0)+dazVar.*t10.*(1.0./4.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxVar.*t20.^2+dvyVar.*t18.^2+dvzVar.*t19.^2,t27-dvxVar.*t20.*t26-dvzVar.*t19.*t29,t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t27-dvzVar.*t19.*(t25-q2.*q3.*2.0)-dvxVar.*t20.*t26,dvxVar.*t26.^2+dvyVar.*t30.^2+dvzVar.*t29.^2,t38,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t37,t38,dvxVar.*t34.^2+dvyVar.*t31.^2+dvzVar.*t33.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[24, 24]);
EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m
Executable
+50
View File
@@ -0,0 +1,50 @@
clear all;
close all;
% add required paths
addpath('../Common');
% load test data
load '../TestData/PX4/baro_data.mat';
load '../TestData/PX4/gps_data.mat';
load '../TestData/PX4/imu_data.mat';
load '../TestData/PX4/mag_data.mat';
% set parameters to default values
run('SetParameterDefaults.m');
% Loop through range of GPS time delays and capture the RMS velocity
% innovation and corresponding delay each time
for index = 1:1:21
index
param.fusion.gpsTimeDelay = 0.01*(index - 1);
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
gps_delay(index) = param.fusion.gpsTimeDelay;
gps_vel_innov_rms(index) = sqrt(sum(output.innovations.vel_innov(:,1).^2) + sum(output.innovations.vel_innov(:,2).^2))/sqrt(length(output.innovations.vel_innov));
end
% plot the innvation level vs time delay
figure;
plot(gps_delay,gps_vel_innov_rms);
grid on;
xlabel('GPS delay (sec)');
ylabel('RMS velocity innovation (m/s)');
% find the smallest value and re-run the replay
param.fusion.gpsTimeDelay = gps_delay(gps_delay == min(gps_delay))
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
% generate and save output plots
runIdentifier = ' : PX4 data replay ';
folder = strcat('../OutputPlots/PX4');
PlotData(output,folder,runIdentifier);
% save output data
folder = '../OutputData/PX4';
fileName = '../OutputData/PX4/ekf_replay_output.mat';
if ~exist(folder,'dir')
mkdir(folder);
end
save(fileName,'output');
EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m
+38
View File
@@ -0,0 +1,38 @@
function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3)
%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX
% ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 13-Jul-2017 08:41:41
t8 = q0.*q1.*2.0;
t9 = q2.*q3.*2.0;
t2 = t8+t9;
t4 = q0.^2;
t5 = q1.^2;
t6 = q2.^2;
t7 = q3.^2;
t3 = t4-t5-t6+t7;
t10 = t3.^2;
t11 = t2.^2;
t12 = t10+t11;
t13 = 1.0./t12;
t14 = 1.0./t3;
t15 = 1.0./t3.^2;
t17 = q0.*q2.*2.0;
t18 = q1.*q3.*2.0;
t16 = t17-t18;
t19 = t16.^2;
t20 = -t19+1.0;
t21 = 1.0./sqrt(t20);
t24 = q0.*q3.*2.0;
t25 = q1.*q2.*2.0;
t22 = t24+t25;
t23 = t4+t5-t6-t7;
t26 = t23.^2;
t27 = t22.^2;
t28 = t26+t27;
t29 = 1.0./t28;
t30 = 1.0./t23;
t31 = 1.0./t23.^2;
error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]);
EKF/matlab/EKF_replay/Filter/replay_apm_data.m
Executable
+46
View File
@@ -0,0 +1,46 @@
clear all;
close all;
% add required paths
addpath('../Common');
% load compulsory data
load '../TestData/APM/baro_data.mat';
load '../TestData/APM/gps_data.mat';
load '../TestData/APM/imu_data.mat';
load '../TestData/APM/mag_data.mat';
% load optional data required for optical flow replay
if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file')
load '../TestData/APM/rng_data.mat';
load '../TestData/APM/flow_data.mat';
else
rng_data = [];
flow_data = [];
end
% load optional data required for ZED camera replay
if exist('../TestData/APM/viso_data.mat','file')
load '../TestData/APM/viso_data.mat';
else
viso_data = [];
end
% load default parameters
run('SetParameters.m');
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
% generate and save output plots
runIdentifier = ' : APM data replay ';
folder = strcat('../OutputPlots/APM');
PlotData(output,folder,runIdentifier);
% save output data
folder = '../OutputData/APM';
fileName = '../OutputData/APM/ekf_replay_output.mat';
if ~exist(folder,'dir')
mkdir(folder);
end
save(fileName,'output');
EKF/matlab/EKF_replay/Filter/replay_px4_data.m
Executable
+30
View File
@@ -0,0 +1,30 @@
clear all;
close all;
% add required paths
addpath('../Common');
% load test data
load '../TestData/PX4/baro_data.mat';
load '../TestData/PX4/gps_data.mat';
load '../TestData/PX4/imu_data.mat';
load '../TestData/PX4/mag_data.mat';
% set parameters to default values
run('SetParameters.m');
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
% generate and save output plots
runIdentifier = ' : PX4 data replay ';
folder = strcat('../OutputPlots/PX4');
PlotData(output,folder,runIdentifier);
% save output data
folder = '../OutputData/PX4';
fileName = '../OutputData/PX4/ekf_replay_output.mat';
if ~exist(folder,'dir')
mkdir(folder);
end
save(fileName,'output');
EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m
+32
View File
@@ -0,0 +1,32 @@
clear all;
close all;
% add required paths
addpath('../Common');
% load test data
load '../TestData/PX4_optical_flow/baro_data.mat';
load '../TestData/PX4_optical_flow/gps_data.mat';
load '../TestData/PX4_optical_flow/imu_data.mat';
load '../TestData/PX4_optical_flow/mag_data.mat';
load '../TestData/PX4_optical_flow/rng_data.mat';
load '../TestData/PX4_optical_flow/flow_data.mat';
% set parameters to default values
run('SetParameterDefaults.m');
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data);
% generate and save output plots
runIdentifier = ' : PX4 optical flow replay ';
folder = strcat('../OutputPlots/PX4_optical_flow');
PlotData(output,folder,runIdentifier);
% save output data
folder = '../OutputData/PX4_optical_flow';
fileName = '../OutputData/PX4_optical_flow/ekf_replay_output.mat';
if ~exist(folder,'dir')
mkdir(folder);
end
save(fileName,'output');
EKF/matlab/EKF_replay/Filter/transfer_matrix.m
+13
View File
@@ -0,0 +1,13 @@
function T_MAG = transfer_matrix(magE,magN)
%TRANSFER_MATRIX
% T_MAG = TRANSFER_MATRIX(MAGE,MAGN)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 29-May-2017 00:16:16
t2 = 1.0./magN.^2;
t3 = magE.^2;
t4 = t2.*t3;
t5 = t4+1.0;
t6 = 1.0./t5;
T_MAG = [-magE.*t2.*t6,t6./magN];
EKF/matlab/EKF_replay/SensorCalibration/allan.m
+19
View File
@@ -0,0 +1,19 @@
function [T,sigma] = allan(omega,fs,pts)
[N,M] = size(omega); % figure out how big the output data set is
n = 2.^(0:floor(log2(N/2)))'; % determine largest bin size
maxN = n(end);
endLogInc = log10(maxN);
m = unique(ceil(logspace(0,endLogInc,pts)))'; % create log spaced vector average factor
t0 = 1/fs;
T = m*t0;
theta = cumsum(omega)/fs;
sigma2 = zeros(length(T),M);
for i=1:length(m)
% t0 = sample interval
% T = length of time for each cluster
% integration of samples over time to obtain output angle ?
% array of dimensions (cluster periods) X (#variables)
% loop over the various cluster sizes
% implements the summation in the AV equation
sigma2 = sigma2./repmat((2*T.^2.*(N-2*m)),1,M);
sigma = sqrt(sigma2)
EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m
+189
View File
@@ -0,0 +1,189 @@
% test ellipsoid sphere fitting algorithms
%
% http://www.st.com/content/ccc/resource/technical/document/design_tip/group0/a2/98/f5/d4/9c/48/4a/d1/DM00286302/files/DM00286302.pdf/jcr:content/translations/en.DM00286302.pdf
%
%% load log data
clear all;
close all;
% uncomment these lines if using legacy .px4log format
%load sysvector.mat;
%mag_meas = [sysvector.IMU_MagX';sysvector.IMU_MagY';sysvector.IMU_MagZ'];
% uncomment these lines if using data imported from ulog format
%load sysdata.mat;
%mag_meas = [magnetometer_ga0';magnetometer_ga1';magnetometer_ga2'];
% thin data points to use data every 5 deg
delta_angle_lim = 5* pi/180;
counter = 1;
angle = 0;
last_angle = 0;
for i = 2:length(data1.IMU.Tsec)
ang_rate = 0.5 * (sqrt(data1.IMU.GyroX(i)^2 + data1.IMU.GyroY(i)^2 + data1.IMU.GyroZ(i)^2) + ...
sqrt(data1.IMU.GyroX(i-1)^2 + data1.IMU.GyroY(i-1)^2 + data1.IMU.GyroZ(i-1)^2));
dt = data1.IMU.Tsec(i) - data1.IMU.Tsec(i-1);
angle = angle + ang_rate * dt;
if ((angle - last_angle) > delta_angle_lim)
mag_meas(:,counter) = [data1.IMU.MagX(i);data1.IMU.MagY(i);data1.IMU.MagZ(i)];
counter = counter + 1;
last_angle = angle;
end
end
angle = 0;
last_angle = 0;
for i = 2:length(data2.IMU.Tsec)
ang_rate = 0.5 * (sqrt(data2.IMU.GyroX(i)^2 + data2.IMU.GyroY(i)^2 + data2.IMU.GyroZ(i)^2) + ...
sqrt(data2.IMU.GyroX(i-1)^2 + data2.IMU.GyroY(i-1)^2 + data2.IMU.GyroZ(i-1)^2));
dt = data2.IMU.Tsec(i) - data2.IMU.Tsec(i-1);
angle = angle + ang_rate * dt;
if ((angle - last_angle) > delta_angle_lim)
mag_meas(:,counter) = [data2.IMU.MagX(i);data2.IMU.MagY(i);data2.IMU.MagZ(i)];
counter = counter + 1;
last_angle = angle;
end
end
angle = 0;
last_angle = 0;
for i = 2:length(data3.IMU.Tsec)
ang_rate = 0.5 * (sqrt(data3.IMU.GyroX(i)^2 + data3.IMU.GyroY(i)^2 + data3.IMU.GyroZ(i)^2) + ...
sqrt(data3.IMU.GyroX(i-1)^2 + data3.IMU.GyroY(i-1)^2 + data3.IMU.GyroZ(i-1)^2));
dt = data3.IMU.Tsec(i) - data3.IMU.Tsec(i-1);
angle = angle + ang_rate * dt;
if ((angle - last_angle) > delta_angle_lim)
mag_meas(:,counter) = [data3.IMU.MagX(i);data3.IMU.MagY(i);data3.IMU.MagZ(i)];
counter = counter + 1;
last_angle = angle;
end
end
%% fit a sphere and determine the fit quality
[offset,gain,rotation]=ellipsoid_fit(mag_meas',5);
% correct the data
mag_corrected_5 = zeros(size(mag_meas));
rotation_correction = inv(rotation); % we apply the inverse of the original rotation
scale_correction = 1./gain;
scale_correction = scale_correction ./ mean(scale_correction);
mag_strength = zeros(length(mag_meas),1);
for i = 1:length(mag_meas)
% subtract the offsets
mag_corrected_5(:,i) = mag_meas(:,i) - offset;
% correct the rotation
mag_corrected_5(:,i) = rotation_correction * mag_corrected_5(:,i);
% correct the scale factor
mag_corrected_5(:,i) = mag_corrected_5(:,i) .* scale_correction;
% calculate the residual
mag_strength(i) = sqrt(dot(mag_corrected_5(:,i),mag_corrected_5(:,i)));
end
% calculate the fit residual for fit option 5
fit_residual_5 = mag_strength - mean(mag_strength);
%% fit a un-rotated ellipsoid and determine the fit quality
[offset,gain,rotation]=ellipsoid_fit(mag_meas',1);
% correct the data
mag_corrected_1 = zeros(size(mag_meas));
rotation_correction = inv(rotation); % we apply the inverse of the original rotation
scale_correction = 1./gain;
scale_correction = scale_correction ./ mean(scale_correction);
mag_strength = zeros(length(mag_meas),1);
angle_change_1 = zeros(length(mag_meas),1);
for i = 1:length(mag_meas)
% subtract the offsets
mag_corrected_1(:,i) = mag_meas(:,i) - offset;
% correct the rotation
mag_corrected_1(:,i) = rotation_correction * mag_corrected_1(:,i);
% correct the scale factor
mag_corrected_1(:,i) = mag_corrected_1(:,i) .* scale_correction;
% calculate the residual
mag_strength(i) = sqrt(dot(mag_corrected_1(:,i),mag_corrected_1(:,i)));
% calculate the angular change due to the fit
angle_change_1(i) = atan2(norm(cross(mag_corrected_1(:,i),mag_meas(:,i))),dot(mag_corrected_1(:,i),mag_meas(:,i)));
end
% calculate the fit residual for fit option 1
fit_residual_1 = mag_strength - mean(mag_strength);
%% fit a rotated ellipsoid and check the fit quality
[offset,gain,rotation]=ellipsoid_fit(mag_meas',0);
% correct the data
mag_corrected_0 = zeros(size(mag_meas));
rotation_correction = inv(rotation); % we apply the inverse of the original rotation
scale_correction = 1./gain;
scale_correction = scale_correction ./ mean(scale_correction);
mag_strength = zeros(length(mag_meas),1);
angle_change_0 = zeros(length(mag_meas),1);
for i = 1:length(mag_meas)
% subtract the offsets
mag_corrected_0(:,i) = mag_meas(:,i) - offset;
% correct the rotation
mag_corrected_0(:,i) = rotation_correction * mag_corrected_0(:,i);
% correct the scale factor
mag_corrected_0(:,i) = mag_corrected_0(:,i) .* scale_correction;
% calculate the residual
mag_strength(i) = sqrt(dot(mag_corrected_0(:,i),mag_corrected_0(:,i)));
% calculate the angular change due to the fit
angle_change_0(i) = atan2(norm(cross(mag_corrected_0(:,i),mag_meas(:,i))),dot(mag_corrected_0(:,i),mag_meas(:,i)));
end
% calculate the fit residual for fit option 0
fit_residual_0 = mag_strength - mean(mag_strength);
%% calculate the residual for uncorrected data
for i = 1:length(mag_meas)
mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i)));
end
uncorrected_residual = mag_strength - mean(mag_strength);
%% plot the fit residuals
plot(uncorrected_residual,'k+');
hold on;
plot(fit_residual_5,'r+');
plot(fit_residual_1,'b+');
plot(fit_residual_0,'g+');
hold off;
grid on;
title('mag fit comparison');
xlabel('measurement index');
ylabel('fit residual (Gauss)');
legend('uncorrected','sphere','non-rotated ellipse','rotated ellipse');
%% plot the data points in 3D
figure;
plot3(mag_meas(1,:),mag_meas(2,:),mag_meas(3,:),' .');hold on;plot3(mag_corrected_1(1,:),mag_corrected_1(2,:),mag_corrected_1(3,:),'r.');
hold off;grid on;axis equal;
xlabel('x (Gauss)');
xlabel('y (Gauss)');
xlabel('z (Gauss)');
legend('uncorrected','unrotated ellipse');
%% calculate and plot the angular error
figure;
plot(angle_change_1*(180/pi),'b+');
title('angle change after un-rotated ellipse fit');
xlabel('measurement index');
ylabel('angle change magnitude (deg)');
EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m
+61
View File
@@ -0,0 +1,61 @@
function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin)
% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points
% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z)
% optional flag f, default to 0 (fitting of rotated ellipsoid)
x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end;
if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid)
elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid)
elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y
elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z
elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z
elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere)
end;
v = (D'*D)\(D'*ones(length(x),1)); % least square fitting
if f==0, % rotated ellipsoid
A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ];
ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid
Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0)
[rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain)
gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid
else % non-rotated ellipsoid
if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere
end;
ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid
rotM=eye(3); % eigenvectors (rotation), identity = no rotation
g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3));
gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale)
end;
EKF/matlab/EKF_replay/readme.txt
+61
View File
@@ -0,0 +1,61 @@
Instructions for running the EKF replay
1) Ensure the ‘EKF_replay’ directory is in a location you have full read and write access
2) Create a ‘TestData’ sub-directory under the ‘EKF_replay’ directory
A sample dataset can be downloaded here: https://drive.google.com/file/d/0By4v2BuLAaCfSW9fWl9aSWNGbGs/view?usp=sharing
3a) If replaying APM data:
Collect data with LOG_REPLAY = 1 and LOG_DISARMED = 1.
Convert data to a .mat file using the MissionPlanner ‘Create Matlab File’ option under the DataFlash Logs tab.
Convert .mat file to the required data format using the convert_apm_data.m script file. This will generate the following data files:
imu_data.mat
baro_data.mat
gps_data.mat
mag_data.mat
and optionally
rng_data.mat
flow_data.mat
viso_data.mat
Note: If the rangefinder, optical flow or ZED camera odometer data are not present in the log, then the corresponding sections in the convert_apm_data.m script file will need to be commented out.
Copy the generated .mat files into the /EKF_replay/TestData/APM directory.
3b) If replaying PX4 data:
Collect data with EK2_REC_RPL = 1
Convert the .ulg log file to .csv files using the PX4/pyulog python script https://github.com/PX4/pyulog/blob/master/pyulog/ulog2csv.py
Import the .csv file containing the sensor_combined_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m. This will generate the following data files:
imu_data.mat
baro_data.mat
mag_data.mat
Import the .csv file containing the vehicle_gps_position_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_vehicle_gps_position_csv. This will generate the gps_data.mat file.
If you have an optical flow and range finder sensor fitted:
Import the .csv file containing the optical_flow_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_optical_flow_csv_data.m.
Import the .csv file containing the distance_sensor_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m.
This will generate the following data files:
flow_data.mat
rng_data.mat
Copy the generated .mat files into the /EKF_replay/TestData/PX4 directory.
4) Make ‘…/EKF_replay/Filter’ the working directory.
5) Execute ‘SetParameterDefaults’ at the command prompt to load the default filter tuning parameter struct ‘param’ into the workspace. The defaults have been set to provide robust estimation across the entire data set, not optimised for accuracy.
6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file.
Output plots are saved as .png files in the ‘…/EKF_replay/OutputPlots/‘ directory.
Output data is written to the Matlab workspace in the ‘output’ struct.
EKF/matlab/README.md
+3
View File
@@ -0,0 +1,3 @@
# MATLAB Derivations
This folder contains the derivations and test suites in Matlab for ECL components.
EKF/matlab/analysis/estimatorLogViewerPX4.m
+133
View File
@@ -0,0 +1,133 @@
function estimatorLogViewer(fname)
close all;
load(fname);
stateFieldNames = [fieldnames(estimatorData.EST0);fieldnames(estimatorData.EST1)];
stateFieldNames(strcmp('T',stateFieldNames) | strcmp('index',stateFieldNames) | ...
strcmp('nStat',stateFieldNames) | strcmp('fNaN',stateFieldNames) | ...
strcmp('fHealth',stateFieldNames) | strcmp('fTOut',stateFieldNames)) = [];
innovFieldNamesT = [fieldnames(estimatorData.EST4);fieldnames(estimatorData.EST5)];
innovFieldNamesT(strcmp('T',innovFieldNamesT) | strcmp('index',innovFieldNamesT)) = [];
innovFieldInds = cellfun(@(x)x(end)=='V',innovFieldNamesT);
innovFieldNames = innovFieldNamesT(~innovFieldInds);
% lay out static objects
f = figure('Units','normalized');
set(f,'Position',[.25,.25,.5,.5]);
tabs = uitabgroup(f);
statesTab = uitab(tabs,'Title','States');
innovsTab = uitab(tabs,'Title','Innovations');
axesConfig = {'Units','Normalized','Position',[.05,.1,.65,.85],'NextPlot','add'};
BGconfig = {'Units','Normalized','Position',[.75,.1,.2,.85]};
TBconfig = {'Style','Checkbox','Units','Normalized','Position',[.75,0,.2,.1],'String','Show variance'};
statesAxes = axes('Parent',statesTab,axesConfig{:},'XLabel',text('string','Time (sec)'));
innovsAxes = axes('Parent',innovsTab,axesConfig{:},'XLabel',text('string','Time (sec)'));
statesBG = uibuttongroup(statesTab,BGconfig{:});
innovsBG = uibuttongroup(innovsTab,BGconfig{:});
statesVarToggle = uicontrol(statesTab,TBconfig{:});
innovsVarToggle = uicontrol(innovsTab,TBconfig{:});
% lay out dynamic objects
stateButtons = zeros(numel(stateFieldNames),1);
for k = 1:numel(stateFieldNames)
stateButtons(k) = uicontrol(statesBG,'Style','Checkbox','Units','normalized',...
'String',stateFieldNames{k},'Position',[0,1 - k/numel(stateFieldNames),1, 1/numel(stateFieldNames)],...
'Callback',@(es,ed)toggleLine(es,ed,statesAxes,statesVarToggle,estimatorData.EST0,estimatorData.EST1,estimatorData.EST2,estimatorData.EST3));
end
innovButtons = zeros(numel(innovFieldNames),1);
for k = 1:numel(innovFieldNames)
innovButtons(k) = uicontrol(innovsBG,'Style','Checkbox','Units','normalized',...
'String',innovFieldNames{k},'Position',[0,1 - k/numel(innovFieldNames),1, 1/numel(innovFieldNames)],...
'Callback',@(es,ed)toggleLine(es,ed,innovsAxes,innovsVarToggle,estimatorData.EST4,estimatorData.EST5,[],[]));
end
set(statesVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,statesAxes,estimatorData.EST2,estimatorData.EST3));
set(innovsVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,innovsAxes,estimatorData.EST4,estimatorData.EST5));
end
function toggleLine(es,~,axes,varToggle,struct1,struct2,vstruct1,vstruct2)
if es.Value == 0
delete(findobj(axes,'UserData',es.String));
else
if any(strcmp(es.String,fieldnames(struct1)))
dataSrc = struct1;
else
dataSrc = struct2;
end
p = plot(dataSrc.Tsec,dataSrc.(es.String));
set(p,'Parent',axes,'UserData',es.String);
end
if varToggle.Value == 1
if ~isempty(vstruct1) && ~isempty(vstruct2)
toggleVariance(varToggle,[],axes,vstruct1,vstruct2);
else
toggleVariance(varToggle,[],axes,struct1,struct2);
end
end
updateLegend(axes);
end
function toggleVariance(es,~,axes,struct1,struct2)
if es.Value == 0
delete(findobj(axes,'Type','Patch'));
else
lines = findobj(axes,'Type','Line');
if isempty(lines)
return;
end
stateNames = {lines.UserData};
if any(strncmp('s',stateNames,1))
varNames = strrep(stateNames,'s','P');
else
varNames = strrep(stateNames,'I','IV');
end
for k = 1:numel(varNames)
if any(strcmp(varNames{k},fieldnames(struct1)))
dataSrc = struct1;
else
dataSrc = struct2;
end
centerLineTimeFull = get(lines(k),'XData');
centerLineDataFull = get(lines(k),'YData');
startTime = max(centerLineTimeFull(1),dataSrc.Tsec(1));
endTime = min(centerLineTimeFull(end),dataSrc.Tsec(end));
plotTimeFull = dataSrc.Tsec(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime);
plotDataFull = dataSrc.(varNames{k})(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime);
centerLineTime = centerLineTimeFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime);
centerLineData = centerLineDataFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime);
plotTime = linspace(startTime,endTime,250);
plotData = sqrt(interp1(plotTimeFull,plotDataFull,plotTime));
centerLineData = interp1(centerLineTime,centerLineData,plotTime);
% plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0);
if strcmp('IV',varNames{k}(end-1:end))
plotDataL = -plotData;
plotDataU = plotData;
else
plotDataL = centerLineData-plotData;
plotDataU = centerLineData+plotData;
end
p = patch([plotTime,fliplr(plotTime)],[plotDataL,fliplr(plotDataU)],lines(k).Color);
set(p,'Parent',axes,'EdgeColor','none','FaceAlpha',.3,'UserData',stateNames{k});
end
end
end
function updateLegend(axes)
lines = findobj(axes,'Type','Line');
if isempty(lines)
legend(axes,'off');
else
legend(axes,lines,{lines.UserData});
end
end
EKF/matlab/analysis/importPX4log.m
+199
View File
@@ -0,0 +1,199 @@
function allData = importPX4log(fname,keep_msgs)
% import a .px4log file
% INPUTS
% fname: path to a valid .px4log file
% keep_msgs: cell array of message names to keep
% OUTPUT
% allData: a Matlab struct with a field names for each message name in the log
% file. The content of each field is itself a struct with field names for
% each label in the corresponding message. The content of each of *these*
% fields will be an array of message data that is nonempty if the message
% name appears in keep_msgs
% import method essentially translated from sdlog2_dump.py
% George Hines, 3D Robotics, Berkeley, CA
% 7 April 2016
BLOCK_SIZE = 8192;
MSG_HEADER_LEN = 3;
MSG_HEAD1 = uint8(hex2dec('A3'));
MSG_HEAD2 = uint8(hex2dec('95'));
MSG_FORMAT_PACKET_LEN = 89;
MSG_TYPE_FORMAT = uint8(hex2dec('80'));
% MSG_FORMAT_STRUCT = {'uint8','uint8','char4','char16','char64'};
FORMAT_TO_STRUCT = {
'b', {'int8', 1};
'B', {'uint8', 1};
'h', {'int16', 1};
'H', {'uint16', 1};
'i', {'int32', 1};
'I', {'uint32', 1};
'f', {'single', 1};
'd', {'double', 1};
'n', {'char4', 1};
'N', {'char16', 1};
'Z', {'char64', 1};
'c', {'int16', 0.01};
'C', {'uint16', 0.01};
'e', {'int32', 0.01};
'E', {'uint32', 0.01};
'L', {'int32', 0.0000001};
'M', {'uint8', 1};
'q', {'int64', 1};
'Q', {'uint64', 1}};
fid = fopen(fname,'r','b');
fInfo = dir(fname);
totalBytes = fInfo.bytes;
bytes_read = 0;
msg_descrs = {};
buffer = [];
ptr = 1;
allData = [];
nextPrint = 0;
disp('Reading file');
while 1
percentDone = bytes_read / totalBytes * 100;
if percentDone >= nextPrint
fprintf('%.0f%%\n',percentDone);
nextPrint = nextPrint + 5;
end
chunk = fread(fid,BLOCK_SIZE,'uint8');
if numel(chunk) == 0;
break
end
buffer = [buffer(ptr:end), chunk'];
ptr = 1;
while numel(buffer) - ptr > MSG_HEADER_LEN
head1 = buffer(ptr);
head2 = buffer(ptr+1);
if head1 ~= MSG_HEAD1 || head2 ~= MSG_HEAD2
ptr = ptr + 1;
continue;
end
msg_type = buffer(ptr+2);
if msg_type == MSG_TYPE_FORMAT
if numel(buffer) - ptr <= MSG_FORMAT_PACKET_LEN
break;
end
% return new pointer, and all message descriptor info
[ptr, msg_descr] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_PACKET_LEN, FORMAT_TO_STRUCT);
msg_descrs(msg_descr{1},:) = msg_descr;
cells = repmat({inf(1,500000)},1,numel(msg_descr{5}));
cells(msg_descr{4}=='n' | msg_descr{4} == 'N' | msg_descr{4} == 'Z') = {[]};
seed = [{'index'},{'Tsec'},msg_descr{5};[{1},{inf(1,500000)},cells]];
allData.(msg_descr{3}) = struct(seed{:});
else
msg_descr = msg_descrs(msg_type,:);
msg_length = msg_descr{2};
if numel(buffer) - ptr <= msg_length
break;
end
% return new pointer, and all message info
if strcmp(msg_descr{3},'TIME') || any(strcmp(msg_descr{3}, keep_msgs)) || isempty(keep_msgs)
[ptr,msg_data] = LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr);
ind = allData.(msg_descr{3}).index;
for k = 1:numel(msg_data)
if isnumeric(msg_data{k})
allData.(msg_descr{3}).(msg_descr{5}{k})(ind) = msg_data{k};
try
allData.(msg_descr{3}).Tsec(ind) = double(allData.TIME.StartTime(max(1,allData.TIME.index-1)))*1e-6;
end
noInc = false;
else
allData.(msg_descr{3}).(msg_descr{5}{k}) = [allData.(msg_descr{3}).(msg_descr{5}{k}), msg_data(k)];
noInc = true;
end
end
if ~noInc
allData.(msg_descr{3}).index = ind + 1;
end
else
ptr = ptr + msg_descr{2};
end
end
end
bytes_read = bytes_read + ptr;
end
disp('Releasing excess preallocated memory');
% clean out inf values
fields1 = fieldnames(allData);
for k = 1:numel(fields1)
fields2 = fieldnames(allData.(fields1{k}));
for m = 1:numel(fields2)
if isnumeric(allData.(fields1{k}).(fields2{m}))
allData.(fields1{k}).(fields2{m})(isinf(allData.(fields1{k}).(fields2{m}))) = [];
end
end
end
disp('Done');
end
function [ptr, msg_descr] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_PACKET_LEN, FORMAT_TO_STRUCT)
thisBlock = buffer((ptr+3):(ptr+MSG_FORMAT_PACKET_LEN+1));
msg_descr = cell(1,7);
msg_type = thisBlock(1);
%if msg_type ~= MSG_TYPE_FORMAT
msg_length = thisBlock(2);
msg_name = LOCAL_parse_string(thisBlock(3:6));
msg_format = LOCAL_parse_string(thisBlock(7:22));
msg_labels = strsplit(LOCAL_parse_string(thisBlock(23:end)),',');
msg_struct = cell(1,numel(msg_format));
msg_mults = zeros(1,numel(msg_format));
for k = 1:numel(msg_format)
info = FORMAT_TO_STRUCT{strcmp(msg_format(k),FORMAT_TO_STRUCT(:,1)),2};
msg_struct{k} = info{1};
msg_mults(k) = info{2};
end
if isempty([msg_labels{:}])
msg_labels = {'none'};
end
msg_descr = {msg_type, msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults};
%end
ptr = ptr + MSG_FORMAT_PACKET_LEN;
end
function [ptr, data] = LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr)
[~, msg_length, ~, ~, ~, msg_struct, msg_mults] = msg_descr{:};
% unpack message per the format specifiers in msg_struct
data = cell(1,numel(msg_struct));
thisBytes = buffer((ptr+MSG_HEADER_LEN):(ptr+msg_length+1));
thisPtr = 1;
for k = 1:numel(msg_struct)
% parse the data chunk per msg_struct{k}
[dataLen,data{k}] = LOCAL_unpack(thisPtr, thisBytes, msg_struct{k}, msg_mults(k));
thisPtr = thisPtr + dataLen;
end
ptr = ptr + msg_length;
end
function [dataLen, data] = LOCAL_unpack(thisPtr, byte_array, format_type, mult)
if strncmp('char',format_type,4)
dataLen = str2double(format_type(5:end));
data = LOCAL_parse_string(byte_array(thisPtr:(thisPtr+dataLen)));
else
if strncmp('int',format_type,3)
dataLen = str2double(format_type(4:end))/8;
elseif strncmp('uint',format_type,4)
dataLen = str2double(format_type(5:end))/8;
elseif strcmp('single',format_type)
dataLen = 4;
end
data = double(typecast(uint8(byte_array(thisPtr:(thisPtr+dataLen-1))),format_type))*mult;
end
end
function str = LOCAL_parse_string(byteArray)
firstZero = find(byteArray==0);
if ~isempty(firstZero)
str = char(byteArray(1:firstZero-1));
else
str = char(byteArray);
end
end
EKF/matlab/analysis/usageSamples.m
+16
View File
@@ -0,0 +1,16 @@
fname = 'indoor_flight_test.px4log';
%% entire log
wholeLog = importPX4log(fname,{});
%% attitude message
attitudeData = importPX4log(fname,{'ATT'});
%% estimator messages
estimatorData = importPX4log(fname,{'EST0','EST1','EST2','EST3','EST4','EST5','EST6'});
%% to save
save ift estimatorData;
%% to run viewer
estimatorLogViewer('ift.mat');
EKF/matlab/generated/Inertial Nav EKF/Airspeed Fusion.txt
+50
View File
@@ -0,0 +1,50 @@
// Auto code for fusion of true airspeed
// Calculate the observation jacobian
// intermediate variable from algebraic optimisation
float SH_TAS[3];
SH_TAS[0] = 1.0f/v_tas_pred;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
// observation jacobian
float H_TAS[24];
H_TAS[4] = SH_TAS[2];
H_TAS[5] = SH_TAS[1];
H_TAS[6] = vd*SH_TAS[0];
H_TAS[22] = -SH_TAS[2];
H_TAS[23] = -SH_TAS[1];
// calculate the Kalman gain matrix
// intermediate variables - note SK_TAS[0] is 1/(innovation variance)
float SK_TAS[2];
SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
SK_TAS[1] = SH_TAS[1];
float Kfusion[24];
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]);
Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]);
Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]);
Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
EKF/matlab/generated/Inertial Nav EKF/Covariance Prediction.txt
+367
View File
@@ -0,0 +1,367 @@
// Auto code for covariance prediction
// Intermediate expressions obtained from algebraic optimisation
float SF[21] = {};
SF[0] = dvz - dvz_b;
SF[1] = dvy - dvy_b;
SF[2] = dvx - dvx_b;
SF[3] = 2.0f*q1*SF[2] + 2.0f*q2*SF[1] + 2.0f*q3*SF[0];
SF[4] = 2.0f*q0*SF[1] - 2.0f*q1*SF[0] + 2.0f*q3*SF[2];
SF[5] = 2.0f*q0*SF[2] + 2.0f*q2*SF[0] - 2.0f*q3*SF[1];
SF[6] = day*0.5f - day_b*0.5f;
SF[7] = daz*0.5f - daz_b*0.5f;
SF[8] = dax*0.5f - dax_b*0.5f;
SF[9] = dax_b*0.5f - dax*0.5f;
SF[10] = daz_b*0.5f - daz*0.5f;
SF[11] = day_b*0.5f - day*0.5f;
SF[12] = 2.0f*q1*SF[1];
SF[13] = 2.0f*q0*SF[0];
SF[14] = q1*0.5f;
SF[15] = q2*0.5f;
SF[16] = q3*0.5f;
SF[17] = sq(q3);
SF[18] = sq(q2);
SF[19] = sq(q1);
SF[20] = sq(q0);
float SG[8] = {};
SG[0] = q0*0.5f;
SG[1] = sq(q3);
SG[2] = sq(q2);
SG[3] = sq(q1);
SG[4] = sq(q0);
SG[5] = 2.0f*q2*q3;
SG[6] = 2.0f*q1*q3;
SG[7] = 2.0f*q1*q2;
float SQ[11] = {};
SQ[0] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[7] + 2.0f*q0*q3);
SQ[1] = dvzVar*(SG[6] + 2.0f*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[7] - 2.0f*q0*q3);
SQ[2] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[6] + 2.0f*q0*q2) - dvyVar*(SG[7] - 2.0f*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2.0f*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f;
SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f;
SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f;
SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f;
SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f;
SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f;
SQ[9] = sq(SG[0]);
SQ[10] = sq(q1);
float SPP[11] = {};
SPP[0] = SF[12] + SF[13] - 2.0f*q2*SF[2];
SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
SPP[4] = 2.0f*q0*q2 - 2.0f*q1*q3;
SPP[5] = 2.0f*q0*q1 - 2.0f*q2*q3;
SPP[6] = 2.0f*q0*q3 - 2.0f*q1*q2;
SPP[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
SPP[8] = 2.0f*q0*q3 + 2.0f*q1*q2;
SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3;
SPP[10] = SF[16];
// Calculate uppder diagonal elements of the predicted covariance matrix
// Use symmetry to assign value to lower diagonal
// Note: this matrix does not include the process noise for stationary states, it only includes the effect of noise on the inertial measurements.
// Process noise for stationary states must be added later.
float nextP[24][24];
nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f;
nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f;
nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f;
nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))*0.5f;
nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)*0.5f + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))*0.5f;
nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P[11][2]*q0)*0.5f + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))*0.25f - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))*0.5f;
nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))*0.5f;
nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)*0.5f + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))*0.5f;
nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)*0.5f + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))*0.5f;
nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P[12][3]*q0)*0.5f + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))*0.25f - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))*0.5f;
nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)*0.5f + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)*0.5f + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)*0.5f + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2.0f*q0*q3) + dvzVar*sq(SG[6] + 2.0f*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)*0.5f + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)*0.5f + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)*0.5f + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2.0f*q0*q3) + dvzVar*sq(SG[5] - 2.0f*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)*0.5f + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)*0.5f + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)*0.5f + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]);
nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2.0f*q0*q2) + dvyVar*sq(SG[5] + 2.0f*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]);
nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)*0.5f + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2);
nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)*0.5f + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2);
nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)*0.5f + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2);
nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]);
nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]);
nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]);
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]);
nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)*0.5f + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2);
nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)*0.5f + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2);
nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)*0.5f + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2);
nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]);
nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]);
nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]);
nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]);
nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)*0.5f + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2);
nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)*0.5f + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2);
nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)*0.5f + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2);
nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]);
nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]);
nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]);
nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10];
nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f;
nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f;
nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)*0.5f;
nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9];
nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5];
nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1];
nextP[7][10] = P[7][10] + P[4][10]*dt;
nextP[8][10] = P[8][10] + P[5][10]*dt;
nextP[9][10] = P[9][10] + P[6][10]*dt;
nextP[10][10] = P[10][10];
nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10];
nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)*0.5f;
nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)*0.5f;
nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)*0.5f;
nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9];
nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5];
nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1];
nextP[7][11] = P[7][11] + P[4][11]*dt;
nextP[8][11] = P[8][11] + P[5][11]*dt;
nextP[9][11] = P[9][11] + P[6][11]*dt;
nextP[10][11] = P[10][11];
nextP[11][11] = P[11][11];
nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10];
nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)*0.5f;
nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)*0.5f;
nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)*0.5f;
nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9];
nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5];
nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1];
nextP[7][12] = P[7][12] + P[4][12]*dt;
nextP[8][12] = P[8][12] + P[5][12]*dt;
nextP[9][12] = P[9][12] + P[6][12]*dt;
nextP[10][12] = P[10][12];
nextP[11][12] = P[11][12];
nextP[12][12] = P[12][12];
nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10];
nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f;
nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f;
nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)*0.5f;
nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9];
nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5];
nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1];
nextP[7][13] = P[7][13] + P[4][13]*dt;
nextP[8][13] = P[8][13] + P[5][13]*dt;
nextP[9][13] = P[9][13] + P[6][13]*dt;
nextP[10][13] = P[10][13];
nextP[11][13] = P[11][13];
nextP[12][13] = P[12][13];
nextP[13][13] = P[13][13];
nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10];
nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)*0.5f;
nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)*0.5f;
nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)*0.5f;
nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9];
nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5];
nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1];
nextP[7][14] = P[7][14] + P[4][14]*dt;
nextP[8][14] = P[8][14] + P[5][14]*dt;
nextP[9][14] = P[9][14] + P[6][14]*dt;
nextP[10][14] = P[10][14];
nextP[11][14] = P[11][14];
nextP[12][14] = P[12][14];
nextP[13][14] = P[13][14];
nextP[14][14] = P[14][14];
nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10];
nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f;
nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f;
nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)*0.5f;
nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9];
nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5];
nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1];
nextP[7][15] = P[7][15] + P[4][15]*dt;
nextP[8][15] = P[8][15] + P[5][15]*dt;
nextP[9][15] = P[9][15] + P[6][15]*dt;
nextP[10][15] = P[10][15];
nextP[11][15] = P[11][15];
nextP[12][15] = P[12][15];
nextP[13][15] = P[13][15];
nextP[14][15] = P[14][15];
nextP[15][15] = P[15][15];
nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10];
nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)*0.5f;
nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)*0.5f;
nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)*0.5f;
nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9];
nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5];
nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1];
nextP[7][16] = P[7][16] + P[4][16]*dt;
nextP[8][16] = P[8][16] + P[5][16]*dt;
nextP[9][16] = P[9][16] + P[6][16]*dt;
nextP[10][16] = P[10][16];
nextP[11][16] = P[11][16];
nextP[12][16] = P[12][16];
nextP[13][16] = P[13][16];
nextP[14][16] = P[14][16];
nextP[15][16] = P[15][16];
nextP[16][16] = P[16][16];
nextP[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10];
nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)*0.5f;
nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)*0.5f;
nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)*0.5f;
nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9];
nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5];
nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1];
nextP[7][17] = P[7][17] + P[4][17]*dt;
nextP[8][17] = P[8][17] + P[5][17]*dt;
nextP[9][17] = P[9][17] + P[6][17]*dt;
nextP[10][17] = P[10][17];
nextP[11][17] = P[11][17];
nextP[12][17] = P[12][17];
nextP[13][17] = P[13][17];
nextP[14][17] = P[14][17];
nextP[15][17] = P[15][17];
nextP[16][17] = P[16][17];
nextP[17][17] = P[17][17];
nextP[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10];
nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)*0.5f;
nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)*0.5f;
nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)*0.5f;
nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9];
nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5];
nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1];
nextP[7][18] = P[7][18] + P[4][18]*dt;
nextP[8][18] = P[8][18] + P[5][18]*dt;
nextP[9][18] = P[9][18] + P[6][18]*dt;
nextP[10][18] = P[10][18];
nextP[11][18] = P[11][18];
nextP[12][18] = P[12][18];
nextP[13][18] = P[13][18];
nextP[14][18] = P[14][18];
nextP[15][18] = P[15][18];
nextP[16][18] = P[16][18];
nextP[17][18] = P[17][18];
nextP[18][18] = P[18][18];
nextP[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10];
nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)*0.5f;
nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)*0.5f;
nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)*0.5f;
nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9];
nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5];
nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1];
nextP[7][19] = P[7][19] + P[4][19]*dt;
nextP[8][19] = P[8][19] + P[5][19]*dt;
nextP[9][19] = P[9][19] + P[6][19]*dt;
nextP[10][19] = P[10][19];
nextP[11][19] = P[11][19];
nextP[12][19] = P[12][19];
nextP[13][19] = P[13][19];
nextP[14][19] = P[14][19];
nextP[15][19] = P[15][19];
nextP[16][19] = P[16][19];
nextP[17][19] = P[17][19];
nextP[18][19] = P[18][19];
nextP[19][19] = P[19][19];
nextP[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10];
nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)*0.5f;
nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)*0.5f;
nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)*0.5f;
nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9];
nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5];
nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1];
nextP[7][20] = P[7][20] + P[4][20]*dt;
nextP[8][20] = P[8][20] + P[5][20]*dt;
nextP[9][20] = P[9][20] + P[6][20]*dt;
nextP[10][20] = P[10][20];
nextP[11][20] = P[11][20];
nextP[12][20] = P[12][20];
nextP[13][20] = P[13][20];
nextP[14][20] = P[14][20];
nextP[15][20] = P[15][20];
nextP[16][20] = P[16][20];
nextP[17][20] = P[17][20];
nextP[18][20] = P[18][20];
nextP[19][20] = P[19][20];
nextP[20][20] = P[20][20];
nextP[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10];
nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)*0.5f;
nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)*0.5f;
nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)*0.5f;
nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9];
nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5];
nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1];
nextP[7][21] = P[7][21] + P[4][21]*dt;
nextP[8][21] = P[8][21] + P[5][21]*dt;
nextP[9][21] = P[9][21] + P[6][21]*dt;
nextP[10][21] = P[10][21];
nextP[11][21] = P[11][21];
nextP[12][21] = P[12][21];
nextP[13][21] = P[13][21];
nextP[14][21] = P[14][21];
nextP[15][21] = P[15][21];
nextP[16][21] = P[16][21];
nextP[17][21] = P[17][21];
nextP[18][21] = P[18][21];
nextP[19][21] = P[19][21];
nextP[20][21] = P[20][21];
nextP[21][21] = P[21][21];
nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10];
nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)*0.5f;
nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)*0.5f;
nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)*0.5f;
nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9];
nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5];
nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1];
nextP[7][22] = P[7][22] + P[4][22]*dt;
nextP[8][22] = P[8][22] + P[5][22]*dt;
nextP[9][22] = P[9][22] + P[6][22]*dt;
nextP[10][22] = P[10][22];
nextP[11][22] = P[11][22];
nextP[12][22] = P[12][22];
nextP[13][22] = P[13][22];
nextP[14][22] = P[14][22];
nextP[15][22] = P[15][22];
nextP[16][22] = P[16][22];
nextP[17][22] = P[17][22];
nextP[18][22] = P[18][22];
nextP[19][22] = P[19][22];
nextP[20][22] = P[20][22];
nextP[21][22] = P[21][22];
nextP[22][22] = P[22][22];
nextP[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10];
nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)*0.5f;
nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)*0.5f;
nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)*0.5f;
nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9];
nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5];
nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1];
nextP[7][23] = P[7][23] + P[4][23]*dt;
nextP[8][23] = P[8][23] + P[5][23]*dt;
nextP[9][23] = P[9][23] + P[6][23]*dt;
nextP[10][23] = P[10][23];
nextP[11][23] = P[11][23];
nextP[12][23] = P[12][23];
nextP[13][23] = P[13][23];
nextP[14][23] = P[14][23];
nextP[15][23] = P[15][23];
nextP[16][23] = P[16][23];
nextP[17][23] = P[17][23];
nextP[18][23] = P[18][23];
nextP[19][23] = P[19][23];
nextP[20][23] = P[20][23];
nextP[21][23] = P[21][23];
nextP[22][23] = P[22][23];
nextP[23][23] = P[23][23];
EKF/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt
+67
View File
@@ -0,0 +1,67 @@
/*
Autocode for fusion of a magnetic declination estimate where the innovation is given by
innovation = atanf(magMeasEarthFrameEast/magMeasEarthFrameNorth) - declinationAngle;
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to earth frame.
declinationAngle is the estimated declination as that location
This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements
(e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors
can cause the magnetic declination of the earth field estimates to slowly rotate.
Divide by zero protection and protection against a badly conditioned covariance matrix must be included.
*/
// Calculate intermediate variable
float t2 = magE*magE;
float t3 = magN*magN;
float t4 = t2+t3;
float t5 = P[16][16]*t2;
float t6 = P[17][17]*t3;
float t7 = t2*t2;
float t8 = R_DECL*t7;
float t9 = t3*t3;
float t10 = R_DECL*t9;
float t11 = R_DECL*t2*t3*2.0f;
float t14 = P[16][17]*magE*magN;
float t15 = P[17][16]*magE*magN;
float t12 = t5+t6+t8+t10+t11-t14-t15;
float t13 = 1.0f / t12;
float t16 = magE;
float t17 = magN;
float t18 = t16*t16;
float t19 = t17*t17;
float t20 = t18+t19;
float t21 = 1.0f/t20;
// Calculate the observation Jacobian
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
H_DECL[16] = -t16*t21;
H_DECL[17] = t17*t21;
// Calculate the Kalman gains
Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN);
Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN);
Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN);
Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN);
Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN);
Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN);
Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN);
Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN);
Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN);
Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN);
Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN);
Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN);
Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN);
Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN);
Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN);
Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN);
Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN);
Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN);
Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
EKF/matlab/generated/Inertial Nav EKF/Magnetometer Fusion.txt
+142
View File
@@ -0,0 +1,142 @@
// Auto code for fusion of XYZ magnetometer vector measurement
// Sequential fusion of each axis is used (assumes uncorrrelated observation errors)
// common expressions for XYZ axis observation jacobians
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
SH_MAG[3] = sq(q3);
SH_MAG[4] = sq(q2);
SH_MAG[5] = sq(q1);
SH_MAG[6] = sq(q0);
SH_MAG[7] = 2.0f*magN*q0;
SH_MAG[8] = 2.0f*magE*q3;
// X axis observation jacobians
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
H_MAG[1] = SH_MAG[0];
H_MAG[2] = -SH_MAG[1];
H_MAG[3] = SH_MAG[2];
H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2;
H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2;
H_MAG[19] = 1.0f;
// common expressions for X axis Kalman gains
SK_MX[0] = 1.0f/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3;
SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
// X axis Kalman gains
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]);
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]);
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]);
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]);
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]);
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]);
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]);
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]);
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]);
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]);
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]);
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]);
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]);
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]);
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]);
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]);
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]);
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
// Y axis observation Jacobians
H_MAG[0] = SH_MAG[2];
H_MAG[1] = SH_MAG[1];
H_MAG[2] = SH_MAG[0];
H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3;
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3;
H_MAG[20] = 1;
// Common expressions for Y axis Kalamn gains
SK_MY[0] = 1.0f/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2;
SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3;
// Y axis Kalmna gains
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
// Z axis observation Jacobians
H_MAG[0] = SH_MAG[1];
H_MAG[1] = -SH_MAG[2];
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
H_MAG[3] = SH_MAG[0];
H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3;
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
H_MAG[21] = 1;
// Common expressions for Z axis Kalman gains
SK_MZ[0] = 1.0f/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3;
// Z axis Kalman gains
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]);
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]);
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]);
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]);
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]);
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]);
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]);
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]);
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]);
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]);
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]);
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]);
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]);
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]);
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]);
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]);
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]);
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
EKF/matlab/generated/Inertial Nav EKF/Notes.txt
+22
View File
@@ -0,0 +1,22 @@
NOTES FOR FUSION OF MEASUREMENTS USING AUTOCODE FRAGMENTS
The auto-code for the fusion of the various measurements provides in most cases the observation Jacobian and Kalman gain matrix for that observation.
Where no Kalman gain is provided, it will need to be calculated using the usual K = P*transpose(H)/(H*P*transpose(H) + R) where:
K = Kalman Gain matrix
H = observation partial derivative matrix (observaton Jacobian)
R = observation variance
(H*P*transpose(H) + R) is the innovation variance (always a scalar)
When the observation is a vector, it is always assumed that the errors in the vectors are uncorelated to each other and the observations are fused
sequentially, so no matrix inversion is required.
It is important that maximum useage of the sparsity in the H matrix be taken advantage of. If a sparse math library is available it could be used,
otherwise the matrix operations should need unrolled with inclusion of conditional statements to improve efficiency. Examples of this technique can
be found in the existing att_pos_ekf_estimator library
NOTES FOR COVARIANCE PREDICTION
Only expression for the upper diagonal is provided. The values will need to be copied across to the lower diagnal elements assuming symmetry
Process noise for time invariant states, eg Gyro bias, has not been included in the auto-code. The process noise variance for these states will
need to be added in a separate operation.
EKF/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt
+267
View File
@@ -0,0 +1,267 @@
// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis
// Conversion from Matlab symbolic toolbox objects to c-code made using custom scripts and auto-coder from the InertialNav repo
// Observations are body modtion compensated optica flow rates about the X and Y body axis
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
float H_LOS[2][24]; // Optical flow observation Jacobians
float Kfusion[24][2]; // Optical flow Kalman gains
// calculate X axis observation Jacobian
float t2 = 1.0f / range;
H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
float t3 = q1*vd*2.0f;
float t4 = q0*ve*2.0f;
float t11 = q3*vn*2.0f;
float t5 = t3+t4-t11;
float t6 = q0*q3*2.0f;
float t29 = q1*q2*2.0f;
float t7 = t6-t29;
float t8 = q0*q1*2.0f;
float t9 = q2*q3*2.0f;
float t10 = t8+t9;
float t12 = P[0][0]*t2*t5;
float t13 = q0*vd*2.0f;
float t14 = q2*vn*2.0f;
float t28 = q1*ve*2.0f;
float t15 = t13+t14-t28;
float t16 = q3*vd*2.0f;
float t17 = q2*ve*2.0f;
float t18 = q1*vn*2.0f;
float t19 = t16+t17+t18;
float t20 = q3*ve*2.0f;
float t21 = q0*vn*2.0f;
float t30 = q2*vd*2.0f;
float t22 = t20+t21-t30;
float t23 = q0*q0;
float t24 = q1*q1;
float t25 = q2*q2;
float t26 = q3*q3;
float t27 = t23-t24+t25-t26;
float t31 = P[1][1]*t2*t15;
float t32 = P[6][0]*t2*t10;
float t33 = P[1][0]*t2*t15;
float t34 = P[2][0]*t2*t19;
float t35 = P[5][0]*t2*t27;
float t79 = P[4][0]*t2*t7;
float t80 = P[3][0]*t2*t22;
float t36 = t12+t32+t33+t34+t35-t79-t80;
float t37 = t2*t5*t36;
float t38 = P[6][1]*t2*t10;
float t39 = P[0][1]*t2*t5;
float t40 = P[2][1]*t2*t19;
float t41 = P[5][1]*t2*t27;
float t81 = P[4][1]*t2*t7;
float t82 = P[3][1]*t2*t22;
float t42 = t31+t38+t39+t40+t41-t81-t82;
float t43 = t2*t15*t42;
float t44 = P[6][2]*t2*t10;
float t45 = P[0][2]*t2*t5;
float t46 = P[1][2]*t2*t15;
float t47 = P[2][2]*t2*t19;
float t48 = P[5][2]*t2*t27;
float t83 = P[4][2]*t2*t7;
float t84 = P[3][2]*t2*t22;
float t49 = t44+t45+t46+t47+t48-t83-t84;
float t50 = t2*t19*t49;
float t51 = P[6][3]*t2*t10;
float t52 = P[0][3]*t2*t5;
float t53 = P[1][3]*t2*t15;
float t54 = P[2][3]*t2*t19;
float t55 = P[5][3]*t2*t27;
float t85 = P[4][3]*t2*t7;
float t86 = P[3][3]*t2*t22;
float t56 = t51+t52+t53+t54+t55-t85-t86;
float t57 = P[6][5]*t2*t10;
float t58 = P[0][5]*t2*t5;
float t59 = P[1][5]*t2*t15;
float t60 = P[2][5]*t2*t19;
float t61 = P[5][5]*t2*t27;
float t88 = P[4][5]*t2*t7;
float t89 = P[3][5]*t2*t22;
float t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62;
float t64 = P[6][4]*t2*t10;
float t65 = P[0][4]*t2*t5;
float t66 = P[1][4]*t2*t15;
float t67 = P[2][4]*t2*t19;
float t68 = P[5][4]*t2*t27;
float t90 = P[4][4]*t2*t7;
float t91 = P[3][4]*t2*t22;
float t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = P[6][6]*t2*t10;
float t71 = P[0][6]*t2*t5;
float t72 = P[1][6]*t2*t15;
float t73 = P[2][6]*t2*t19;
float t74 = P[5][6]*t2*t27;
float t93 = P[4][6]*t2*t7;
float t94 = P[3][6]*t2*t22;
float t75 = t70+t71+t72+t73+t74-t93-t94;
float t76 = t2*t10*t75;
float t87 = t2*t22*t56;
float t92 = t2*t7*t69;
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
float t78 = 1.0f / t77;
// calculate Kalman gains for X-axis observation
Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
// calculate Y axis observation Jacobian
float t2 = 1.0f / range;
H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
// calculate intermediate variables for the Y axis observaton innovation variance and Kalman gains
float t3 = q3*ve*2.0f;
float t4 = q0*vn*2.0f;
float t11 = q2*vd*2.0f;
float t5 = t3+t4-t11;
float t6 = q0*q3*2.0f;
float t7 = q1*q2*2.0f;
float t8 = t6+t7;
float t9 = q0*q2*2.0f;
float t28 = q1*q3*2.0f;
float t10 = t9-t28;
float t12 = P[0][0]*t2*t5;
float t13 = q3*vd*2.0f;
float t14 = q2*ve*2.0f;
float t15 = q1*vn*2.0f;
float t16 = t13+t14+t15;
float t17 = q0*vd*2.0f;
float t18 = q2*vn*2.0f;
float t29 = q1*ve*2.0f;
float t19 = t17+t18-t29;
float t20 = q1*vd*2.0f;
float t21 = q0*ve*2.0f;
float t30 = q3*vn*2.0f;
float t22 = t20+t21-t30;
float t23 = q0*q0;
float t24 = q1*q1;
float t25 = q2*q2;
float t26 = q3*q3;
float t27 = t23+t24-t25-t26;
float t31 = P[1][1]*t2*t16;
float t32 = P[5][0]*t2*t8;
float t33 = P[1][0]*t2*t16;
float t34 = P[3][0]*t2*t22;
float t35 = P[4][0]*t2*t27;
float t80 = P[6][0]*t2*t10;
float t81 = P[2][0]*t2*t19;
float t36 = t12+t32+t33+t34+t35-t80-t81;
float t37 = t2*t5*t36;
float t38 = P[5][1]*t2*t8;
float t39 = P[0][1]*t2*t5;
float t40 = P[3][1]*t2*t22;
float t41 = P[4][1]*t2*t27;
float t82 = P[6][1]*t2*t10;
float t83 = P[2][1]*t2*t19;
float t42 = t31+t38+t39+t40+t41-t82-t83;
float t43 = t2*t16*t42;
float t44 = P[5][2]*t2*t8;
float t45 = P[0][2]*t2*t5;
float t46 = P[1][2]*t2*t16;
float t47 = P[3][2]*t2*t22;
float t48 = P[4][2]*t2*t27;
float t79 = P[2][2]*t2*t19;
float t84 = P[6][2]*t2*t10;
float t49 = t44+t45+t46+t47+t48-t79-t84;
float t50 = P[5][3]*t2*t8;
float t51 = P[0][3]*t2*t5;
float t52 = P[1][3]*t2*t16;
float t53 = P[3][3]*t2*t22;
float t54 = P[4][3]*t2*t27;
float t86 = P[6][3]*t2*t10;
float t87 = P[2][3]*t2*t19;
float t55 = t50+t51+t52+t53+t54-t86-t87;
float t56 = t2*t22*t55;
float t57 = P[5][4]*t2*t8;
float t58 = P[0][4]*t2*t5;
float t59 = P[1][4]*t2*t16;
float t60 = P[3][4]*t2*t22;
float t61 = P[4][4]*t2*t27;
float t88 = P[6][4]*t2*t10;
float t89 = P[2][4]*t2*t19;
float t62 = t57+t58+t59+t60+t61-t88-t89;
float t63 = t2*t27*t62;
float t64 = P[5][5]*t2*t8;
float t65 = P[0][5]*t2*t5;
float t66 = P[1][5]*t2*t16;
float t67 = P[3][5]*t2*t22;
float t68 = P[4][5]*t2*t27;
float t90 = P[6][5]*t2*t10;
float t91 = P[2][5]*t2*t19;
float t69 = t64+t65+t66+t67+t68-t90-t91;
float t70 = t2*t8*t69;
float t71 = P[5][6]*t2*t8;
float t72 = P[0][6]*t2*t5;
float t73 = P[1][6]*t2*t16;
float t74 = P[3][6]*t2*t22;
float t75 = P[4][6]*t2*t27;
float t92 = P[6][6]*t2*t10;
float t93 = P[2][6]*t2*t19;
float t76 = t71+t72+t73+t74+t75-t92-t93;
float t85 = t2*t19*t49;
float t94 = t2*t10*t76;
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
float t78 = 1.0f / t77;
// calculate Kalman gains for Y-axis observation
Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
EKF/matlab/generated/Inertial Nav EKF/Sideslip Fusion.txt
+69
View File
File diff suppressed because one or more lines are too long
EKF/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt
+42
View File
@@ -0,0 +1,42 @@
/*
Observation jacobian for fusion of the horizontal components of magnetometer measurements.
innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
by rotating the measured magnetometer readings from body into earth axes.
This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
Divide by zero protection hs been added
*/
// calculate intermediate variables for observation jacobian
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9+t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3+t4-t5-t6;
float t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
}
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
return;
}
// calculate observation jacobian
float H_DECL[4] = {};
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
EKF/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
+67
View File
@@ -0,0 +1,67 @@
/*
Code fragments for fusion of an Euler yaw measurement from a 321 sequence.
*/
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9+t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3+t4-t5-t6;
float t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
}
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
return;
}
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
/*
Code fragments for fusion of an Euler yaw measurement from a 312 sequence.
*/
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9-t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3-t4+t5-t6;
float t8 = t7*t7;
if (t8 > 1e-6f) {
t8 = 1.0f/t8;
} else {
return;
}
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
return;
}
H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f;
EKF/matlab/generated/README.md
+5
View File
@@ -0,0 +1,5 @@
# Generated Code
These directories contains code generated using the derivation script files that has been 'cleaned up'.
Inertia Nav EKF: contains code generated by GenerateNavFilterEquations.m
EKF/matlab/scripts/Inertial Nav EKF/Airspeed.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/C_code24.txt
+686
View File
File diff suppressed because one or more lines are too long
EKF/matlab/scripts/Inertial Nav EKF/ConvertToC.m
+213
View File
@@ -0,0 +1,213 @@
function ConvertToC(nStates)
%% Define file to read in
fileName = strcat('M_code',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);
%% 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 ^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('C_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;
EKF/matlab/scripts/Inertial Nav EKF/ConvertToM.m
+46
View File
@@ -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);
%% 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
EKF/matlab/scripts/Inertial Nav EKF/Drag.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/EulToQuat.m
+23
View File
@@ -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;
EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
+491
View File
@@ -0,0 +1,491 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% XYZ body fixed frame
% Sequential fusion of velocity and position measurements
% Fusion of true airspeed
% Sequential fusion of magnetic flux measurements
% 24 state architecture.
% IMU data is assumed to arrive at a constant rate with a time step of dt
% IMU delta angle and velocity data are used as control inputs,
% not observations
% Author: Paul Riseborough
% State vector:
% attitude quaternion
% Velocity - m/sec (North, East, Down)
% Position - m (North, East, Down)
% Delta Angle bias - rad (X,Y,Z)
% Delta Velocity bias - m/s (X,Y,Z)
% Earth Magnetic Field Vector - (North, East, Down)
% Body Magnetic Field Vector - (X,Y,Z)
% Wind Vector - m/sec (North,East)
% Observations:
% NED velocity - m/s
% NED position - m
% True airspeed - m/s
% angle of sideslip - rad
% XYZ magnetic flux
% Time varying parameters:
% XYZ delta angle measurements in body axes - rad
% XYZ delta velocity measurements in body axes - m/sec
%% define symbolic variables and constants
clear all;
reset(symengine);
syms dax day daz real % IMU delta angle measurements in body axes - rad
syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
syms vn ve vd real % NED velocity - m/sec
syms pn pe pd real % NED position - m
syms dax_b day_b daz_b real % delta angle bias - rad
syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
syms dt real % IMU time step - sec
syms gravity real % gravity - m/sec^2
syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
syms vwn vwe real; % NE wind velocity - m/sec
syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss
syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2
syms R_PN R_PE R_PD real % variances for NED position measurements - m^2
syms R_TAS real % variance for true airspeed measurement - (m/sec)^2
syms R_MAG real % variance for magnetic flux measurements - milligauss^2
syms R_BETA real % variance of sidelsip measurements rad^2
syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
syms ptd real % location of terrain in D axis
syms decl real; % earth magnetic field declination from true north
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
syms rho real % air density (kg/m^3)
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)
%% define the state prediction equations
% define the measured Delta angle and delta velocity vectors
dAngMeas = [dax; day; daz];
dVelMeas = [dvx; dvy; dvz];
% define the IMU bias errors and scale factor
dAngBias = [dax_b; day_b; daz_b];
dVelBias = [dvx_b; dvy_b; dvz_b];
% define the quaternion rotation vector for the state estimate
quat = [q0;q1;q2;q3];
% derive the truth body to nav direction cosine matrix
Tbn = Quat2Tbn(quat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas - dAngBias;
% Define the truth delta velocity -ignore sculling and transport rate
% corrections as these negligible are in terms of covariance growth for our
% application and grade of sensor
dVelTruth = dVelMeas - dVelBias;
% define the attitude update equations
% use a first order expansion of rotation to calculate the quaternion increment
% acceptable for propagation of covariances
deltaQuat = [1;
0.5*dAngTruth(1);
0.5*dAngTruth(2);
0.5*dAngTruth(3);
];
quatNew = QuatMult(quat,deltaQuat);
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
% define the position update equations
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
% define the IMU error update equations
dAngBiasNew = dAngBias;
dVelBiasNew = dVelBias;
% define the wind velocity update equations
vwnNew = vwn;
vweNew = vwe;
% define the earth magnetic field update equations
magNnew = magN;
magEnew = magE;
magDnew = magD;
% define the body magnetic field update equations
magXnew = magX;
magYnew = magY;
magZnew = magZ;
% Define the state vector & number of states
stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
nStates=numel(stateVector);
% Define vector of process equations
newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
% derive the state transition matrix
F = jacobian(newStateVector, stateVector);
% set the rotation error states to zero
[F,SF]=OptimiseAlgebra(F,'SF');
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
end
end
save 'StatePrediction.mat';
%% derive the covariance prediction equations
% This reduces the number of floating point operations by a factor of 6 or
% more compared to using the standard matrix operations in code
% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
% velocities, after bias effects have been removed.
% derive the control(disturbance) influence matrix from IMu noise to state
% noise
G = jacobian(newStateVector, [dAngMeas;dVelMeas]);
[G,SG]=OptimiseAlgebra(G,'SG');
% derive the state error matrix
distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]);
Q = G*distMatrix*transpose(G);
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
% Derive the predicted covariance matrix using the standard equation
PP = F*P*transpose(F) + Q;
% Collect common expressions to optimise processing
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
save('StateAndCovariancePrediction.mat');
clear all;
reset(symengine);
%% derive equations for fusion of true airspeed measurements
load('StatePrediction.mat');
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);
[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
% save equations and reset workspace
save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
clear all;
reset(symengine);
%% derive equations for fusion of angle of sideslip measurements
load('StatePrediction.mat');
% calculate wind relative velocities in nav frame and rotate into body frame
Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd];
% calculate predicted angle of sideslip using small angle assumption
BetaPred = Vbw(2)/Vbw(1);
H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
% save equations and reset workspace
save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
clear all;
reset(symengine);
%% derive equations for fusion of magnetic field measurement
load('StatePrediction.mat');
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
[H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');
K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
[K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX');
K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector
[K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY');
K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
% save equations and reset workspace
save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
clear all;
reset(symengine);
%% derive equations for sequential fusion of optical flow measurements
load('StatePrediction.mat');
% Range is defined as distance from camera focal point to object measured
% along sensor Z axis
syms range real;
% Define rotation matrix from body to sensor frame
syms Tbs_a_x Tbs_a_y Tbs_a_z real;
syms Tbs_b_x Tbs_b_y Tbs_b_z real;
syms Tbs_c_x Tbs_c_y Tbs_c_z real;
Tbs = [ ...
Tbs_a_x Tbs_a_y Tbs_a_z ; ...
Tbs_b_x Tbs_b_y Tbs_b_z ; ...
Tbs_c_x Tbs_c_y Tbs_c_z ...
];
% Calculate earth relative velocity in a non-rotating sensor frame
relVelSensor = Tbs * transpose(Tbn) * [vn;ve;vd];
% Divide by range to get predicted angular LOS rates relative to X and Y
% axes. Note these are rates in a non-rotating sensor frame
losRateSensorX = +relVelSensor(2)/range;
losRateSensorY = -relVelSensor(1)/range;
save('temp1.mat','losRateSensorX','losRateSensorY');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian and Kalman gain for the X axis
H_LOSX = jacobian(losRateSensorX,stateVector); % measurement Jacobian
H_LOSX = simplify(H_LOSX);
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
K_LOSX = simplify(K_LOSX);
save('temp2.mat','H_LOSX','K_LOSX');
ccode([H_LOSX;transpose(K_LOSX)],'file','LOSX.c');
fix_c_code('LOSX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Y axis
H_LOSY = jacobian(losRateSensorY,stateVector); % measurement Jacobian
H_LOSY = simplify(H_LOSY);
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector
K_LOSY = simplify(K_LOSY);
save('temp3.mat','H_LOSY','K_LOSY');
ccode([H_LOSY;transpose(K_LOSY)],'file','LOSY.c');
fix_c_code('LOSY.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for sequential fusion of body frame velocity measurements
load('StatePrediction.mat');
% body frame velocity observations
syms velX velY velZ real;
% velocity observation variance
syms R_VEL real;
% calculate relative velocity in body frame
relVelBody = transpose(Tbn)*[vn;ve;vd];
save('temp1.mat','relVelBody','R_VEL');
% calculate the observation Jacobian for the X axis
H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
H_VELX = simplify(H_VELX);
save('temp2.mat','H_VELX');
ccode(H_VELX,'file','H_VELX.c');
fix_c_code('H_VELX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Y axis
H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
H_VELY = simplify(H_VELY);
save('temp3.mat','H_VELY');
ccode(H_VELY,'file','H_VELY.c');
fix_c_code('H_VELY.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Z axis
H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
H_VELZ = simplify(H_VELZ);
save('temp4.mat','H_VELZ');
ccode(H_VELZ,'file','H_VELZ.c');
fix_c_code('H_VELZ.c');
clear all;
reset(symengine);
% calculate Kalman gain vector for the X axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELX = simplify(K_VELX);
ccode(K_VELX,'file','K_VELX.c');
fix_c_code('K_VELX.c');
clear all;
reset(symengine);
% calculate Kalman gain vector for the Y axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp3.mat');
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELY = simplify(K_VELY);
ccode(K_VELY,'file','K_VELY.c');
fix_c_code('K_VELY.c');
clear all;
reset(symengine);
% calculate Kalman gain vector for the Z axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp4.mat');
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VELZ = simplify(K_VELZ);
ccode(K_VELZ,'file','K_VELZ.c');
fix_c_code('K_VELZ.c');
% reset workspace
clear all;
reset(symengine);
% calculate Kalman gains vectors for X,Y,Z to take advantage of common
% terms
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
load('temp3.mat');
load('temp4.mat');
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VEL = simplify([K_VELX,K_VELY,K_VELZ]);
ccode(K_VEL,'file','K_VEL.c');
fix_c_code('K_VEL.c');
%% derive equations for fusion of 321 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from the 321 rotation sequence
angMeas = atan(Tbn(2,1)/Tbn(1,1));
H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
H_YAW321 = simplify(H_YAW321);
ccode(H_YAW321,'file','calcH_YAW321.c');
fix_c_code('calcH_YAW321.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of 312 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from an Euler 312 sequence
angMeas = atan(-Tbn(1,2)/Tbn(2,2));
H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
H_YAW312 = simplify(H_YAW312);
ccode(H_YAW312,'file','calcH_YAW312.c');
fix_c_code('calcH_YAW312.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of declination
load('StatePrediction.mat');
% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = atan(magE/magN);
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = simplify(H_MAGD);
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
K_MAGD = simplify(K_MAGD);
ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
fix_c_code('calcMAGD.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of lateral body acceleration (multirotors only)
load('StatePrediction.mat');
% use relationship between airspeed along the X and Y body axis and the
% drag to predict the lateral acceleration for a multirotor vehicle type
% where propulsion forces are generated primarily along the Z body axis
vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
% calculate drag assuming flight along axis in positive direction
% sign change will be looked after in implementation rather than by adding
% sign functions to symbolic derivation which genererates output with dirac
% functions
% accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
% Use a simple viscous drag model for the linear estimator equations
% Use the the derivative from speed to acceleration averaged across the
% speed range
% The nonlinear equation will be used to calculate the predicted
% measurement in implementation
accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis
% Derive observation Jacobian and Kalman gain matrix for X accel fusion
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
H_ACCX = simplify(H_ACCX);
[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
H_ACCY = simplify(H_ACCY);
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
% save equations and reset workspace
save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
clear all;
reset(symengine);
%% Save output and convert to m and c code fragments
% load equations for predictions and updates
load('StateAndCovariancePrediction.mat');
load('Airspeed.mat');
load('Sideslip.mat');
load('Magnetometer.mat');
load('Drag.mat');
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c
+8
View File
@@ -0,0 +1,8 @@
t2 = 1.0/range;
A0[0][0] = t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
A0[0][1] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
A0[0][2] = t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
A0[0][3] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
A0[0][4] = -t2*(q0*q3*2.0-q1*q2*2.0);
A0[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
A0[0][6] = t2*(q0*q1*2.0+q2*q3*2.0);
EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c
+8
View File
@@ -0,0 +1,8 @@
t2 = 1.0/range;
A0[0][0] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
A0[0][1] = -t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
A0[0][2] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
A0[0][3] = -t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
A0[0][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
A0[0][5] = -t2*(q0*q3*2.0+q1*q2*2.0);
A0[0][6] = t2*(q0*q2*2.0-q1*q3*2.0);
EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c
+7
View File
@@ -0,0 +1,7 @@
H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f;
H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3;
H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f;
H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f;
EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c
+7
View File
@@ -0,0 +1,7 @@
H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f;
H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f;
H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3;
H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f;
EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c
+7
View File
@@ -0,0 +1,7 @@
H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f;
H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f;
H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f;
H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3;
EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c
+117
View File
@@ -0,0 +1,117 @@
t2 = 1.0/range;
t3 = q1*vd*2.0;
t4 = q0*ve*2.0;
t11 = q3*vn*2.0;
t5 = t3+t4-t11;
t6 = q0*q3*2.0;
t29 = q1*q2*2.0;
t7 = t6-t29;
t8 = q0*q1*2.0;
t9 = q2*q3*2.0;
t10 = t8+t9;
t12 = P[0][0]*t2*t5;
t13 = q0*vd*2.0;
t14 = q2*vn*2.0;
t28 = q1*ve*2.0;
t15 = t13+t14-t28;
t16 = q3*vd*2.0;
t17 = q2*ve*2.0;
t18 = q1*vn*2.0;
t19 = t16+t17+t18;
t20 = q3*ve*2.0;
t21 = q0*vn*2.0;
t30 = q2*vd*2.0;
t22 = t20+t21-t30;
t23 = q0*q0;
t24 = q1*q1;
t25 = q2*q2;
t26 = q3*q3;
t27 = t23-t24+t25-t26;
t31 = P[1][1]*t2*t15;
t32 = P[6][0]*t2*t10;
t33 = P[1][0]*t2*t15;
t34 = P[2][0]*t2*t19;
t35 = P[5][0]*t2*t27;
t79 = P[4][0]*t2*t7;
t80 = P[3][0]*t2*t22;
t36 = t12+t32+t33+t34+t35-t79-t80;
t37 = t2*t5*t36;
t38 = P[6][1]*t2*t10;
t39 = P[0][1]*t2*t5;
t40 = P[2][1]*t2*t19;
t41 = P[5][1]*t2*t27;
t81 = P[4][1]*t2*t7;
t82 = P[3][1]*t2*t22;
t42 = t31+t38+t39+t40+t41-t81-t82;
t43 = t2*t15*t42;
t44 = P[6][2]*t2*t10;
t45 = P[0][2]*t2*t5;
t46 = P[1][2]*t2*t15;
t47 = P[2][2]*t2*t19;
t48 = P[5][2]*t2*t27;
t83 = P[4][2]*t2*t7;
t84 = P[3][2]*t2*t22;
t49 = t44+t45+t46+t47+t48-t83-t84;
t50 = t2*t19*t49;
t51 = P[6][3]*t2*t10;
t52 = P[0][3]*t2*t5;
t53 = P[1][3]*t2*t15;
t54 = P[2][3]*t2*t19;
t55 = P[5][3]*t2*t27;
t85 = P[4][3]*t2*t7;
t86 = P[3][3]*t2*t22;
t56 = t51+t52+t53+t54+t55-t85-t86;
t57 = P[6][5]*t2*t10;
t58 = P[0][5]*t2*t5;
t59 = P[1][5]*t2*t15;
t60 = P[2][5]*t2*t19;
t61 = P[5][5]*t2*t27;
t88 = P[4][5]*t2*t7;
t89 = P[3][5]*t2*t22;
t62 = t57+t58+t59+t60+t61-t88-t89;
t63 = t2*t27*t62;
t64 = P[6][4]*t2*t10;
t65 = P[0][4]*t2*t5;
t66 = P[1][4]*t2*t15;
t67 = P[2][4]*t2*t19;
t68 = P[5][4]*t2*t27;
t90 = P[4][4]*t2*t7;
t91 = P[3][4]*t2*t22;
t69 = t64+t65+t66+t67+t68-t90-t91;
t70 = P[6][6]*t2*t10;
t71 = P[0][6]*t2*t5;
t72 = P[1][6]*t2*t15;
t73 = P[2][6]*t2*t19;
t74 = P[5][6]*t2*t27;
t93 = P[4][6]*t2*t7;
t94 = P[3][6]*t2*t22;
t75 = t70+t71+t72+t73+t74-t93-t94;
t76 = t2*t10*t75;
t87 = t2*t22*t56;
t92 = t2*t7*t69;
t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
t78 = 1.0/t77;
A0[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
A0[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
A0[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
A0[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
A0[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
A0[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
A0[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
A0[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
A0[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
A0[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
A0[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
A0[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
A0[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
A0[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
A0[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
A0[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
A0[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
A0[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
A0[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
A0[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
A0[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
A0[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
A0[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
A0[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c
+117
View File
@@ -0,0 +1,117 @@
t2 = 1.0/range;
t3 = q3*ve*2.0;
t4 = q0*vn*2.0;
t11 = q2*vd*2.0;
t5 = t3+t4-t11;
t6 = q0*q3*2.0;
t7 = q1*q2*2.0;
t8 = t6+t7;
t9 = q0*q2*2.0;
t28 = q1*q3*2.0;
t10 = t9-t28;
t12 = P[0][0]*t2*t5;
t13 = q3*vd*2.0;
t14 = q2*ve*2.0;
t15 = q1*vn*2.0;
t16 = t13+t14+t15;
t17 = q0*vd*2.0;
t18 = q2*vn*2.0;
t29 = q1*ve*2.0;
t19 = t17+t18-t29;
t20 = q1*vd*2.0;
t21 = q0*ve*2.0;
t30 = q3*vn*2.0;
t22 = t20+t21-t30;
t23 = q0*q0;
t24 = q1*q1;
t25 = q2*q2;
t26 = q3*q3;
t27 = t23+t24-t25-t26;
t31 = P[1][1]*t2*t16;
t32 = P[5][0]*t2*t8;
t33 = P[1][0]*t2*t16;
t34 = P[3][0]*t2*t22;
t35 = P[4][0]*t2*t27;
t80 = P[6][0]*t2*t10;
t81 = P[2][0]*t2*t19;
t36 = t12+t32+t33+t34+t35-t80-t81;
t37 = t2*t5*t36;
t38 = P[5][1]*t2*t8;
t39 = P[0][1]*t2*t5;
t40 = P[3][1]*t2*t22;
t41 = P[4][1]*t2*t27;
t82 = P[6][1]*t2*t10;
t83 = P[2][1]*t2*t19;
t42 = t31+t38+t39+t40+t41-t82-t83;
t43 = t2*t16*t42;
t44 = P[5][2]*t2*t8;
t45 = P[0][2]*t2*t5;
t46 = P[1][2]*t2*t16;
t47 = P[3][2]*t2*t22;
t48 = P[4][2]*t2*t27;
t79 = P[2][2]*t2*t19;
t84 = P[6][2]*t2*t10;
t49 = t44+t45+t46+t47+t48-t79-t84;
t50 = P[5][3]*t2*t8;
t51 = P[0][3]*t2*t5;
t52 = P[1][3]*t2*t16;
t53 = P[3][3]*t2*t22;
t54 = P[4][3]*t2*t27;
t86 = P[6][3]*t2*t10;
t87 = P[2][3]*t2*t19;
t55 = t50+t51+t52+t53+t54-t86-t87;
t56 = t2*t22*t55;
t57 = P[5][4]*t2*t8;
t58 = P[0][4]*t2*t5;
t59 = P[1][4]*t2*t16;
t60 = P[3][4]*t2*t22;
t61 = P[4][4]*t2*t27;
t88 = P[6][4]*t2*t10;
t89 = P[2][4]*t2*t19;
t62 = t57+t58+t59+t60+t61-t88-t89;
t63 = t2*t27*t62;
t64 = P[5][5]*t2*t8;
t65 = P[0][5]*t2*t5;
t66 = P[1][5]*t2*t16;
t67 = P[3][5]*t2*t22;
t68 = P[4][5]*t2*t27;
t90 = P[6][5]*t2*t10;
t91 = P[2][5]*t2*t19;
t69 = t64+t65+t66+t67+t68-t90-t91;
t70 = t2*t8*t69;
t71 = P[5][6]*t2*t8;
t72 = P[0][6]*t2*t5;
t73 = P[1][6]*t2*t16;
t74 = P[3][6]*t2*t22;
t75 = P[4][6]*t2*t27;
t92 = P[6][6]*t2*t10;
t93 = P[2][6]*t2*t19;
t76 = t71+t72+t73+t74+t75-t92-t93;
t85 = t2*t19*t49;
t94 = t2*t10*t76;
t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
t78 = 1.0/t77;
A0[0][0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
A0[1][0] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
A0[2][0] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
A0[3][0] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
A0[4][0] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
A0[5][0] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
A0[6][0] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
A0[7][0] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
A0[8][0] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
A0[9][0] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
A0[10][0] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
A0[11][0] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
A0[12][0] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
A0[13][0] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
A0[14][0] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
A0[15][0] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
A0[16][0] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
A0[17][0] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
A0[18][0] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
A0[19][0] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
A0[20][0] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
A0[21][0] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
A0[22][0] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
A0[23][0] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
EKF/matlab/scripts/Inertial Nav EKF/K_VELX.c
+116
View File
@@ -0,0 +1,116 @@
float t2 = q0*q3*2.0f;
float t3 = q1*q2*2.0f;
float t4 = t2+t3;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = t5+t6-t7-t8;
float t10 = q0*q2*2.0f;
float t25 = q1*q3*2.0f;
float t11 = t10-t25;
float t12 = q3*ve*2.0f;
float t13 = q0*vn*2.0f;
float t26 = q2*vd*2.0f;
float t14 = t12+t13-t26;
float t15 = q3*vd*2.0f;
float t16 = q2*ve*2.0f;
float t17 = q1*vn*2.0f;
float t18 = t15+t16+t17;
float t19 = q0*vd*2.0f;
float t20 = q2*vn*2.0f;
float t27 = q1*ve*2.0f;
float t21 = t19+t20-t27;
float t22 = q1*vd*2.0f;
float t23 = q0*ve*2.0f;
float t28 = q3*vn*2.0f;
float t24 = t22+t23-t28;
float t29 = P[0][0]*t14;
float t30 = P[1][1]*t18;
float t31 = P[4][5]*t9;
float t32 = P[5][5]*t4;
float t33 = P[0][5]*t14;
float t34 = P[1][5]*t18;
float t35 = P[3][5]*t24;
float t79 = P[6][5]*t11;
float t80 = P[2][5]*t21;
float t36 = t31+t32+t33+t34+t35-t79-t80;
float t37 = t4*t36;
float t38 = P[4][6]*t9;
float t39 = P[5][6]*t4;
float t40 = P[0][6]*t14;
float t41 = P[1][6]*t18;
float t42 = P[3][6]*t24;
float t81 = P[6][6]*t11;
float t82 = P[2][6]*t21;
float t43 = t38+t39+t40+t41+t42-t81-t82;
float t44 = P[4][0]*t9;
float t45 = P[5][0]*t4;
float t46 = P[1][0]*t18;
float t47 = P[3][0]*t24;
float t84 = P[6][0]*t11;
float t85 = P[2][0]*t21;
float t48 = t29+t44+t45+t46+t47-t84-t85;
float t49 = t14*t48;
float t50 = P[4][1]*t9;
float t51 = P[5][1]*t4;
float t52 = P[0][1]*t14;
float t53 = P[3][1]*t24;
float t86 = P[6][1]*t11;
float t87 = P[2][1]*t21;
float t54 = t30+t50+t51+t52+t53-t86-t87;
float t55 = t18*t54;
float t56 = P[4][2]*t9;
float t57 = P[5][2]*t4;
float t58 = P[0][2]*t14;
float t59 = P[1][2]*t18;
float t60 = P[3][2]*t24;
float t78 = P[2][2]*t21;
float t88 = P[6][2]*t11;
float t61 = t56+t57+t58+t59+t60-t78-t88;
float t62 = P[4][3]*t9;
float t63 = P[5][3]*t4;
float t64 = P[0][3]*t14;
float t65 = P[1][3]*t18;
float t66 = P[3][3]*t24;
float t90 = P[6][3]*t11;
float t91 = P[2][3]*t21;
float t67 = t62+t63+t64+t65+t66-t90-t91;
float t68 = t24*t67;
float t69 = P[4][4]*t9;
float t70 = P[5][4]*t4;
float t71 = P[0][4]*t14;
float t72 = P[1][4]*t18;
float t73 = P[3][4]*t24;
float t92 = P[6][4]*t11;
float t93 = P[2][4]*t21;
float t74 = t69+t70+t71+t72+t73-t92-t93;
float t75 = t9*t74;
float t83 = t11*t43;
float t89 = t21*t61;
float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89;
float t77 = 1.0f/t76;
Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24);
Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24);
Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24);
Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21);
Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24);
Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24);
Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24);
Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24);
Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24);
Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24);
Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24);
Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24);
Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24);
Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24);
Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24);
Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24);
Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24);
Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24);
Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24);
Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24);
Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24);
Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24);
Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c
+116
View File
@@ -0,0 +1,116 @@
float t2 = q0*q3*2.0f;
float t9 = q1*q2*2.0f;
float t3 = t2-t9;
float t4 = q0*q0;
float t5 = q1*q1;
float t6 = q2*q2;
float t7 = q3*q3;
float t8 = t4-t5+t6-t7;
float t10 = q0*q1*2.0f;
float t11 = q2*q3*2.0f;
float t12 = t10+t11;
float t13 = q1*vd*2.0f;
float t14 = q0*ve*2.0f;
float t26 = q3*vn*2.0f;
float t15 = t13+t14-t26;
float t16 = q0*vd*2.0f;
float t17 = q2*vn*2.0f;
float t27 = q1*ve*2.0f;
float t18 = t16+t17-t27;
float t19 = q3*vd*2.0f;
float t20 = q2*ve*2.0f;
float t21 = q1*vn*2.0f;
float t22 = t19+t20+t21;
float t23 = q3*ve*2.0f;
float t24 = q0*vn*2.0f;
float t28 = q2*vd*2.0f;
float t25 = t23+t24-t28;
float t29 = P[0][0]*t15;
float t30 = P[1][1]*t18;
float t31 = P[5][4]*t8;
float t32 = P[6][4]*t12;
float t33 = P[0][4]*t15;
float t34 = P[1][4]*t18;
float t35 = P[2][4]*t22;
float t78 = P[4][4]*t3;
float t79 = P[3][4]*t25;
float t36 = t31+t32+t33+t34+t35-t78-t79;
float t37 = P[5][6]*t8;
float t38 = P[6][6]*t12;
float t39 = P[0][6]*t15;
float t40 = P[1][6]*t18;
float t41 = P[2][6]*t22;
float t81 = P[4][6]*t3;
float t82 = P[3][6]*t25;
float t42 = t37+t38+t39+t40+t41-t81-t82;
float t43 = t12*t42;
float t44 = P[5][0]*t8;
float t45 = P[6][0]*t12;
float t46 = P[1][0]*t18;
float t47 = P[2][0]*t22;
float t83 = P[4][0]*t3;
float t84 = P[3][0]*t25;
float t48 = t29+t44+t45+t46+t47-t83-t84;
float t49 = t15*t48;
float t50 = P[5][1]*t8;
float t51 = P[6][1]*t12;
float t52 = P[0][1]*t15;
float t53 = P[2][1]*t22;
float t85 = P[4][1]*t3;
float t86 = P[3][1]*t25;
float t54 = t30+t50+t51+t52+t53-t85-t86;
float t55 = t18*t54;
float t56 = P[5][2]*t8;
float t57 = P[6][2]*t12;
float t58 = P[0][2]*t15;
float t59 = P[1][2]*t18;
float t60 = P[2][2]*t22;
float t87 = P[4][2]*t3;
float t88 = P[3][2]*t25;
float t61 = t56+t57+t58+t59+t60-t87-t88;
float t62 = t22*t61;
float t63 = P[5][3]*t8;
float t64 = P[6][3]*t12;
float t65 = P[0][3]*t15;
float t66 = P[1][3]*t18;
float t67 = P[2][3]*t22;
float t89 = P[4][3]*t3;
float t90 = P[3][3]*t25;
float t68 = t63+t64+t65+t66+t67-t89-t90;
float t69 = P[5][5]*t8;
float t70 = P[6][5]*t12;
float t71 = P[0][5]*t15;
float t72 = P[1][5]*t18;
float t73 = P[2][5]*t22;
float t92 = P[4][5]*t3;
float t93 = P[3][5]*t25;
float t74 = t69+t70+t71+t72+t73-t92-t93;
float t75 = t8*t74;
float t80 = t3*t36;
float t91 = t25*t68;
float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91;
float t77 = 1.0f/t76;
Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25);
Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25);
Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25);
Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22);
Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25);
Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25);
Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25);
Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25);
Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25);
Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25);
Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25);
Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25);
Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25);
Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25);
Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25);
Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25);
Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25);
Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25);
Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25);
Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25);
Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25);
Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25);
Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c
+116
View File
@@ -0,0 +1,116 @@
float t2 = q0*q2*2.0;
float t3 = q1*q3*2.0;
float t4 = t2+t3;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = t5-t6-t7+t8;
float t10 = q0*q1*2.0;
float t25 = q2*q3*2.0;
float t11 = t10-t25;
float t12 = q0*vd*2.0;
float t13 = q2*vn*2.0;
float t26 = q1*ve*2.0;
float t14 = t12+t13-t26;
float t15 = q1*vd*2.0;
float t16 = q0*ve*2.0;
float t27 = q3*vn*2.0;
float t17 = t15+t16-t27;
float t18 = q3*ve*2.0;
float t19 = q0*vn*2.0;
float t28 = q2*vd*2.0;
float t20 = t18+t19-t28;
float t21 = q3*vd*2.0;
float t22 = q2*ve*2.0;
float t23 = q1*vn*2.0;
float t24 = t21+t22+t23;
float t29 = P[0][0]*t14;
float t30 = P[6][4]*t9;
float t31 = P[4][4]*t4;
float t32 = P[0][4]*t14;
float t33 = P[2][4]*t20;
float t34 = P[3][4]*t24;
float t78 = P[5][4]*t11;
float t79 = P[1][4]*t17;
float t35 = t30+t31+t32+t33+t34-t78-t79;
float t36 = t4*t35;
float t37 = P[6][5]*t9;
float t38 = P[4][5]*t4;
float t39 = P[0][5]*t14;
float t40 = P[2][5]*t20;
float t41 = P[3][5]*t24;
float t80 = P[5][5]*t11;
float t81 = P[1][5]*t17;
float t42 = t37+t38+t39+t40+t41-t80-t81;
float t43 = P[6][0]*t9;
float t44 = P[4][0]*t4;
float t45 = P[2][0]*t20;
float t46 = P[3][0]*t24;
float t83 = P[5][0]*t11;
float t84 = P[1][0]*t17;
float t47 = t29+t43+t44+t45+t46-t83-t84;
float t48 = t14*t47;
float t49 = P[6][1]*t9;
float t50 = P[4][1]*t4;
float t51 = P[0][1]*t14;
float t52 = P[2][1]*t20;
float t53 = P[3][1]*t24;
float t85 = P[5][1]*t11;
float t86 = P[1][1]*t17;
float t54 = t49+t50+t51+t52+t53-t85-t86;
float t55 = P[6][2]*t9;
float t56 = P[4][2]*t4;
float t57 = P[0][2]*t14;
float t58 = P[2][2]*t20;
float t59 = P[3][2]*t24;
float t88 = P[5][2]*t11;
float t89 = P[1][2]*t17;
float t60 = t55+t56+t57+t58+t59-t88-t89;
float t61 = t20*t60;
float t62 = P[6][3]*t9;
float t63 = P[4][3]*t4;
float t64 = P[0][3]*t14;
float t65 = P[2][3]*t20;
float t66 = P[3][3]*t24;
float t90 = P[5][3]*t11;
float t91 = P[1][3]*t17;
float t67 = t62+t63+t64+t65+t66-t90-t91;
float t68 = t24*t67;
float t69 = P[6][6]*t9;
float t70 = P[4][6]*t4;
float t71 = P[0][6]*t14;
float t72 = P[2][6]*t20;
float t73 = P[3][6]*t24;
float t92 = P[5][6]*t11;
float t93 = P[1][6]*t17;
float t74 = t69+t70+t71+t72+t73-t92-t93;
float t75 = t9*t74;
float t82 = t11*t42;
float t87 = t17*t54;
float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87;
float t77 = 1.0f/t76;
Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24);
Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24);
Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24);
Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20);
Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24);
Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24);
Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24);
Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24);
Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24);
Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24);
Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24);
Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24);
Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24);
Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24);
Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24);
Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24);
Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24);
Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24);
Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24);
Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24);
Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24);
Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24);
Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
EKF/matlab/scripts/Inertial Nav EKF/LOSX.c
+160
View File
@@ -0,0 +1,160 @@
float t2 = 1.0f/range;
float t3 = Tbs.b.y*q0*2.0f;
float t4 = Tbs.b.x*q3*2.0f;
float t18 = Tbs.b.z*q1*2.0f;
float t5 = t3+t4-t18;
float t6 = Tbs.b.y*q1*2.0f;
float t7 = Tbs.b.z*q0*2.0f;
float t16 = Tbs.b.x*q2*2.0f;
float t8 = t6+t7-t16;
float t9 = Tbs.b.x*q0*2.0f;
float t10 = Tbs.b.z*q2*2.0f;
float t17 = Tbs.b.y*q3*2.0f;
float t11 = t9+t10-t17;
float t12 = Tbs.b.x*q1*2.0f;
float t13 = Tbs.b.y*q2*2.0f;
float t14 = Tbs.b.z*q3*2.0f;
float t15 = t12+t13+t14;
float t19 = q0*q0;
float t20 = q1*q1;
float t21 = q2*q2;
float t22 = q3*q3;
float t23 = q0*q3*2.0f;
float t24 = q0*q2*2.0f;
float t25 = q1*q3*2.0f;
float t26 = q0*q1*2.0f;
float t27 = t19+t20-t21-t22;
float t28 = Tbs.b.x*t27;
float t29 = q1*q2*2.0f;
float t30 = t24+t25;
float t31 = Tbs.b.z*t30;
float t32 = t19-t20+t21-t22;
float t33 = Tbs.b.y*t32;
float t34 = t23+t29;
float t35 = Tbs.b.x*t34;
float t36 = q2*q3*2.0f;
float t37 = t19-t20-t21+t22;
float t38 = Tbs.b.z*t37;
float t39 = t24-t25;
float t40 = t26+t36;
float t41 = Tbs.b.y*t40;
float t60 = Tbs.b.x*t39;
float t42 = t38+t41-t60;
float t43 = t8*vd;
float t44 = t5*ve;
float t45 = t11*vn;
float t46 = t43+t44+t45;
float t47 = t5*vd;
float t48 = t15*vn;
float t62 = t8*ve;
float t49 = t47+t48-t62;
float t50 = t15*ve;
float t51 = t8*vn;
float t63 = t11*vd;
float t52 = t50+t51-t63;
float t53 = t15*vd;
float t54 = t11*ve;
float t64 = t5*vn;
float t55 = t53+t54-t64;
float t56 = t23-t29;
float t65 = Tbs.b.y*t56;
float t57 = t28+t31-t65;
float t58 = t26-t36;
float t66 = Tbs.b.z*t58;
float t59 = t33+t35-t66;
float t61 = P[0][0]*t2*t46;
float t67 = P[1][1]*t2*t49;
float t68 = P[4][0]*t2*t57;
float t69 = P[5][0]*t2*t59;
float t70 = P[6][0]*t2*t42;
float t71 = P[1][0]*t2*t49;
float t72 = P[2][0]*t2*t52;
float t73 = P[3][0]*t2*t55;
float t74 = t61+t68+t69+t70+t71+t72+t73;
float t75 = t2*t46*t74;
float t76 = P[4][1]*t2*t57;
float t77 = P[5][1]*t2*t59;
float t78 = P[6][1]*t2*t42;
float t79 = P[0][1]*t2*t46;
float t80 = P[2][1]*t2*t52;
float t81 = P[3][1]*t2*t55;
float t82 = t67+t76+t77+t78+t79+t80+t81;
float t83 = t2*t49*t82;
float t84 = P[4][2]*t2*t57;
float t85 = P[5][2]*t2*t59;
float t86 = P[6][2]*t2*t42;
float t87 = P[0][2]*t2*t46;
float t88 = P[1][2]*t2*t49;
float t89 = P[2][2]*t2*t52;
float t90 = P[3][2]*t2*t55;
float t91 = t84+t85+t86+t87+t88+t89+t90;
float t92 = t2*t52*t91;
float t93 = P[4][3]*t2*t57;
float t94 = P[5][3]*t2*t59;
float t95 = P[6][3]*t2*t42;
float t96 = P[0][3]*t2*t46;
float t97 = P[1][3]*t2*t49;
float t98 = P[2][3]*t2*t52;
float t99 = P[3][3]*t2*t55;
float t100 = t93+t94+t95+t96+t97+t98+t99;
float t101 = t2*t55*t100;
float t102 = P[4][4]*t2*t57;
float t103 = P[5][4]*t2*t59;
float t104 = P[6][4]*t2*t42;
float t105 = P[0][4]*t2*t46;
float t106 = P[1][4]*t2*t49;
float t107 = P[2][4]*t2*t52;
float t108 = P[3][4]*t2*t55;
float t109 = t102+t103+t104+t105+t106+t107+t108;
float t110 = t2*t57*t109;
float t111 = P[4][5]*t2*t57;
float t112 = P[5][5]*t2*t59;
float t113 = P[6][5]*t2*t42;
float t114 = P[0][5]*t2*t46;
float t115 = P[1][5]*t2*t49;
float t116 = P[2][5]*t2*t52;
float t117 = P[3][5]*t2*t55;
float t118 = t111+t112+t113+t114+t115+t116+t117;
float t119 = t2*t59*t118;
float t120 = P[4][6]*t2*t57;
float t121 = P[5][6]*t2*t59;
float t122 = P[6][6]*t2*t42;
float t123 = P[0][6]*t2*t46;
float t124 = P[1][6]*t2*t49;
float t125 = P[2][6]*t2*t52;
float t126 = P[3][6]*t2*t55;
float t127 = t120+t121+t122+t123+t124+t125+t126;
float t128 = t2*t42*t127;
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128;
float t130 = 1.0f/t129;
H_LOS[0] = t2*t46;
H_LOS[1] = t2*t49;
H_LOS[2] = t2*t52;
H_LOS[3] = t2*t55;
H_LOS[4] = t2*(t28+t31-Tbs.b.y*(t23-q1*q2*2.0));
H_LOS[5] = t2*(t33+t35-Tbs.b.z*(t26-q2*q3*2.0));
H_LOS[6] = t2*t42;
Kfusion[0] = t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59);
Kfusion[1] = t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59);
Kfusion[2] = t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59);
Kfusion[3] = t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59);
Kfusion[4] = t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59);
Kfusion[5] = t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57);
Kfusion[6] = t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59);
Kfusion[7] = t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59);
Kfusion[8] = t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59);
Kfusion[9] = t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59);
Kfusion[10] = t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59);
Kfusion[11] = t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59);
Kfusion[12] = t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59);
Kfusion[13] = t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59);
Kfusion[14] = t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59);
Kfusion[15] = t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59);
Kfusion[16] = t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59);
Kfusion[17] = t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59);
Kfusion[18] = t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59);
Kfusion[19] = t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59);
Kfusion[20] = t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59);
Kfusion[21] = t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59);
Kfusion[22] = t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59);
Kfusion[23] = t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59);
EKF/matlab/scripts/Inertial Nav EKF/LOSY.c
+162
View File
@@ -0,0 +1,162 @@
float t2 = 1.0f/range;
float t3 = Tbs.a.y*q0*2.0f;
float t4 = Tbs.a.x*q3*2.0f;
float t18 = Tbs.a.z*q1*2.0f;
float t5 = t3+t4-t18;
float t6 = Tbs.a.y*q1*2.0f;
float t7 = Tbs.a.z*q0*2.0f;
float t16 = Tbs.a.x*q2*2.0f;
float t8 = t6+t7-t16;
float t9 = Tbs.a.x*q0*2.0f;
float t10 = Tbs.a.z*q2*2.0f;
float t17 = Tbs.a.y*q3*2.0f;
float t11 = t9+t10-t17;
float t12 = Tbs.a.x*q1*2.0f;
float t13 = Tbs.a.y*q2*2.0f;
float t14 = Tbs.a.z*q3*2.0f;
float t15 = t12+t13+t14;
float t19 = q0*q0;
float t20 = q1*q1;
float t21 = q2*q2;
float t22 = q3*q3;
float t23 = q0*q3*2.0f;
float t24 = q0*q2*2.0f;
float t25 = q1*q3*2.0f;
float t26 = q0*q1*2.0f;
float t27 = t19+t20-t21-t22;
float t28 = Tbs.a.x*t27;
float t29 = q1*q2*2.0f;
float t30 = t24+t25;
float t31 = Tbs.a.z*t30;
float t32 = t19-t20+t21-t22;
float t33 = Tbs.a.y*t32;
float t34 = t23+t29;
float t35 = Tbs.a.x*t34;
float t36 = q2*q3*2.0f;
float t37 = t19-t20-t21+t22;
float t38 = Tbs.a.z*t37;
float t39 = t24-t25;
float t40 = t26+t36;
float t41 = Tbs.a.y*t40;
float t60 = Tbs.a.x*t39;
float t42 = t38+t41-t60;
float t43 = t8*vd;
float t44 = t5*ve;
float t45 = t11*vn;
float t46 = t43+t44+t45;
float t47 = t5*vd;
float t48 = t15*vn;
float t62 = t8*ve;
float t49 = t47+t48-t62;
float t50 = t15*ve;
float t51 = t8*vn;
float t63 = t11*vd;
float t52 = t50+t51-t63;
float t53 = t15*vd;
float t54 = t11*ve;
float t64 = t5*vn;
float t55 = t53+t54-t64;
float t56 = t23-t29;
float t65 = Tbs.a.y*t56;
float t57 = t28+t31-t65;
float t58 = t26-t36;
float t66 = Tbs.a.z*t58;
float t59 = t33+t35-t66;
float t61 = P[0][0]*t2*t46;
float t67 = P[1][1]*t2*t49;
float t68 = P[4][0]*t2*t57;
float t69 = P[5][0]*t2*t59;
float t70 = P[6][0]*t2*t42;
float t71 = P[1][0]*t2*t49;
float t72 = P[2][0]*t2*t52;
float t73 = P[3][0]*t2*t55;
float t74 = t61+t68+t69+t70+t71+t72+t73;
float t75 = t2*t46*t74;
float t76 = P[4][1]*t2*t57;
float t77 = P[5][1]*t2*t59;
float t78 = P[6][1]*t2*t42;
float t79 = P[0][1]*t2*t46;
float t80 = P[2][1]*t2*t52;
float t81 = P[3][1]*t2*t55;
float t82 = t67+t76+t77+t78+t79+t80+t81;
float t83 = t2*t49*t82;
float t84 = P[4][2]*t2*t57;
float t85 = P[5][2]*t2*t59;
float t86 = P[6][2]*t2*t42;
float t87 = P[0][2]*t2*t46;
float t88 = P[1][2]*t2*t49;
float t89 = P[2][2]*t2*t52;
float t90 = P[3][2]*t2*t55;
float t91 = t84+t85+t86+t87+t88+t89+t90;
float t92 = t2*t52*t91;
float t93 = P[4][3]*t2*t57;
float t94 = P[5][3]*t2*t59;
float t95 = P[6][3]*t2*t42;
float t96 = P[0][3]*t2*t46;
float t97 = P[1][3]*t2*t49;
float t98 = P[2][3]*t2*t52;
float t99 = P[3][3]*t2*t55;
float t100 = t93+t94+t95+t96+t97+t98+t99;
float t101 = t2*t55*t100;
float t102 = P[4][4]*t2*t57;
float t103 = P[5][4]*t2*t59;
float t104 = P[6][4]*t2*t42;
float t105 = P[0][4]*t2*t46;
float t106 = P[1][4]*t2*t49;
float t107 = P[2][4]*t2*t52;
float t108 = P[3][4]*t2*t55;
float t109 = t102+t103+t104+t105+t106+t107+t108;
float t110 = t2*t57*t109;
float t111 = P[4][5]*t2*t57;
float t112 = P[5][5]*t2*t59;
float t113 = P[6][5]*t2*t42;
float t114 = P[0][5]*t2*t46;
float t115 = P[1][5]*t2*t49;
float t116 = P[2][5]*t2*t52;
float t117 = P[3][5]*t2*t55;
float t118 = t111+t112+t113+t114+t115+t116+t117;
float t119 = t2*t59*t118;
float t120 = P[4][6]*t2*t57;
float t121 = P[5][6]*t2*t59;
float t122 = P[6][6]*t2*t42;
float t123 = P[0][6]*t2*t46;
float t124 = P[1][6]*t2*t49;
float t125 = P[2][6]*t2*t52;
float t126 = P[3][6]*t2*t55;
float t127 = t120+t121+t122+t123+t124+t125+t126;
float t128 = t2*t42*t127;
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128;
float t130 = 1.0f/t129;
H_LOS[0] = -t2*t46;
H_LOS[1] = -t2*t49;
H_LOS[2] = -t2*t52;
H_LOS[3] = -t2*t55;
H_LOS[4] = -t2*(t28+t31-Tbs.a.y*(t23-q1*q2*2.0f));
H_LOS[5] = -t2*(t33+t35-Tbs.a.z*(t26-q2*q3*2.0f));
H_LOS[6] = -t2*t42;
Kfusion[0] = -t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59);
Kfusion[1] = -t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59);
Kfusion[2] = -t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59);
Kfusion[3] = -t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59);
Kfusion[4] = -t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59);
Kfusion[5] = -t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57);
Kfusion[6] = -t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59);
Kfusion[7] = -t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59);
Kfusion[8] = -t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59);
Kfusion[9] = -t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59);
Kfusion[10] = -t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59);
Kfusion[11] = -t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59);
Kfusion[12] = -t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59);
Kfusion[13] = -t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59);
Kfusion[14] = -t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59);
Kfusion[15] = -t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59);
Kfusion[16] = -t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59);
Kfusion[17] = -t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59);
Kfusion[18] = -t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59);
Kfusion[19] = -t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59);
Kfusion[20] = -t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59);
Kfusion[21] = -t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59);
Kfusion[22] = -t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59);
Kfusion[23] = -t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59);
EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt
+686
View File
File diff suppressed because one or more lines are too long
EKF/matlab/scripts/Inertial Nav EKF/Magnetometer.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/NormQuat.m
+5
View File
@@ -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;
EKF/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m
+29
View File
@@ -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
EKF/matlab/scripts/Inertial Nav EKF/Q_airdata.c
+89
View File
@@ -0,0 +1,89 @@
float t3 = ve-vwe;
float t4 = q0*q0;
float t5 = q1*q1;
float t6 = q2*q2;
float t7 = q3*q3;
float t8 = vd-vwd;
float t9 = q0*q1*2.0f;
float t10 = q2*q3*2.0f;
float t11 = vn-vwn;
float t13 = q0*q2*2.0f;
float t14 = q1*q3*2.0f;
float t18 = t4-t5-t6+t7;
float t19 = t8*t18;
float t20 = t9-t10;
float t21 = t3*t20;
float t22 = t13+t14;
float t23 = t11*t22;
float t2 = t19-t21+t23;
float t15 = q0*q3*2.0f;
float t16 = q1*q2*2.0f;
float t24 = t9+t10;
float t25 = t4-t5+t6-t7;
float t26 = t3*t25;
float t27 = t8*t24;
float t28 = t15-t16;
float t29 = t11*t28;
float t12 = t26+t27-t29;
float t30 = t13-t14;
float t31 = t4+t5-t6-t7;
float t32 = t11*t31;
float t33 = t8*t30;
float t34 = t15+t16;
float t35 = t3*t34;
float t17 = t32-t33+t35;
float t44 = t2*t18*2.0f;
float t45 = t12*t24*2.0f;
float t46 = t17*t30*2.0f;
float t36 = t44+t45-t46;
float t37 = t2*t2;
float t38 = t12*t12;
float t39 = t17*t17;
float t40 = t37+t38+t39;
float t41 = 1.0f/t40;
float t48 = t12*t25*2.0f;
float t49 = t2*t20*2.0f;
float t50 = t17*t34*2.0f;
float t42 = t48-t49+t50;
float t52 = t17*t31*2.0f;
float t53 = t2*t22*2.0f;
float t54 = t12*t28*2.0f;
float t43 = t52+t53-t54;
float t47 = t36*t36;
float t51 = t42*t42;
float t55 = t43*t43;
float t57 = 1.0f/(t17*t17);
float t58 = 1.0f/t17;
float t63 = t18*t58;
float t64 = t2*t30*t57;
float t56 = t63+t64;
float t66 = t22*t58;
float t67 = t2*t31*t57;
float t59 = t66-t67;
float t60 = t37*t57;
float t61 = t60+1.0f;
float t62 = 1.0f/(t61*t61);
float t65 = t56*t56;
float t68 = t59*t59;
float t70 = t20*t58;
float t71 = t2*t34*t57;
float t69 = t70+t71;
float t72 = t69*t69;
float t78 = t25*t58;
float t79 = t12*t34*t57;
float t73 = t78-t79;
float t81 = t28*t58;
float t82 = t12*t31*t57;
float t74 = t81+t82;
float t75 = t38*t57;
float t76 = t75+1.0f;
float t77 = 1.0f/(t76*t76);
float t80 = t73*t73;
float t83 = t74*t74;
float t85 = t24*t58;
float t86 = t12*t30*t57;
float t84 = t85+t86;
float t87 = t84*t84;
float tas_var = t41*t47*vd_var*0.25f+t41*t51*ve_var*0.25f+t41*t55*vn_var*0.25f+t41*t47*vwd_var*0.25f+t41*t51*vwe_var*0.25f+t41*t55*vwn_var*0.25f;
float aoa_var = t62*t65*vd_var+t62*t72*ve_var+t62*t68*vn_var+t62*t65*vwd_var+t62*t72*vwe_var+t62*t68*vwn_var;
float aos_var = t77*t87*vd_var+t77*t80*ve_var+t77*t83*vn_var+t77*t87*vwd_var+t77*t80*vwe_var+t77*t83*vwn_var;

Some files were not shown because too many files have changed in this diff Show More