本文主要记录自己学习ardupilot的ekf2代码的过程，相信很多人想移植或者学习ekf2,看到眼花缭乱的代码无从下手。九天揽月将带你玩转Ardupilot 的EKF2纸老虎，只有对整个代码熟悉了，我们才能用的的心用手。
(1)先来看下最基础也是最重要的线性kalman
总结：不要害怕kalman,只要你可以懂得线性kalman,其他的一样可以掌握，只不过换了个马夹，一切都是纸老虎。重点记住这五个公式的含义就行。
可以参考下面三篇文章
第一篇
第二篇
第三篇
（2）对公式的理解
1.线性系统状态方程
2.线性观测方程*
*
*（3）EKF学习
（1）用到的所有类之间的关系
（2）EKF代码流程分析
—————————————————————————————————————————————————-
下一步看Viso流程图ahrs属于AP_AHRS_NavEKF的对象，因此我们要去AP_AHRS_NavEKF找更新函数
*并在串口打印输出
（1）我们主要分析三个函数 *
（1） update_DCM(skip_ins_update);
*（2）update_EKF2();
*（3）update_EKF3();
———- *
（1） update_DCM(skip_ins_update)代码分析
*（2）update_EKF2();函数分析
void AP_AHRS_NavEKF::update_EKF2(void)
{
// printf("_ekf2_started=%d\r\n",_ekf2_started);
if (!_ekf2_started) //_ekf2_started=0
{
// printf("AAA1\r\n");
//等待1秒用于DCM输出有效的倾斜误差估计-----wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0)
{
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf)
{
// printf("AAA2\r\n");
_ekf2_started = EKF2.InitialiseFilter(); //初始化EKF2，为运行EKF2更新做准备
// printf("AAA3\r\n");
if (_force_ekf)
{
return;
}
}
}
if (_ekf2_started)
{
// printf("BBB1\r\n");
EKF2.UpdateFilter();
// printf("BBB12\r\n");
if (active_EKF_type() == EKF_TYPE2)
{
// printf("CCC1\r\n");
Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
//使用优先级比较高的gyro,做EKF运算----Use the primary EKF to select the primary gyro
const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
const AP_InertialSensor &_ins = AP::ins();
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
_gyro_drift.zero();
EKF2.getGyroBias(-1,_gyro_drift);
_gyro_drift = -_gyro_drift;
// calculate corrected gyro estimate for get_gyro()
_gyro_estimate.zero();
if (primary_imu == -1)
{
// the primary IMU is undefined so use an uncorrected default value from the INS library
_gyro_estimate = _ins.get_gyro();
} else {
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
}
// get z accel bias estimate from active EKF (this is usually for the primary IMU)
//从有源EKF得到Z Accel-偏差估计（通常用于初级IMU）
float abias = 0;
EKF2.getAccelZBias(-1,abias);
// This EKF is currently using primary_imu, and abias applies to only that IMU
//这个EKF目前使用优先的IIMU，而偏差仅适用于IMU。
for (uint8_t i=0; i<_ins.get_accel_count(); i++)
{
Vector3f accel = _ins.get_accel(i);
if (i == primary_imu) {
accel.z -= abias;
}
if (_ins.get_accel_health(i))
{
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
nav_filter_status filt_state;
EKF2.getFilterStatus(-1,filt_state);
AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS.
AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching;
AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs;
}
}
}
EKF2初始化调试串口显示
1.我们先看下ekf2怎么进行初始化的
_ekf2_started = EKF2.InitialiseFilter(); //初始化EKF2，为运行EKF2更新做准备** 先在串口中显示下，运行过程
看初始化代码
bool NavEKF2::InitialiseFilter(void)
{
// printf("AAA4\r\n");
if (_enable == 0)
{
return false;
}
const AP_InertialSensor &ins = AP::ins(); //获取传感器数据，gyro,acc数据
imuSampleTime_us = AP_HAL::micros64(); //获取imus采样时间，单位是us
//记录预期的时间--------------------remember expected frame time
_frameTimeUsec = 1e6 / ins.get_sample_rate();
// 算有多少个预测imu方程，因为这里有好几组imu，都可以进行ekf------expected number of IMU frames per prediction
_framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
//看看我们是否会做日志记录-----------------------------------see if we will be doing logging
DataFlash_Class *dataflash = DataFlash_Class::instance();
if (dataflash != nullptr)
{
logging.enabled = dataflash->log_replay();
}
if (core == nullptr)
{
// printf("AAA5\r\n");
//不要对一个IMU进行多次滤波---------don't run multiple filters for 1 IMU
uint8_t mask = (1U<<ins.get_accel_count())-1;
_imuMask.set(_imuMask.get() & mask);
//看下总共有多少个IMU,,怎么算的后面在仔细分析-------------count IMUs from mask
num_cores = 0;
for (uint8_t i=0; i<7; i++)
{
if (_imuMask & (1U<<i))
{
num_cores++;
}
}
//检查是否有足够的内存来创建EKF内核---- check if there is enough memory to create the EKF cores
if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096)
{
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
_enable.set(0);
return false;
}
// try to allocate from CCM RAM, fallback to Normal RAM if not available or full
core = (NavEKF2_core*)hal.util->malloc_type(sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
if (core == nullptr)
{
_enable.set(0);
gcs().send_text(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
return false;
}
for (uint8_t i = 0; i < num_cores; i++)
{
//Call Constructors
new (&core[i]) NavEKF2_core(); //创建对象，用来做EKF2运算，这里创建了两个对象，对应两组IMU
}
//为内核设置IMU索引------- set the IMU index for the cores
num_cores = 0;
for (uint8_t i=0; i<7; i++)
{
if (_imuMask & (1U<<i))
{
if(!core[num_cores].setup_core(this, i, num_cores))
{
return false;
}
num_cores++;
}
}
// Set the primary initially to be the lowest index
primary = 0;
}
// printf("AAA6\r\n");
// initialise the cores. We return success only if all cores
// initialise successfully
bool ret = true;
// printf("num_cores=%d\r\n",num_cores);
for (uint8_t i=0; i<num_cores; i++) //num_cores=2
{
// printf("AAA7\r\n");
ret &= core[i].InitialiseFilterBootstrap(); //重要函数，这里就是初始化不同的IMU
}
// zero the structs used capture reset events
memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
memset(&pos_reset_data, 0, sizeof(pos_reset_data));
memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
check_log_write();
// printf("AAA8\r\n");
return ret;
}首先大致说下这个初始化的过程，网上有人总结的，我截取一部分
讲解之前，先串口调试下
可以看到总共有两组IMU可以进行EKF2运算，具体看代码，前半部分主要检查有几组IMU需要进行EKF运算，也就创建几个对象。
我们在来看下传感器初始化函数：
ret &= core[i].InitialiseFilterBootstrap(); //重要函数，这里就是初始化不同的IMU，你会看到代码调试出现AAA7两次，说明这个代码进行了两次，我们先看代码调试流程
从打印可以看出，由于我的飞控没有连接GPS，所以进行了打印DDD1之后，不打印输出DDD21,然后statesInitialised=0，进行输出DDD4,后面由于statesInitialised=1，又进行了初始化DDD3
下面我们看代码：
readIMUData(); //初始化IMU
readMagData(); //初始化地磁
readGpsData(); //初始化GPS
readBaroData();//初始化气压计这四个初始化函数，先不研究，后面自己看的时候在做说明，再往下看代码
到这里我们是否还有一个疑问？EKF的状态方程和观测方程式是什么？或者我们找到状态输入变量，观测变量也行，这里我们看个pix4的姿态EKF算法
我们要求F,G,H
我们还是的去找是哪位大牛写的这个EKF,这里提供网址EKF2_24维网址
对就是这哥们
可以看出EKF2的实现是基于matlab生成后，修改的，要是自己写那真的要累崩溃！！！！
% 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
% 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 = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise];
% derive the control(disturbance) influence matrix
G = jacobian(newStateVector, distVector);
G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[G,SG]=OptimiseAlgebra(G,'SG');
% derive the state error matrix
distMatrix = diag(distVector.^2);
Q = G*distMatrix*transpose(G);
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
% remove the disturbance noise from the process equations as it is only
% needed when calculating the disturbance influence matrix
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
% 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 = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[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 = 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 = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[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 = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[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 - method 1
load('StatePrediction.mat');
% calculate range from plane to centre of sensor fov assuming flat earth
% and camera axes aligned with body axes
range = ((ptd - pd)/Tbn(3,3));
% 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;
% calculte the observation Jacobian
H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian
H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOS = simplify(H_LOS);
% recursively simplify the equations
[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS');
% combine into a single K matrix to enable common expressions to be found
% note this matrix cannot be used in a single step fusion
K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector
K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
K_LOSY = (P*transpose(H_LOS(2,:)))/(H_LOS(2,:)*P*transpose(H_LOS(2,:)) + R_LOS); % Kalman gain vector
K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
K_LOS = [K_LOSX,K_LOSY];
simplify(K_LOS);
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
K_LOSX = K_LOS(:,1);
K_LOSY = K_LOS(:,2);
% save equations and reset workspace
save('OpticalFlow.mat','SH_LOS','H_LOS','SK_LOS','K_LOSX','K_LOSY');
clear all;
reset(symengine);
%% derive equations for sequential fusion of optical flow measurements - method 2
load('StatePrediction.mat');
% calculate range from plane to centre of sensor fov assuming flat earth
% and camera axes aligned with body axes
%range = ((ptd - pd)/Tbn(3,3));
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;
save('temp1.mat','losRateX','losRateY');
% calculate the observation Jacobian for the X axis
H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian
H_LOSX = subs(H_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOSX = simplify(H_LOSX);
save('temp2.mat','H_LOSX');
ccode(H_LOSX,'file','H_LOSX.c');
fix_c_code('H_LOSX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Y axis
H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian
H_LOSY = subs(H_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOSY = simplify(H_LOSY);
save('temp3.mat','H_LOSY');
ccode(H_LOSY,'file','H_LOSY.c');
fix_c_code('H_LOSY.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
% calculate Kalman gain vector for the X axis
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
K_LOSX = simplify(K_LOSX);
ccode(K_LOSX,'file','K_LOSX.c');
fix_c_code('K_LOSX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
load('temp3.mat');
% calculate Kalman gain vector for the Y axis
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector
K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
K_LOSY = simplify(K_LOSY);
ccode(K_LOSY,'file','K_LOSY.c');
fix_c_code('K_LOSY.c');
% reset workspace
clear all;
reset(symengine);
%% 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 = subs(H_YAW321, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
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 = subs(H_YAW312, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
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 = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
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 = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
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 = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
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);
%% Derive equations for fusion of range only measurements to a beacon at a known NED position
load('StatePrediction.mat');
syms bcn_pn bcn_pe bcn_pd 'real' % beacon NED position
syms R_BCN 'real' % observation variance for range measurement
rangePred = sqrt((pn - bcn_pn)^2 + (pe - bcn_pe)^2 + (pd - bcn_pd)^2);
H_BCN = jacobian(rangePred,stateVector); % measurement Jacobian
H_BCN = subs(H_BCN, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_BCN = simplify(H_BCN);
[H_BCN,SH_BCN]=OptimiseAlgebra(H_BCN,'SH_BCN'); % optimise processing
K_BCN = (P*transpose(H_BCN))/(H_BCN*P*transpose(H_BCN) + R_BCN);
[K_BCN,SK_BCN]=OptimiseAlgebra(K_BCN,'SK_BCN'); % Kalman gain vector
save('RngBcn.mat','SH_BCN','H_BCN','SK_BCN','K_BCN');
clear all;
reset(symengine);
%% Derive equations for fusion of range only measurements to a beacon at a known NED position
load('StatePrediction.mat');
syms bcn_pn bcn_pe bcn_pd 'real' % beacon NED position
syms R_BCN 'real' % observation variance for range measurement
rangePred = sqrt((pn - bcn_pn)^2 + (pe - bcn_pe)^2 + (pd - bcn_pd)^2);
H_BCN = jacobian(rangePred,stateVector); % measurement Jacobian
H_BCN = subs(H_BCN, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_BCN = simplify(H_BCN);
K_BCN = (P*transpose(H_BCN))/(H_BCN*P*transpose(H_BCN) + R_BCN);
ccode([K_BCN,H_BCN'],'file','calcBCN.c');
fix_c_code('calcBCN.c');
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('OpticalFlow.mat');
load('Drag.mat');
load('RngBcn.mat');
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);有个重要的地方我们可以看，是参数表
// Define tuning parameters
const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable EKF2
// @Description: This enables EKF2. Enabling EKF2 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_TYPE=2. A reboot or restart will need to be performed after changing the value of EK2_ENABLE for it to take effect.
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 1, AP_PARAM_FLAG_ENABLE),
// GPS measurement parameters
// @Param: GPS_TYPE
// @DisplayName: GPS mode control
// @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = Inhibit GPS use - this can be useful when flying with an optical flow sensor in an environment where GPS quality is poor and subject to large multipath errors.
// @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS
// @User: Advanced
AP_GROUPINFO("GPS_TYPE", 1, NavEKF2, _fusionModeGPS, 0),
// @Param: VELNE_M_NSE
// @DisplayName: GPS horizontal velocity measurement noise (m/s)
// @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set horizontal velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS horizontal velocity measurements.
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF2, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT),
// @Param: VELD_M_NSE
// @DisplayName: GPS vertical velocity measurement noise (m/s)
// @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set vertical velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS vertical velocity measurements.
// @Range: 0.05 5.0
// @Increment: 0.05
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("VELD_M_NSE", 3, NavEKF2, _gpsVertVelNoise, VELD_M_NSE_DEFAULT),
// @Param: VEL_I_GATE
// @DisplayName: GPS velocity innovation gate size
// @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("VEL_I_GATE", 4, NavEKF2, _gpsVelInnovGate, VEL_I_GATE_DEFAULT),
// @Param: POSNE_M_NSE
// @DisplayName: GPS horizontal position measurement noise (m)
// @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("POSNE_M_NSE", 5, NavEKF2, _gpsHorizPosNoise, POSNE_M_NSE_DEFAULT),
// @Param: POS_I_GATE
// @DisplayName: GPS position measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("POS_I_GATE", 6, NavEKF2, _gpsPosInnovGate, POS_I_GATE_DEFAULT),
// @Param: GLITCH_RAD
// @DisplayName: GPS glitch radius gate size (m)
// @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position.
// @Range: 10 100
// @Increment: 5
// @User: Advanced
// @Units: m
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// @Param: GPS_DELAY
// @DisplayName: GPS measurement delay (msec)
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("GPS_DELAY", 8, NavEKF2, _gpsDelay_ms, 220),
// Height measurement parameters
// @Param: ALT_SOURCE
// @DisplayName: Primary altitude sensor source
// @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK2_GPS_TYPE = 3). Setting 2 uses GPS. Setting 3 uses the range beacon data. NOTE - the EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
// @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("ALT_SOURCE", 9, NavEKF2, _altSource, 0),
// @Param: ALT_M_NSE
// @DisplayName: Altitude measurement noise (m)
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("ALT_M_NSE", 10, NavEKF2, _baroAltNoise, ALT_M_NSE_DEFAULT),
// @Param: HGT_I_GATE
// @DisplayName: Height measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("HGT_I_GATE", 11, NavEKF2, _hgtInnovGate, HGT_I_GATE_DEFAULT),
// @Param: HGT_DELAY
// @DisplayName: Height measurement delay (msec)
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("HGT_DELAY", 12, NavEKF2, _hgtDelay_ms, 60),
// Magnetometer measurement parameters
// @Param: MAG_M_NSE
// @DisplayName: Magnetometer measurement noise (Gauss)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.01 0.5
// @Increment: 0.01
// @User: Advanced
// @Units: Gauss
AP_GROUPINFO("MAG_M_NSE", 13, NavEKF2, _magNoise, MAG_M_NSE_DEFAULT),
// @Param: MAG_CAL
// @DisplayName: Magnetometer default fusion mode
// @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK2_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK2_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK2_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK2_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK2_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter.
// @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
// @User: Advanced
AP_GROUPINFO("MAG_CAL", 14, NavEKF2, _magCal, MAG_CAL_DEFAULT),
// @Param: MAG_I_GATE
// @DisplayName: Magnetometer measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("MAG_I_GATE", 15, NavEKF2, _magInnovGate, MAG_I_GATE_DEFAULT),
// Airspeed measurement parameters
// @Param: EAS_M_NSE
// @DisplayName: Equivalent airspeed measurement noise (m/s)
// @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("EAS_M_NSE", 16, NavEKF2, _easNoise, 1.4f),
// @Param: EAS_I_GATE
// @DisplayName: Airspeed measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("EAS_I_GATE", 17, NavEKF2, _tasInnovGate, 400),
// Rangefinder measurement parameters
// @Param: RNG_M_NSE
// @DisplayName: Range finder measurement noise (m)
// @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("RNG_M_NSE", 18, NavEKF2, _rngNoise, 0.5f),
// @Param: RNG_I_GATE
// @DisplayName: Range finder measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("RNG_I_GATE", 19, NavEKF2, _rngInnovGate, 500),
// Optical flow measurement parameters
// @Param: MAX_FLOW
// @DisplayName: Maximum valid optical flow rate
// @Description: This sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
// @Range: 1.0 4.0
// @Increment: 0.1
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("MAX_FLOW", 20, NavEKF2, _maxFlowRate, 2.5f),
// @Param: FLOW_M_NSE
// @DisplayName: Optical flow measurement noise (rad/s)
// @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 1.0
// @Increment: 0.05
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("FLOW_M_NSE", 21, NavEKF2, _flowNoise, FLOW_M_NSE_DEFAULT),
// @Param: FLOW_I_GATE
// @DisplayName: Optical Flow measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("FLOW_I_GATE", 22, NavEKF2, _flowInnovGate, FLOW_I_GATE_DEFAULT),
// @Param: FLOW_DELAY
// @DisplayName: Optical Flow measurement delay (msec)
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
// @Range: 0 127
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("FLOW_DELAY", 23, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),
// State and Covariance Predition Parameters
// @Param: GYRO_P_NSE
// @DisplayName: Rate gyro noise (rad/s)
// @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
// @Range: 0.0001 0.1
// @Increment: 0.0001
// @User: Advanced
// @Units: rad/s
AP_GROUPINFO("GYRO_P_NSE", 24, NavEKF2, _gyrNoise, GYRO_P_NSE_DEFAULT),
// @Param: ACC_P_NSE
// @DisplayName: Accelerometer noise (m/s^2)
// @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
// @Range: 0.01 1.0
// @Increment: 0.01
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("ACC_P_NSE", 25, NavEKF2, _accNoise, ACC_P_NSE_DEFAULT),
// @Param: GBIAS_P_NSE
// @DisplayName: Rate gyro bias stability (rad/s/s)
// @Description: This state process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier.
// @Range: 0.00001 0.001
// @User: Advanced
// @Units: rad/s/s
AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF2, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
// @Param: GSCL_P_NSE
// @DisplayName: Rate gyro scale factor stability (1/s)
// @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
// @Range: 0.000001 0.001
// @User: Advanced
// @Units: Hz
AP_GROUPINFO("GSCL_P_NSE", 27, NavEKF2, _gyroScaleProcessNoise, GSCALE_P_NSE_DEFAULT),
// @Param: ABIAS_P_NSE
// @DisplayName: Accelerometer bias stability (m/s^3)
// @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
// @Range: 0.00001 0.001
// @User: Advanced
// @Units: m/s/s/s
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF2, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE
// @Param: WIND_P_NSE
// @DisplayName: Wind velocity process noise (m/s^2)
// @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
// @Range: 0.01 1.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("WIND_P_NSE", 30, NavEKF2, _windVelProcessNoise, 0.1f),
// @Param: WIND_PSCALE
// @DisplayName: Height rate to wind process noise scaler
// @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser.
// @Range: 0.0 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("WIND_PSCALE", 31, NavEKF2, _wndVarHgtRateScale, 0.5f),
// @Param: GPS_CHECK
// @DisplayName: GPS preflight check
// @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat.
// @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
// @User: Advanced
AP_GROUPINFO("GPS_CHECK", 32, NavEKF2, _gpsCheck, 31),
// @Param: IMU_MASK
// @DisplayName: Bitmask of active IMUs
// @Description: 1 byte bitmap of IMUs to use in EKF2. A separate instance of EKF2 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF2 will fail to start.
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("IMU_MASK", 33, NavEKF2, _imuMask, 3),
// @Param: CHECK_SCALE
// @DisplayName: GPS accuracy check scaler (%)
// @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error.
// @Range: 50 200
// @User: Advanced
// @Units: %
AP_GROUPINFO("CHECK_SCALE", 34, NavEKF2, _gpsCheckScaler, CHECK_SCALER_DEFAULT),
// @Param: NOAID_M_NSE
// @DisplayName: Non-GPS operation position uncertainty (m)
// @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
// @Range: 0.5 50.0
// @User: Advanced
// @Units: m
AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
// @Param: LOG_MASK
// @DisplayName: EKF sensor logging IMU mask
// @Description: This sets the IMU mask of sensors to do full logging for
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("LOG_MASK", 36, NavEKF2, _logging_mask, 1),
// control of magentic yaw angle fusion
// @Param: YAW_M_NSE
// @DisplayName: Yaw measurement noise (rad)
// @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 1.0
// @Increment: 0.05
// @User: Advanced
// @Units: rad
AP_GROUPINFO("YAW_M_NSE", 37, NavEKF2, _yawNoise, 0.5f),
// @Param: YAW_I_GATE
// @DisplayName: Yaw measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 300),
// @Param: TAU_OUTPUT
// @DisplayName: Output complementary filter time constant (centi-sec)
// @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
// @Range: 10 50
// @Increment: 5
// @User: Advanced
// @Units: cs
AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 25),
// @Param: MAGE_P_NSE
// @DisplayName: Earth magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: Gauss/s
AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF2, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT),
// @Param: MAGB_P_NSE
// @DisplayName: Body magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: Gauss/s
AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF2, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT),
// @Param: RNG_USE_HGT
// @DisplayName: Range finder switch height percentage
// @Description: The range finder will be used as the primary height source when below a specified percentage of the sensor maximum as set by the RNGFND_MAX_CM parameter. Set to -1 to prevent range finder use.
// @Range: -1 70
// @Increment: 1
// @User: Advanced
// @Units: %
AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF2, _useRngSwHgt, -1),
// @Param: TERR_GRAD
// @DisplayName: Maximum terrain gradient
// @Description: Specifies the maximum gradient of the terrain below the vehicle when it is using range finder as a height reference
// @Range: 0 0.2
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("TERR_GRAD", 43, NavEKF2, _terrGradMax, 0.1f),
// @Param: BCN_M_NSE
// @DisplayName: Range beacon measurement noise (m)
// @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement.
// @Range: 0.1 10.0
// @Increment: 0.1
// @User: Advanced
// @Units: m
AP_GROUPINFO("BCN_M_NSE", 44, NavEKF2, _rngBcnNoise, 1.0f),
// @Param: BCN_I_GTE
// @DisplayName: Range beacon measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the range beacon measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("BCN_I_GTE", 45, NavEKF2, _rngBcnInnovGate, 500),
// @Param: BCN_DELAY
// @DisplayName: Range beacon measurement delay (msec)
// @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
// @Range: 0 127
// @Increment: 10
// @User: Advanced
// @Units: ms
// @RebootRequired: True
AP_GROUPINFO("BCN_DELAY", 46, NavEKF2, _rngBcnDelay_ms, 50),
// @Param: RNG_USE_SPD
// @DisplayName: Range finder max ground speed
// @Description: The range finder will not be used as the primary height source when the horizontal ground speed is greater than this value.
// @Range: 2.0 6.0
// @Increment: 0.5
// @User: Advanced
// @Units: m/s
AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF2, _useRngSwSpd, 2.0f),
// @Param: MAG_MASK
// @DisplayName: Bitmask of active EKF cores that will always use heading fusion
// @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely.
// @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MAG_MASK", 48, NavEKF2, _magMask, 0),
// @Param: OGN_HGT_MASK
// @DisplayName: Bitmask control of EKF reference height correction
// @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position or to the reported EKF origin height (default).
// @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("OGN_HGT_MASK", 49, NavEKF2, _originHgtMode, 0),
AP_GROUPEND
};
用visio流程图总结了以下：
找到状态转移变量和观测变量更方便我们后面对EKF2的理解
CovarianceInit();函数
void NavEKF2_core::StoreOutputReset()+
EKF2初始化总结：
到这里初始化函数基本完成，我们总结下：开始进行计算我们的飞控有几组IMU,根据IMU数，就创建多少用于进行EKF2处理的对象，然后根据创建的不同的IMU对象进行传感器初始化
（3）EKF代码流程分析： EKF2.UpdateFilter()
下面我们开始看EKF滤波处理函数，这个是最重要的函数，上面只是进行了传感器初始化，下面我们要找到Kalman的五条公式，也是要找到EKF变成KF的公式
void NavEKF2::UpdateFilter(void)
{
if (!core)
{
return;
}
imuSampleTime_us = AP_HAL::micros64(); //获取imu采样时间
const AP_InertialSensor &ins = AP::ins(); //获取ins数据
bool statePredictEnabled[num_cores];
printf("num_cores=%d\r\n",num_cores);
for (uint8_t i=0; i<num_cores; i++) //因为前面有两个内核，所以这里对所有的满足条件的imu内核都进行EKF处理
{
printf("FFF1\r\n");
// if we have not overrun by more than 3 IMU frames, and we
// have already used more than 1/3 of the CPU budget for this
// loop then suppress the prediction step. This allows
// multiple EKF instances to cooperate on scheduling
if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
(AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3)
{
printf("FFF2\r\n");
statePredictEnabled[i] = false;
} else
{
printf("FFF3\r\n");
statePredictEnabled[i] = true;
}
printf("FFF4\r\n");
core[i].UpdateFilter(statePredictEnabled[i]); //重要函数
}
// If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
// Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
// due to initial alignment fluctuations and race conditions
if (!runCoreSelection)
{
static uint64_t lastUnhealthyTime_us = 0;
if (!core[primary].healthy() || lastUnhealthyTime_us == 0)
{
lastUnhealthyTime_us = imuSampleTime_us;
}
runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
}
float primaryErrorScore = core[primary].errorScore();
if ((primaryErrorScore > 1.0f || !core[primary].healthy()) && runCoreSelection)
{
float lowestErrorScore = 0.67f * primaryErrorScore;
uint8_t newPrimaryIndex = primary; // index for new primary
for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++)
{
if (coreIndex != primary) {
// an alternative core is available for selection only if healthy and if states have been updated on this time step
bool altCoreAvailable = core[coreIndex].healthy() && statePredictEnabled[coreIndex];
// If the primary core is unhealthy and another core is available, then switch now
// If the primary core is still healthy,then switching is optional and will only be done if
// a core with a significantly lower error score can be found
float altErrorScore = core[coreIndex].errorScore();
if (altCoreAvailable && (!core[primary].healthy() || altErrorScore < lowestErrorScore))
{
newPrimaryIndex = coreIndex;
lowestErrorScore = altErrorScore;
}
}
}
// update the yaw and position reset data to capture changes due to the lane switch
if (newPrimaryIndex != primary) {
updateLaneSwitchYawResetData(newPrimaryIndex, primary);
updateLaneSwitchPosResetData(newPrimaryIndex, primary);
updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
primary = newPrimaryIndex;
}
}
check_log_write();
}
继续往下看代码：
core[i].UpdateFilter(statePredictEnabled[i]); //重要函数
void NavEKF2_core::UpdateFilter(bool predict)
{
// Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started
//设置标志用来向ekf2滤波器,指示前端已允许启动新的状态预测周期。
startPredictEnabled = predict;
//如果未初始化状态完成，请不要运行过滤器更新-----------don't run filter updates if states have not been initialised
if (!statesInitialised)
{
return;
}
//启动用于负载测量的定时器-------------------------start the timer used for load measurement
#if EK2_DISABLE_INTERRUPTS
irqstate_t istate = irqsave(); //负载计算，这里先不看
#endif
hal.util->perf_begin(_perf_UpdateFilter);
// 飞行器重启方法--------------------------------TODO - in-flight restart method
//获取更新步骤的开始时间---------------------------get starting time for update step
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
//检查遥控器是否解锁电机和执行必要的检查和模式---------Check arm status and perform required checks and mode changes
controlFilterModes();
//读取IMU数据作为角度和速度-------------------------read IMU data as delta angles and velocities
readIMUData();
// Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
//如果在缓冲区中存在新的IMU数据，在满足的融合时间内，运行EKF方程
if (runUpdates)
{
//预测方程：使用IMU的数据，从延迟尺度时间-------Predict states using IMU data from the delayed time horizon
UpdateStrapdownEquationsNED();
//预测协方差增长----------------------------Predict the covariance growth
CovariancePrediction();
//使用地磁更新滤波器状态---------------------Update states using magnetometer data
SelectMagFusion();
//使用GPS和气压计进行状态更新----------------Update states using GPS and altimeter data
SelectVelPosFusion();
//使用测距仪器更新滤波器状态-----------------Update states using range beacon data
SelectRngBcnFusion();
//使用光流传感器进行更新数据-----------------Update states using optical flow data
SelectFlowFusion();
//使用空速计数据进行更新数据-----------------Update states using airspeed data
SelectTasFusion();
//更新应用侧滑约束假设的飞越飞行器状态---------Update states using sideslip constraint assumption for fly-forward vehicles
SelectBetaFusion();
//更新滤波器的状态-------------------------Update the filter status
updateFilterStatus();
}
//从融合数据到数据输出-------------------------Wind output forward from the fusion to output time horizon
calcOutputStates();
//停止用于负载测量的计时器----------------------stop the timer used for load measurement
hal.util->perf_end(_perf_UpdateFilter);
#if EK2_DISABLE_INTERRUPTS
irqrestore(istate);
#endif
}
我们对这段代码重点进行分析
（1）controlFilterModes()函数检查点击解锁和必要的检查和模式改变;
（2）读取传感器数据函数：readIMUData()
（3）重要的函数EKF2预测方程：UpdateStrapdownEquationsNED();
这里后面还得重点分析
（4）更新协方差矩阵 CovariancePrediction();
void NavEKF2_core::CovariancePrediction()
{
hal.util->perf_begin(_perf_CovariancePrediction);
float windVelSigma; // wind velocity 1-sigma process noise - m/s
float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
float dAngScaleSigma;// delta angle scale factor 1-Sigma process noise
float magEarthSigma;// earth magnetic field 1-sigma process noise
float magBodySigma; // body magnetic field 1-sigma process noise
float daxNoise; // X axis delta angle noise variance rad^2
float dayNoise; // Y axis delta angle noise variance rad^2
float dazNoise; // Z axis delta angle noise variance rad^2
float dvxNoise; // X axis delta velocity variance noise (m/s)^2
float dvyNoise; // Y axis delta velocity variance noise (m/s)^2
float dvzNoise; // Z axis delta velocity variance noise (m/s)^2
float dvx; // X axis delta velocity (m/s)
float dvy; // Y axis delta velocity (m/s)
float dvz; // Z axis delta velocity (m/s)
float dax; // X axis delta angle (rad)
float day; // Y axis delta angle (rad)
float daz; // Z axis delta angle (rad)
float q0; // attitude quaternion
float q1; // attitude quaternion
float q2; // attitude quaternion
float q3; // attitude quaternion
float dax_b; // X axis delta angle measurement bias (rad)
float day_b; // Y axis delta angle measurement bias (rad)
float daz_b; // Z axis delta angle measurement bias (rad)
float dax_s; // X axis delta angle measurement scale factor
float day_s; // Y axis delta angle measurement scale factor
float daz_s; // Z axis delta angle measurement scale factor
float dvz_b; // Z axis delta velocity measurement bias (rad)
// calculate covariance prediction process noise
// use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects.
// filter height rate using a 10 second time constant filter
dt = imuDataDelayed.delAngDT;
float alpha = 0.1f * dt;
hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
// use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects.
windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);
dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);
dAngScaleSigma = dt * constrain_float(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f);
magEarthSigma = dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f);
magBodySigma = dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f);
for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f;
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
if (expectGndEffectTakeoff) {
processNoise[15] = 0.0f;
} else {
processNoise[15] = dVelBiasSigma;
}
for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma;
for (uint8_t i=19; i<=21; i++) processNoise[i] = magBodySigma;
for (uint8_t i=22; i<=23; i++) processNoise[i] = windVelSigma;
for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]);
// set variables used to calculate covariance growth
dvx = imuDataDelayed.delVel.x;
dvy = imuDataDelayed.delVel.y;
dvz = imuDataDelayed.delVel.z;
dax = imuDataDelayed.delAng.x;
day = imuDataDelayed.delAng.y;
daz = imuDataDelayed.delAng.z;
q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1];
q2 = stateStruct.quat[2];
q3 = stateStruct.quat[3];
dax_b = stateStruct.gyro_bias.x;
day_b = stateStruct.gyro_bias.y;
daz_b = stateStruct.gyro_bias.z;
dax_s = stateStruct.gyro_scale.x;
day_s = stateStruct.gyro_scale.y;
daz_s = stateStruct.gyro_scale.z;
dvz_b = stateStruct.accel_zbias;
float _gyrNoise = constrain_float(frontend->_gyrNoise, 0.0f, 1.0f);
daxNoise = dayNoise = dazNoise = sq(dt*_gyrNoise);
float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f);
dvxNoise = dvyNoise = dvzNoise = sq(dt*_accNoise);
// calculate the predicted covariance due to inertial sensor error propagation
// we calculate the upper diagonal and copy to take advantage of symmetry
SF[0] = daz_b/2 - (daz*daz_s)/2;
SF[1] = day_b/2 - (day*day_s)/2;
SF[2] = dax_b/2 - (dax*dax_s)/2;
SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2;
SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2;
SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2;
SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2;
SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2;
SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2;
SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2;
SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2;
SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2;
SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2;
SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2;
SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2;
SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
SF[16] = dvz_b - dvz;
SF[17] = dvx;
SF[18] = dvy;
SF[19] = sq(q2);
SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3);
SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3);
SF[22] = 2*q0*q1 - 2*q2*q3;
SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3);
SF[24] = 2*q1*q2;
SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
SG[1] = sq(q3);
SG[2] = sq(q2);
SG[3] = sq(q1);
SG[4] = sq(q0);
SQ[0] = - dvyNoise*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
SQ[1] = dvxNoise*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvzNoise*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
SQ[2] = dvyNoise*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxNoise*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
SQ[3] = sq(SG[0]);
SQ[4] = 2*q2*q3;
SQ[5] = 2*q1*q3;
SQ[6] = 2*q1*q2;
SQ[7] = SG[4];
SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];
SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10];
SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12];
SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14];
SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12];
SPP[7] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3);
SPP[8] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9];
SPP[9] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9];
SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3);
SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3);
SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3);
SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9];
SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13];
SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3);
SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3);
SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3);
SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3);
SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3);
SPP[20] = SF[16]*SF[21] - SF[18]*SF[22];
SPP[21] = 2*q0*q2 + 2*q1*q3;
SPP[22] = SF[15];
if (inhibitMagStates) {
zeroRows(P,16,21);
zeroCols(P,16,21);
} else if (inhibitWindStates) {
zeroRows(P,22,23);
zeroCols(P,22,23);
}
nextP[0][0] = daxNoise*SQ[3] + SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[9][0]*SPP[22] + P[12][0]*SPP[18] + P[2][0]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[9][1]*SPP[22] + P[12][1]*SPP[18] + P[2][1]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[8]*(P[0][2]*SPP[5] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18] - P[1][2]*(2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12])) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[9][9]*SPP[22] + P[12][9]*SPP[18] + P[2][9]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[9][12]*SPP[22] + P[12][12]*SPP[18] + P[2][12]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]));
nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18]) - (2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9])*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
nextP[1][1] = dayNoise*SQ[3] - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[9]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17]);
nextP[0][2] = SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]);
nextP[1][2] = SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]);
nextP[2][2] = dazNoise*SQ[3] - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]);
nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + dvyNoise*sq(SQ[6] - 2*q0*q3) + dvzNoise*sq(SQ[5] + 2*q0*q2) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[2][15]*SPP[15] - P[15][15]*SPP[21] + P[1][15]*(SF[16]*SF[23] - SF[17]*SPP[21])) + dvxNoise*sq(SG[1] + SG[2] - SG[3] - SQ[7]);
nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]);
nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]);
nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]);
nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + dvxNoise*sq(SQ[6] + 2*q0*q3) + dvzNoise*sq(SQ[4] - 2*q0*q1) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + dvyNoise*sq(SG[1] - SG[2] + SG[3] - SQ[7]);
nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[7]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[7]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[7]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[7]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]);
nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[7]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]);
nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + dvxNoise*sq(SQ[5] - 2*q0*q2) + dvyNoise*sq(SQ[4] + 2*q0*q1) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[7]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[7] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[7] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[7] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + dvzNoise*sq(SG[1] - SG[2] - SG[3] + SQ[7]);
nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[8] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18]);
nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[9] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17]);
nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]);
nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]);
nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]);
nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[7] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[7] + P[1][3]*SPP[10] + P[2][3]*SPP[0]);
nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt);
nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[8] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18]);
nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[9] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17]);
nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]);
nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]);
nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]);
nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[7] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[7] + P[1][4]*SPP[10] + P[2][4]*SPP[0]);
nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt);
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[8] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18]);
nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[9] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17]);
nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]);
nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]);
nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]);
nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[7] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0]);
nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt);
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]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[8] + P[9][9]*SPP[22] + P[12][9]*SPP[18];
nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[9] + P[10][9]*SPP[22] + P[13][9]*SPP[17];
nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16];
nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21];
nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11];
nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[7] + P[1][9]*SPP[10] + P[2][9]*SPP[0];
nextP[6][9] = P[6][9] + P[3][9]*dt;
nextP[7][9] = P[7][9] + P[4][9]*dt;
nextP[8][9] = P[8][9] + P[5][9]*dt;
nextP[9][9] = P[9][9];
nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18];
nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17];
nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16];
nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21];
nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11];
nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[7] + P[1][10]*SPP[10] + P[2][10]*SPP[0];
nextP[6][10] = P[6][10] + P[3][10]*dt;
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];
nextP[10][10] = P[10][10];
nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18];
nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17];
nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16];
nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21];
nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11];
nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[7] + P[1][11]*SPP[10] + P[2][11]*SPP[0];
nextP[6][11] = P[6][11] + P[3][11]*dt;
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];
nextP[10][11] = P[10][11];
nextP[11][11] = P[11][11];
nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[8] + P[9][12]*SPP[22] + P[12][12]*SPP[18];
nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[9] + P[10][12]*SPP[22] + P[13][12]*SPP[17];
nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16];
nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21];
nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11];
nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[7] + P[1][12]*SPP[10] + P[2][12]*SPP[0];
nextP[6][12] = P[6][12] + P[3][12]*dt;
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];
nextP[10][12] = P[10][12];
nextP[11][12] = P[11][12];
nextP[12][12] = P[12][12];
nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18];
nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17];
nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16];
nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21];
nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11];
nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[7] + P[1][13]*SPP[10] + P[2][13]*SPP[0];
nextP[6][13] = P[6][13] + P[3][13]*dt;
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];
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]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18];
nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17];
nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16];
nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21];
nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11];
nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[7] + P[1][14]*SPP[10] + P[2][14]*SPP[0];
nextP[6][14] = P[6][14] + P[3][14]*dt;
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];
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]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18];
nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17];
nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16];
nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21];
nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11];
nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0];
nextP[6][15] = P[6][15] + P[3][15]*dt;
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];
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];
if (stateIndexLim > 15) {
nextP[0][16] = P[0][16]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[8] + P[9][16]*SPP[22] + P[12][16]*SPP[18];
nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[9] + P[10][16]*SPP[22] + P[13][16]*SPP[17];
nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16];
nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21];
nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11];
nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[7] + P[1][16]*SPP[10] + P[2][16]*SPP[0];
nextP[6][16] = P[6][16] + P[3][16]*dt;
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];
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]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[8] + P[9][17]*SPP[22] + P[12][17]*SPP[18];
nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[9] + P[10][17]*SPP[22] + P[13][17]*SPP[17];
nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16];
nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21];
nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11];
nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[7] + P[1][17]*SPP[10] + P[2][17]*SPP[0];
nextP[6][17] = P[6][17] + P[3][17]*dt;
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];
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]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[8] + P[9][18]*SPP[22] + P[12][18]*SPP[18];
nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[9] + P[10][18]*SPP[22] + P[13][18]*SPP[17];
nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16];
nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21];
nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11];
nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[7] + P[1][18]*SPP[10] + P[2][18]*SPP[0];
nextP[6][18] = P[6][18] + P[3][18]*dt;
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];
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]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[8] + P[9][19]*SPP[22] + P[12][19]*SPP[18];
nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[9] + P[10][19]*SPP[22] + P[13][19]*SPP[17];
nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16];
nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21];
nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11];
nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[7] + P[1][19]*SPP[10] + P[2][19]*SPP[0];
nextP[6][19] = P[6][19] + P[3][19]*dt;
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];
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]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[8] + P[9][20]*SPP[22] + P[12][20]*SPP[18];
nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[9] + P[10][20]*SPP[22] + P[13][20]*SPP[17];
nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16];
nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21];
nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11];
nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[7] + P[1][20]*SPP[10] + P[2][20]*SPP[0];
nextP[6][20] = P[6][20] + P[3][20]*dt;
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];
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]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[8] + P[9][21]*SPP[22] + P[12][21]*SPP[18];
nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[9] + P[10][21]*SPP[22] + P[13][21]*SPP[17];
nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16];
nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21];
nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11];
nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[7] + P[1][21]*SPP[10] + P[2][21]*SPP[0];
nextP[6][21] = P[6][21] + P[3][21]*dt;
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];
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];
if (stateIndexLim > 21) {
nextP[0][22] = P[0][22]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[8] + P[9][22]*SPP[22] + P[12][22]*SPP[18];
nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[9] + P[10][22]*SPP[22] + P[13][22]*SPP[17];
nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16];
nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21];
nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11];
nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[7] + P[1][22]*SPP[10] + P[2][22]*SPP[0];
nextP[6][22] = P[6][22] + P[3][22]*dt;
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];
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]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[8] + P[9][23]*SPP[22] + P[12][23]*SPP[18];
nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[9] + P[10][23]*SPP[22] + P[13][23]*SPP[17];
nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16];
nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21];
nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11];
nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[7] + P[1][23]*SPP[10] + P[2][23]*SPP[0];
nextP[6][23] = P[6][23] + P[3][23]*dt;
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];
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];
}
}
// Copy upper diagonal to lower diagonal taking advantage of symmetry
for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++)
{
for (uint8_t rowIndex=0; rowIndex<colIndex; rowIndex++)
{
nextP[colIndex][rowIndex] = nextP[rowIndex][colIndex];
}
}
// add the general state process noise variances
for (uint8_t i=0; i<=stateIndexLim; i++)
{
nextP[i][i] = nextP[i][i] + processNoise[i];
}
// if the total position variance exceeds 1e4 (100m), then stop covariance
// growth by setting the predicted to the previous values
// This prevent an ill conditioned matrix from occurring for long periods
// without GPS
if ((P[6][6] + P[7][7]) > 1e4f)
{
for (uint8_t i=6; i<=7; i++)
{
for (uint8_t j=0; j<=stateIndexLim; j++)
{
nextP[i][j] = P[i][j];
nextP[j][i] = P[j][i];
}
}
}
// copy covariances to output
CopyCovariances();
// constrain diagonals to prevent ill-conditioning
ConstrainVariances();
hal.util->perf_end(_perf_CovariancePrediction);
}（5）更新状态
（6）计算增益kg，输出数据，更新协方差 calcOutputStates();
void NavEKF2_core::calcOutputStates()
{
//对IMU数据进行更正------------------apply corrections to the IMU data
Vector3f delAngNewCorrected = imuDataNew.delAng; //获取陀螺仪积分角度
Vector3f delVelNewCorrected = imuDataNew.delVel; //获取加速度积分速度
correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT); //更正角度
correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT); //修正加速度积分的速度
//imu积分的角度加上ekf限制角度的修正值--------apply corections to track EKF solution
Vector3f delAng = delAngNewCorrected + delAngCorrection;
//将获取的角度矢量转换到四元数----------------convert the rotation vector to its equivalent quaternion
Quaternion deltaQuat;
deltaQuat.from_axis_angle(delAng);
//过从先前的姿态旋转来更新四元数状态--------------update the quaternion states by rotating from the previous attitude through
//对获取的四元数进行归一化---------------------the delta angle rotation quaternion and normalise
outputDataNew.quat *= deltaQuat;
outputDataNew.quat.normalize();
//计算从机体坐标系到导航坐标系的旋转矩阵----------calculate the body to nav cosine matrix
Matrix3f Tbn_temp;
outputDataNew.quat.rotation_matrix(Tbn_temp); //四元数到旋转矩阵
//转变机体速度到导航坐标系中--------------------transform body delta velocities to delta velocities in the nav frame
Vector3f delVelNav = Tbn_temp*delVelNewCorrected;
delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
//保存速度值，为了下一次使用--------------------save velocity for use in trapezoidal integration for position calcuation
Vector3f lastVelocity = outputDataNew.velocity;
//计算当前速度-------------------------------sum delta velocities to get velocity
outputDataNew.velocity += delVelNav;
//通过开始速度和当前速度计算位置信息--------------apply a trapezoidal integration to velocities to calculate position
outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
// If the IMU accelerometer is offset from the body frame origin, then calculate corrections
// that can be added to the EKF velocity and position outputs so that they represent the velocity
// and position of the body frame origin.
// Note the * operator has been overloaded to operate as a dot product
//如果IMU加速度计与机体原点偏移，则计算可以添加到EKF速度和位置输出的校正，以便它们代表机体原点的速度和位置。注意*运算符已被重载以作为点积操作。
if (!accelPosOffset.is_zero())
{
// calculate the average angular rate across the last IMU update
// note delAngDT is prevented from being zero in readIMUData()
Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
// Calculate the velocity of the body frame origin relative to the IMU in body frame
// and rotate into earth frame. Note % operator has been overloaded to perform a cross product
Vector3f velBodyRelIMU = angRate % (- accelPosOffset);
velOffsetNED = Tbn_temp * velBodyRelIMU;
// calculate the earth frame position of the body frame origin relative to the IMU
posOffsetNED = Tbn_temp * (- accelPosOffset);
} else
{
velOffsetNED.zero();
posOffsetNED.zero();
}
//不使用 ekf 互补滤波， 到这里姿态、 速度、 位置的预测就完成了
// store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
//把新的数据存储到环形缓冲区buffer,这里不会每次保存的，运行一次ekf，保存一次
if (runUpdates)
{
//存储当前的状态---------store the states at the output time horizon
storedOutput[storedIMU.get_youngest_index()] = outputDataNew;
// recall the states from the fusion time horizon
//获取最久的一次保存， 时间差在 0.02s 以内
outputDataDelayed = storedOutput[storedIMU.get_oldest_index()];
// compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
// divide the demanded quaternion by the estimated to get the error
//将四元数数据与EKF四元数在融合时间域上进行比较，并计算修正所需四元数的估计值，得到误差。
Quaternion quatErr = stateStruct.quat / outputDataDelayed.quat;
//使用小角度旋转近似
// Convert to a delta rotation using a small angle approximation
这个地方是求四元素旋转转换成三角角度表示， 做了一个近似处理， 当 theta 很小的时候，
quatErr.normalize();
Vector3f deltaAngErr;
float scaler;
if (quatErr[0] >= 0.0f)
{
scaler = 2.0f;
} else
{
scaler = -2.0f;
}
deltaAngErr.x = scaler * quatErr[1];
deltaAngErr.y = scaler * quatErr[2];
deltaAngErr.z = scaler * quatErr[3];
// calculate a gain that provides tight tracking of the estimator states and
// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
//这里计算了一个阻尼比， 计算方式也很简单， 就是： 1/2 * (dtIMUavg/timeDelay)
float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms);
timeDelay = fmaxf(timeDelay, dtIMUavg);
float errorGain = 0.5f / timeDelay;
// calculate a corrrection to the delta angle
// that will cause the INS to track the EKF quaternions
计算姿态的修正值， 在 400hz 的循环里用这个值
delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
///计算速度和位置的差------- calculate velocity and position tracking errors
Vector3f velErr = (stateStruct.velocity - outputDataDelayed.velocity);
Vector3f posErr = (stateStruct.position - outputDataDelayed.position);
//这里通过设置的时间常数和 ekf 平均运行的时间相比， 求出一个增益
//用这个增益的的二次函数对速度和位置差的积分进行了处理， 得到了速度
//位置修正值， 这里应该有用原始数据仿真过
//误差收集-----collect magnitude tracking error for diagnostics
outputTrackError.x = deltaAngErr.length();
outputTrackError.y = velErr.length();
outputTrackError.z = posErr.length();
//将用户指定的时间常数从秒秒转换为秒----- convert user specified time constant from centi-seconds to seconds
float tauPosVel = constrain_float(0.01f*(float)frontend->_tauVelPosOutput, 0.1f, 0.5f);
//计算增益以指定时间常数跟踪EKF位置状态----- calculate a gain to track the EKF position states with the specified time constant
float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f);
//使用PI反馈来计算将应用于输出状态历史的校正。---- use a PI feedback to calculate a correction that will be applied to the output state history
posErrintegral += posErr;
velErrintegral += velErr;
Vector3f velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f; //更正后的速度
Vector3f posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f; //更正后位置
// loop through the output filter state history and apply the corrections to the velocity and position states
// this method is too expensive to use for the attitude states due to the quaternion operations required
// but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
// to be used
//循环通过输出滤波器的状态历史，并将校正应用到速度和位置状态。由于四元数操作所需的这种方法对于姿态状态来说太昂贵，但是在“校正环”中不引入时间延迟，
//并且允许较小的跟踪T。要使用的IME常数
output_elements outputStates;
for (unsigned index=0; index < imu_buffer_length; index++)
{
outputStates = storedOutput[index];
//应用等速校正-----a constant velocity correction is applied
outputStates.velocity += velCorrection;
//应用恒定位置校正----- a constant position correction is applied
outputStates.position += posCorrection;
//将更新后的数据推送到缓冲区----- push the updated data to the buffer
storedOutput[index] = outputStates;
}
//将输出状态更新为校正值------ update output state to corrected values
outputDataNew = storedOutput[storedIMU.get_youngest_index()];
}
}
可以参考网上的注释，这里自己还没整理
输出总结：最后得到数据，四元数，速度，位置
还在整理中。。。。。。
