zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
72 Commits
14 Branches
0 Tags
306 MiB
 
 
 
 
 
 
Tree: 001d621eb1
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '001d621eb1'
${ noResults }
px4_autopilot/EKF/covariance.cpp

758 lines
43 KiB
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file covariance.cpp
* Contains functions for initialising, predicting and updating the state
* covariance matrix
*
* @author Roman Bast <bastroman@gmail.com>
*
*/
#include "ekf.h"
#include <math.h>
#include <mathlib/mathlib.h>
#define sq(_arg) powf(_arg, 2.0f)
void Ekf::initialiseCovariance()
{
for (unsigned i = 0; i < _k_num_states; i++) {
for (unsigned j = 0; j < _k_num_states; j++) {
P[i][j] = 0.0f;
}
}
// XXX use initial guess for the diagonal elements for the covariance matrix
// angle error
P[0][0] = 0.001f;
P[1][1] = 0.001f;
P[2][2] = 0.001f;
// velocity
P[3][3] = 0.1f;
P[4][4] = 0.1f;
P[5][5] = 0.1f;
// position
P[6][6] = 0.1f;
P[7][7] = 0.1f;
P[8][8] = 0.1f;
// gyro bias
P[9][9] = 0.00001f;
P[10][10] = 0.00001f;
P[11][11] = 0.00001f;
// gyro scale
P[12][12] = 0.0001f;
P[13][13] = 0.0001f;
P[14][14] = 0.0001f;
// accel z bias
P[15][15] = 0.0001f;
// earth magnetic field
P[16][16] = 0.0001f;
P[17][17] = 0.0001f;
P[18][18] = 0.0001f;
// body magnetic field
P[19][19] = 0.0001f;
P[20][20] = 0.0001f;
P[21][21] = 0.0001f;
// wind
P[22][22] = 0.01f;
P[23][23] = 0.01f;
}
void Ekf::predictCovariance()
{
// assign intermediate state variables
float q0 = _state.quat_nominal(0);
float q1 = _state.quat_nominal(1);
float q2 = _state.quat_nominal(2);
float q3 = _state.quat_nominal(3);
float dax = _imu_sample_delayed.delta_ang(0);
float day = _imu_sample_delayed.delta_ang(1);
float daz = _imu_sample_delayed.delta_ang(2);
float dvx = _imu_sample_delayed.delta_vel(0);
float dvy = _imu_sample_delayed.delta_vel(1);
float dvz = _imu_sample_delayed.delta_vel(2);
float dax_b = _state.gyro_bias(0);
float day_b = _state.gyro_bias(1);
float daz_b = _state.gyro_bias(2);
float dax_s = _state.gyro_scale(0);
float day_s = _state.gyro_scale(1);
float daz_s = _state.gyro_scale(2);
float dvz_b = _state.accel_z_bias;
float dt = _imu_sample_delayed.delta_vel_dt;
// compute process noise
float process_noise[_k_num_states] = {};
float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 1e-6f, 1e-2f);
float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 1e-6f, 1e-2f);
float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f);
float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f);
float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.01f, 1.0f);
for (unsigned i = 0; i < 9; i++) {
process_noise[i] = 0.0f;
}
for (unsigned i = 9; i < 12; i++) {
process_noise[i] = sq(d_ang_bias_sig);
}
for (unsigned i = 12; i < 15; i++) {
process_noise[i] = sq(d_ang_scale_sig);
}
process_noise[15] = sq(d_vel_bias_sig);
for (unsigned i = 16; i < 19; i++) {
process_noise[i] = sq(mag_I_sig);
}
for (unsigned i = 19; i < 22; i++) {
process_noise[i] = sq(mag_B_sig);
}
for (unsigned i = 22; i < 24; i++) {
process_noise[i] = sq(wind_vel_sig);
}
// assign input noise
// inputs to the system are 3 delta angles and 3 delta velocities
float daxNoise, dayNoise, dazNoise;
float dvxNoise, dvyNoise, dvzNoise;
float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-2f);
daxNoise = dayNoise = dazNoise = dt * gyro_noise;
float accel_noise = math::constrain(_params.accel_noise, 1e-2f, 1.0f);
dvxNoise = dvyNoise = dvzNoise = dt * accel_noise;
// predict covarinace matrix
// intermediate calculations
float SF[25] = {};
SF[0] = daz_b / 2 + dazNoise / 2 - (daz * daz_s) / 2;
SF[1] = day_b / 2 + dayNoise / 2 - (day * day_s) / 2;
SF[2] = dax_b / 2 + daxNoise / 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 + dvzNoise;
SF[17] = dvx - dvxNoise;
SF[18] = dvy - dvyNoise;
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;
float SG[5] = {};
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);
float SQ[8] = {};
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];
float SPP[23] = {};
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] = 2 * q1 * SF[3] - 2 * q2 * SF[4] - 2 * q3 * SF[5] + 2 * q0 * SF[9];
SPP[8] = 2 * q0 * SF[5] - 2 * q1 * SF[4] - 2 * q2 * SF[3] + 2 * q3 * SF[9];
SPP[9] = SF[18] * SF[20] - SF[16] * (2 * q0 * q1 + 2 * q2 * q3);
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];
// covariance update
float nextP[24][24] = {};
nextP[0][0] = daxNoise * SQ[3] + SPP[5] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] +
P[12][0] * SPP[18]) - SPP[4] * (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] *
SPP[18]) + SPP[7] * (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) +
SPP[22] * (P[0][9] * SPP[5] - P[1][9] * SPP[4] + P[2][9] * SPP[7] + P[9][9] * SPP[22] + P[12][9] * SPP[18]) +
SPP[18] * (P[0][12] * SPP[5] - P[1][12] * SPP[4] + P[2][12] * SPP[7] + P[9][12] * SPP[22] + P[12][12] * SPP[18]);
nextP[0][1] = SPP[6] * (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + 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[7] + P[9][0] * SPP[22] + P[12][0] * SPP[18]) -
SPP[8] * (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) + SPP[22] *
(P[0][10] * SPP[5] - P[1][10] * SPP[4] + P[2][10] * SPP[7] + 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[7] + P[9][13] * SPP[22] + P[12][13] * SPP[18]);
nextP[1][1] = dayNoise * SQ[3] - SPP[2] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + 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[8] + P[10][1] * SPP[22] + P[13][1] *
SPP[17]) - SPP[8] * (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + 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[8] + 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[8] + P[10][13] * SPP[22] + P[13][13] * SPP[17]);
nextP[0][2] = SPP[14] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + P[9][0] * SPP[22] + P[12][0] *
SPP[18]) - SPP[3] * (P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]) +
SPP[13] * (P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + P[9][2] * SPP[22] + P[12][2] * SPP[18]) +
SPP[22] * (P[0][11] * SPP[5] - P[1][11] * SPP[4] + P[2][11] * SPP[7] + 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[7] + P[9][14] * SPP[22] + P[12][14] * SPP[18]);
nextP[1][2] = SPP[14] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + P[10][0] * SPP[22] + P[13][0] *
SPP[17]) - SPP[3] * (P[1][1] * SPP[6] - P[0][1] * SPP[2] - P[2][1] * SPP[8] + P[10][1] * SPP[22] + P[13][1] * SPP[17]) +
SPP[13] * (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + P[10][2] * SPP[22] + P[13][2] * SPP[17]) +
SPP[22] * (P[1][11] * SPP[6] - P[0][11] * SPP[2] - P[2][11] * SPP[8] + 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[8] + P[10][14] * SPP[22] + P[13][14] * 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[7] + 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[7] + P[9][0] * SPP[22] + P[12][0] * SPP[18]) + SPP[19] *
(P[0][1] * SPP[5] - P[1][1] * SPP[4] + P[2][1] * SPP[7] + P[9][1] * SPP[22] + P[12][1] * SPP[18]) + SPP[15] *
(P[0][2] * SPP[5] - P[1][2] * SPP[4] + P[2][2] * SPP[7] + 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[7] + P[9][15] * SPP[22] + P[12][15] * SPP[18]);
nextP[1][3] = P[1][3] * SPP[6] - P[0][3] * SPP[2] - P[2][3] * SPP[8] + 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[8] + P[10][0] * SPP[22] + P[13][0] * SPP[17]) +
SPP[19] * (P[1][1] * SPP[6] - P[0][1] * SPP[2] - P[2][1] * SPP[8] + P[10][1] * SPP[22] + P[13][1] * SPP[17]) +
SPP[15] * (P[1][2] * SPP[6] - P[0][2] * SPP[2] - P[2][2] * SPP[8] + 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[8] + P[10][15] * SPP[22] + P[13][15] * 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[19] * (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[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]);
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[1][15] * SPP[19] + P[2][15] * SPP[15] - P[15][15] * 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[7] + 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[7] + 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[7] + 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[7] + 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[7] + 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[8] + 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[8] + 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[8] + 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[8] + 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[8] + 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[7] + 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[7] + P[9][15] * SPP[22] + P[12][15] * SPP[18]) -
SPP[9] * (P[0][0] * SPP[5] - P[1][0] * SPP[4] + P[2][0] * SPP[7] + 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[7] + 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[7] + 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[8] + 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[8] + P[10][15] * SPP[22] + P[13][15] * SPP[17]) -
SPP[9] * (P[1][0] * SPP[6] - P[0][0] * SPP[2] - P[2][0] * SPP[8] + 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[8] + 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[8] + 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[9] * (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[9] *
(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[9] *
(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[9] + 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[9]
+ P[1][15] * SPP[10] + P[2][15] * SPP[0]) - SPP[9] * (P[5][0] + P[15][0] * SF[20] - P[0][0] * SPP[9] + P[1][0] * SPP[10]
+ P[2][0] * SPP[0]) + SPP[0] * (P[5][2] + P[15][2] * SF[20] - P[0][2] * SPP[9] + P[1][2] * SPP[10] + P[2][2] * SPP[0]) +
SPP[10] * (P[5][1] + P[15][1] * SF[20] - P[0][1] * SPP[9] + 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[7] + 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[7] + 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[8] + 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[8] + 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[9] + P[1][6] * SPP[10] + P[2][6] * SPP[0] + dt *
(P[5][3] + P[15][3] * SF[20] - P[0][3] * SPP[9] + 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[7] + 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[7] + 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[8] + 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[8] + 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[9] + P[1][7] * SPP[10] + P[2][7] * SPP[0] + dt *
(P[5][4] + P[15][4] * SF[20] - P[0][4] * SPP[9] + 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[7] + 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[7] + 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[8] + 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[8] + 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[9] + P[1][8] * SPP[10] + P[2][8] * SPP[0] + dt *
(P[5][5] + P[15][5] * SF[20] - P[0][5] * SPP[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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];
nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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];
nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + 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[8] + 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[9] + 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[7] + 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[8] + 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[9] + 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];
// add process noise
for (unsigned i = 0; i < _k_num_states; i++) {
nextP[i][i] += process_noise[i];
}
// stop position covariance growth if our total position variance reaches 100m
// this can happen if we loose gps for some time
if ((P[6][6] + P[7][7]) > 1e4f) {
for (uint8_t i = 6; i < 8; i++) {
for (uint8_t j = 0; j < _k_num_states; j++) {
nextP[i][j] = P[i][j];
nextP[j][i] = P[j][i];
}
}
}
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 1; row < _k_num_states; row++) {
for (unsigned column = 0 ; column < row; column++) {
nextP[row][column] = nextP[column][row];
}
}
for (unsigned i = 0; i < _k_num_states; i++) {
P[i][i] = nextP[i][i];
}
for (unsigned row = 1; row < _k_num_states; row++) {
for (unsigned column = 0; column < row; column++) {
P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]);
P[column][row] = P[row][column];
}
}
limitCov();
}
void Ekf::limitCov()
{
// Covariance diagonal limits. Use same values for states which
// belong to the same group (e.g. vel_x, vel_y, vel_z)
float P_lim[9] = {};
P_lim[0] = 1.0f; // angle error max var
P_lim[1] = 1000.0f; // velocity max var
P_lim[2] = 1000000.0f; // positiion max var
P_lim[3] = 0.001f; // gyro bias max var
P_lim[4] = 0.01f; // gyro scale max var
P_lim[5] = 0.1f; // delta velocity z bias max var
P_lim[6] = 0.01f; // earth mag field max var
P_lim[7] = 0.01f; // body mag field max var
P_lim[8] = 1000.0f; // wind max var
for (int i = 0; i < 3; i++) {
math::constrain(P[i][i], 0.0f, P_lim[0]);
}
for (int i = 3; i < 6; i++) {
math::constrain(P[i][i], 0.0f, P_lim[1]);
}
for (int i = 6; i < 9; i++) {
math::constrain(P[i][i], 0.0f, P_lim[2]);
}
for (int i = 9; i < 12; i++) {
math::constrain(P[i][i], 0.0f, P_lim[3]);
}
for (int i = 12; i < 15; i++) {
math::constrain(P[i][i], 0.0f, P_lim[4]);
}
math::constrain(P[15][15], 0.0f, P_lim[5]);
for (int i = 16; i < 19; i++) {
math::constrain(P[i][i], 0.0f, P_lim[6]);
}
for (int i = 19; i < 22; i++) {
math::constrain(P[i][i], 0.0f, P_lim[7]);
}
for (int i = 22; i < 24; i++) {
math::constrain(P[i][i], 0.0f, P_lim[8]);
}
}