mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
Merge pull request #117 from PX4/ekf2AccuracyImprovement
EKF: Accuracy Improvements
This commit is contained in:
Paul Riseborough
commit
e06c0f41d7
@ -73,26 +73,20 @@ void Ekf::fuseAirspeed()
|
||||
if (v_tas_pred > 1.0f) {
|
||||
// Calculate the observation jacobian
|
||||
// intermediate variable from algebraic optimisation
|
||||
SH_TAS[0] = 1 / v_tas_pred;
|
||||
SH_TAS[1] = (SH_TAS[0] * (2 * ve - 2 * vwe)) / 2.0f;
|
||||
SH_TAS[2] = (SH_TAS[0] * (2 * vn - 2 * vwn)) / 2.0f;
|
||||
SH_TAS[0] = 1.0f/v_tas_pred;
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
|
||||
|
||||
for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; }
|
||||
|
||||
H_TAS[3] = SH_TAS[2];
|
||||
H_TAS[4] = SH_TAS[1];
|
||||
H_TAS[5] = vd * SH_TAS[0];
|
||||
H_TAS[4] = SH_TAS[2];
|
||||
H_TAS[5] = SH_TAS[1];
|
||||
H_TAS[6] = vd*SH_TAS[0];
|
||||
H_TAS[22] = -SH_TAS[2];
|
||||
H_TAS[23] = -SH_TAS[1];
|
||||
|
||||
// We don't want to update the innovation variance if the calculation is ill conditioned
|
||||
float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2] * (P[3][3] * SH_TAS[2] + P[4][3] * SH_TAS[1] - P[22][3] * SH_TAS[2]
|
||||
- P[23][3] * SH_TAS[1] + P[5][3] * vd * SH_TAS[0]) + SH_TAS[1] * (P[3][4] * SH_TAS[2] + P[4][4] * SH_TAS[1] - P[22][4] *
|
||||
SH_TAS[2] - P[23][4] * SH_TAS[1] + P[5][4] * vd * SH_TAS[0]) - SH_TAS[2] * (P[3][22] * SH_TAS[2] + P[4][22] * SH_TAS[1]
|
||||
- P[22][22] * SH_TAS[2] - P[23][22] * SH_TAS[1] + P[5][22] * vd * SH_TAS[0]) - SH_TAS[1] *
|
||||
(P[3][23] * SH_TAS[2] + P[4][23] * SH_TAS[1] - P[22][23] * SH_TAS[2] - P[23][23] * SH_TAS[1] + P[5][23] * vd *
|
||||
SH_TAS[0]) + vd * SH_TAS[0] * (P[3][5] * SH_TAS[2] + P[4][5] * SH_TAS[1] - P[22][5] * SH_TAS[2] - P[23][5] * SH_TAS[1] +
|
||||
P[5][5] * vd * SH_TAS[0]));
|
||||
float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
|
||||
|
||||
if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
|
||||
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
|
||||
@ -104,63 +98,30 @@ void Ekf::fuseAirspeed()
|
||||
|
||||
SK_TAS[1] = SH_TAS[1];
|
||||
|
||||
Kfusion[0] = SK_TAS[0] * (P[0][3] * SH_TAS[2] - P[0][22] * SH_TAS[2] + P[0][4] * SK_TAS[1] - P[0][23] * SK_TAS[1] +
|
||||
P[0][5] * vd * SH_TAS[0]);
|
||||
Kfusion[1] = SK_TAS[0] * (P[1][3] * SH_TAS[2] - P[1][22] * SH_TAS[2] + P[1][4] * SK_TAS[1] - P[1][23] * SK_TAS[1] +
|
||||
P[1][5] * vd * SH_TAS[0]);
|
||||
Kfusion[2] = SK_TAS[0] * (P[2][3] * SH_TAS[2] - P[2][22] * SH_TAS[2] + P[2][4] * SK_TAS[1] - P[2][23] * SK_TAS[1] +
|
||||
P[2][5] * vd * SH_TAS[0]);
|
||||
Kfusion[3] = SK_TAS[0] * (P[3][3] * SH_TAS[2] - P[3][22] * SH_TAS[2] + P[3][4] * SK_TAS[1] - P[3][23] * SK_TAS[1] +
|
||||
P[3][5] * vd * SH_TAS[0]);
|
||||
Kfusion[4] = SK_TAS[0] * (P[4][3] * SH_TAS[2] - P[4][22] * SH_TAS[2] + P[4][4] * SK_TAS[1] - P[4][23] * SK_TAS[1] +
|
||||
P[4][5] * vd * SH_TAS[0]);
|
||||
Kfusion[5] = SK_TAS[0] * (P[5][3] * SH_TAS[2] - P[5][22] * SH_TAS[2] + P[5][4] * SK_TAS[1] - P[5][23] * SK_TAS[1] +
|
||||
P[5][5] * vd * SH_TAS[0]);
|
||||
Kfusion[6] = SK_TAS[0] * (P[6][3] * SH_TAS[2] - P[6][22] * SH_TAS[2] + P[6][4] * SK_TAS[1] - P[6][23] * SK_TAS[1] +
|
||||
P[6][5] * vd * SH_TAS[0]);
|
||||
Kfusion[7] = SK_TAS[0] * (P[7][3] * SH_TAS[2] - P[7][22] * SH_TAS[2] + P[7][4] * SK_TAS[1] - P[7][23] * SK_TAS[1] +
|
||||
P[7][5] * vd * SH_TAS[0]);
|
||||
Kfusion[8] = SK_TAS[0] * (P[8][3] * SH_TAS[2] - P[8][22] * SH_TAS[2] + P[8][4] * SK_TAS[1] - P[8][23] * SK_TAS[1] +
|
||||
P[8][5] * vd * SH_TAS[0]);
|
||||
Kfusion[9] = SK_TAS[0] * (P[9][3] * SH_TAS[2] - P[9][22] * SH_TAS[2] + P[9][4] * SK_TAS[1] - P[9][23] * SK_TAS[1] +
|
||||
P[9][5] * vd * SH_TAS[0]);
|
||||
Kfusion[10] = SK_TAS[0] * (P[10][3] * SH_TAS[2] - P[10][22] * SH_TAS[2] + P[10][4] * SK_TAS[1] - P[10][23] * SK_TAS[1] +
|
||||
P[10][5] * vd * SH_TAS[0]);
|
||||
Kfusion[11] = SK_TAS[0] * (P[11][3] * SH_TAS[2] - P[11][22] * SH_TAS[2] + P[11][4] * SK_TAS[1] - P[11][23] * SK_TAS[1] +
|
||||
P[11][5] * vd * SH_TAS[0]);
|
||||
Kfusion[12] = SK_TAS[0] * (P[12][3] * SH_TAS[2] - P[12][22] * SH_TAS[2] + P[12][4] * SK_TAS[1] - P[12][23] * SK_TAS[1] +
|
||||
P[12][5] * vd * SH_TAS[0]);
|
||||
Kfusion[13] = SK_TAS[0] * (P[13][3] * SH_TAS[2] - P[13][22] * SH_TAS[2] + P[13][4] * SK_TAS[1] - P[13][23] * SK_TAS[1] +
|
||||
P[13][5] * vd * SH_TAS[0]);
|
||||
Kfusion[14] = SK_TAS[0] * (P[14][3] * SH_TAS[2] - P[14][22] * SH_TAS[2] + P[14][4] * SK_TAS[1] - P[14][23] * SK_TAS[1] +
|
||||
P[14][5] * vd * SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS[0] * (P[15][3] * SH_TAS[2] - P[15][22] * SH_TAS[2] + P[15][4] * SK_TAS[1] - P[15][23] * SK_TAS[1] +
|
||||
P[15][5] * vd * SH_TAS[0]);
|
||||
Kfusion[22] = SK_TAS[0] * (P[22][3] * SH_TAS[2] - P[22][22] * SH_TAS[2] + P[22][4] * SK_TAS[1] - P[22][23] * SK_TAS[1] +
|
||||
P[22][5] * vd * SH_TAS[0]);
|
||||
Kfusion[23] = SK_TAS[0] * (P[23][3] * SH_TAS[2] - P[23][22] * SH_TAS[2] + P[23][4] * SK_TAS[1] - P[23][23] * SK_TAS[1] +
|
||||
P[23][5] * vd * SH_TAS[0]);
|
||||
|
||||
// Only update the magnetometer states if we are airborne and using 3D mag fusion
|
||||
if (_control_status.flags.mag_3D && _control_status.flags.in_air) {
|
||||
Kfusion[16] = SK_TAS[0] * (P[16][3] * SH_TAS[2] - P[16][22] * SH_TAS[2] + P[16][4] * SK_TAS[1] - P[16][23] * SK_TAS[1] +
|
||||
P[16][5] * vd * SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS[0] * (P[17][3] * SH_TAS[2] - P[17][22] * SH_TAS[2] + P[17][4] * SK_TAS[1] - P[17][23] * SK_TAS[1] +
|
||||
P[17][5] * vd * SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS[0] * (P[18][3] * SH_TAS[2] - P[18][22] * SH_TAS[2] + P[18][4] * SK_TAS[1] - P[18][23] * SK_TAS[1] +
|
||||
P[18][5] * vd * SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS[0] * (P[19][3] * SH_TAS[2] - P[19][22] * SH_TAS[2] + P[19][4] * SK_TAS[1] - P[19][23] * SK_TAS[1] +
|
||||
P[19][5] * vd * SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS[0] * (P[20][3] * SH_TAS[2] - P[20][22] * SH_TAS[2] + P[20][4] * SK_TAS[1] - P[20][23] * SK_TAS[1] +
|
||||
P[20][5] * vd * SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS[0] * (P[21][3] * SH_TAS[2] - P[21][22] * SH_TAS[2] + P[21][4] * SK_TAS[1] - P[21][23] * SK_TAS[1] +
|
||||
P[21][5] * vd * SH_TAS[0]);
|
||||
|
||||
} else {
|
||||
for (int i = 16; i <= 21; i++) {
|
||||
Kfusion[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
|
||||
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
|
||||
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
|
||||
Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
|
||||
Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
|
||||
Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
|
||||
Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
|
||||
Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
|
||||
Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
|
||||
Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
|
||||
Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
|
||||
Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
|
||||
Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
|
||||
Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
|
||||
Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
|
||||
Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]);
|
||||
Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
|
||||
|
||||
|
||||
// calculate measurement innovation
|
||||
@ -182,19 +143,6 @@ void Ekf::fuseAirspeed()
|
||||
// airspeed measurement sample has passed check so record it
|
||||
_time_last_arsp_fuse = _time_last_imu;
|
||||
|
||||
|
||||
// by definition the angle error state is zero at the fusion time
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// Fuse airspeed measurement
|
||||
fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero?
|
||||
|
||||
// correct the nominal quaternion
|
||||
Quaternion dq;
|
||||
dq.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = dq * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
// update covariance matrix via Pnew = (I - KH)P = P - KHP
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that...
|
||||
@ -210,13 +158,40 @@ void Ekf::fuseAirspeed()
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] = P[row][column] - KHP[row][column];
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_airspeed = false;
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
if (P[i][i] < KHP[i][i]) {
|
||||
// zero rows and columns
|
||||
zeroRows(P,i,i);
|
||||
zeroCols(P,i,i);
|
||||
|
||||
//flag as unhealthy
|
||||
healthy = false;
|
||||
|
||||
// update individual measurement health status
|
||||
_fault_status.flags.bad_airspeed = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
// only apply covariance and state corrrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] = P[row][column] - KHP[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
fuse(Kfusion, _airspeed_innov);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
75
EKF/common.h
75
EKF/common.h
@ -139,12 +139,12 @@ struct flowSample {
|
||||
// Bit locations for fusion_mode
|
||||
#define MASK_USE_GPS (1<<0) // set to true to use GPS data
|
||||
#define MASK_USE_OF (1<<1) // set to true to use optical flow data
|
||||
#define MASK_INHIBIT_ACC_BIAS (1<<2) // set to true to inhibit estimation of accelerometer delta velocity bias
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
|
||||
|
||||
// Maximum sensor intervals in usec
|
||||
#define GPS_MAX_INTERVAL 5e5
|
||||
@ -169,9 +169,8 @@ struct parameters {
|
||||
float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec)
|
||||
|
||||
// process noise
|
||||
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec)
|
||||
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec)
|
||||
float gyro_scale_p_noise; // process noise for gyro scale factor prediction (N/A)
|
||||
float gyro_bias_p_noise; // process noise for IMU rate gyro bias prediction (rad/sec**2)
|
||||
float accel_bias_p_noise; // process noise for IMU accelerometer bias prediction (m/sec**3)
|
||||
float mage_p_noise; // process noise for earth magnetic field prediction (Guass/sec)
|
||||
float magb_p_noise; // process noise for body magnetic field prediction (Guass/sec)
|
||||
float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec)
|
||||
@ -246,15 +245,14 @@ struct parameters {
|
||||
range_delay_ms = 5.0f;
|
||||
|
||||
// input noise
|
||||
gyro_noise = 6.0e-2f;
|
||||
accel_noise = 2.5e-1f;
|
||||
gyro_noise = 1.5e-2f;
|
||||
accel_noise = 3.5e-1f;
|
||||
|
||||
// process noise
|
||||
gyro_bias_p_noise = 2.5e-6f;
|
||||
accel_bias_p_noise = 3.0e-5f;
|
||||
gyro_scale_p_noise = 3.0e-4f;
|
||||
mage_p_noise = 2.5e-3f;
|
||||
magb_p_noise = 5.0e-4f;
|
||||
gyro_bias_p_noise = 1.0e-3f;
|
||||
accel_bias_p_noise = 3.0e-3f;
|
||||
mage_p_noise = 1.0e-3f;
|
||||
magb_p_noise = 1.0e-4f;
|
||||
wind_vel_p_noise = 1.0e-1f;
|
||||
terrain_p_noise = 5.0f;
|
||||
terrain_gradient = 0.5f;
|
||||
@ -275,7 +273,7 @@ struct parameters {
|
||||
mag_declination_deg = 0.0f;
|
||||
heading_innov_gate = 2.6f;
|
||||
mag_innov_gate = 3.0f;
|
||||
mag_declination_source = 3;
|
||||
mag_declination_source = 7;
|
||||
mag_fusion_type = 0;
|
||||
|
||||
// airspeed fusion
|
||||
@ -313,28 +311,36 @@ struct parameters {
|
||||
};
|
||||
|
||||
struct stateSample {
|
||||
Vector3f ang_error; // attitude axis angle error (error state formulation)
|
||||
Quaternion quat_nominal; // quaternion defining the rotaton from earth to body frame
|
||||
Vector3f vel; // NED velocity in earth frame in m/s
|
||||
Vector3f pos; // NED position in earth frame in m
|
||||
Vector3f gyro_bias; // gyro bias estimate in rad/s
|
||||
Vector3f gyro_scale; // gyro scale estimate
|
||||
float accel_z_bias; // accelerometer z axis bias estimate
|
||||
Vector3f accel_bias; // accelerometer bias estimate in m/s
|
||||
Vector3f mag_I; // NED earth magnetic field in gauss
|
||||
Vector3f mag_B; // magnetometer bias estimate in body frame in gauss
|
||||
Vector2f wind_vel; // wind velocity in m/s
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
};
|
||||
|
||||
struct fault_status_t {
|
||||
bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error
|
||||
bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error
|
||||
bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error
|
||||
bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error
|
||||
bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error
|
||||
bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error
|
||||
bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
union fault_status_u {
|
||||
struct {
|
||||
bool bad_mag_x: 1; // 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
|
||||
bool bad_mag_y: 1; // 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
|
||||
bool bad_mag_z: 1; // 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
|
||||
bool bad_mag_hdg: 1; // 3 - true if the fusion of the magnetic heading has encountered a numerical error
|
||||
bool bad_mag_decl: 1; // 4 - true if the fusion of the magnetic declination has encountered a numerical error
|
||||
bool bad_airspeed: 1; // 5 - true if fusion of the airspeed has encountered a numerical error
|
||||
bool bad_sideslip: 1; // 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_optflow_X: 1; // 7 - true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool bad_optflow_Y: 1; // 8 - true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
bool bad_vel_N: 1; // 9 - true if fusion of the North velocity has encountered a numerical error
|
||||
bool bad_vel_E: 1; // 10 - true if fusion of the East velocity has encountered a numerical error
|
||||
bool bad_vel_D: 1; // 11 - true if fusion of the Down velocity has encountered a numerical error
|
||||
bool bad_pos_N: 1; // 12 - true if fusion of the North position has encountered a numerical error
|
||||
bool bad_pos_E: 1; // 13 - true if fusion of the East position has encountered a numerical error
|
||||
bool bad_pos_D: 1; // 14 - true if fusion of the Down position has encountered a numerical error
|
||||
} flags;
|
||||
uint16_t value;
|
||||
|
||||
};
|
||||
|
||||
// publish the status of various GPS quality checks
|
||||
@ -362,15 +368,14 @@ union filter_control_status_u {
|
||||
uint16_t gps : 1; // 2 - true if GPS measurements are being fused
|
||||
uint16_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
|
||||
uint16_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused
|
||||
uint16_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
|
||||
uint16_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused
|
||||
uint16_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused
|
||||
uint16_t in_air : 1; // 8 - true when the vehicle is airborne
|
||||
uint16_t armed : 1; // 9 - true when the vehicle motors are armed
|
||||
uint16_t wind : 1; // 10 - true when wind velocity is being estimated
|
||||
uint16_t baro_hgt : 1; // 11 - true when baro height is being fused as a primary height reference
|
||||
uint16_t rng_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference
|
||||
uint16_t gps_hgt : 1; // 15 - true when range finder height is being fused as a primary height reference
|
||||
uint16_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused
|
||||
uint16_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused
|
||||
uint16_t in_air : 1; // 7 - true when the vehicle is airborne
|
||||
uint16_t armed : 1; // 8 - true when the vehicle motors are armed
|
||||
uint16_t wind : 1; // 9 - true when wind velocity is being estimated
|
||||
uint16_t baro_hgt : 1; // 10 - true when baro height is being fused as a primary height reference
|
||||
uint16_t rng_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference
|
||||
uint16_t gps_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
||||
@ -51,13 +51,9 @@ void Ekf::controlFusionModes()
|
||||
// Get the magnetic declination
|
||||
calcMagDeclination();
|
||||
|
||||
// Check for tilt convergence during initial alignment
|
||||
// filter the tilt error vector using a 1 sec time constant LPF
|
||||
float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
|
||||
_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
|
||||
|
||||
// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
|
||||
if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
|
||||
// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
|
||||
float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
|
||||
if (total_angle_variance < 0.002f && !_control_status.flags.tilt_align) {
|
||||
_control_status.flags.tilt_align = true;
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
}
|
||||
@ -77,18 +73,16 @@ void Ekf::controlFusionModes()
|
||||
_control_status.flags.opt_flow = true;
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
|
||||
// if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading
|
||||
// if we are not using GPS then the velocity and position states and covariances need to be set
|
||||
if (!_control_status.flags.gps) {
|
||||
// calculate the rotation matrix from body to earth frame
|
||||
matrix::Dcm<float> body_to_earth(_state.quat_nominal);
|
||||
|
||||
// constrain height above ground to be above minimum possible
|
||||
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
|
||||
|
||||
// calculate absolute distance from focal point to centre of frame assuming a flat earth
|
||||
float range = heightAboveGndEst / body_to_earth(2, 2);
|
||||
float range = heightAboveGndEst / _R_to_earth(2, 2);
|
||||
|
||||
if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
|
||||
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
|
||||
// we should ahve reliable OF measurements so
|
||||
// calculate X and Y body relative velocities from OF measurements
|
||||
Vector3f vel_optflow_body;
|
||||
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
|
||||
@ -97,14 +91,36 @@ void Ekf::controlFusionModes()
|
||||
|
||||
// rotate from body to earth frame
|
||||
Vector3f vel_optflow_earth;
|
||||
vel_optflow_earth = body_to_earth * vel_optflow_body;
|
||||
vel_optflow_earth = _R_to_earth * vel_optflow_body;
|
||||
|
||||
// take x and Y components
|
||||
_state.vel(0) = vel_optflow_earth(0);
|
||||
_state.vel(1) = vel_optflow_earth(1);
|
||||
|
||||
} else {
|
||||
_state.vel.setZero();
|
||||
_state.vel(0) = 0.0f;
|
||||
_state.vel(1) = 0.0f;
|
||||
}
|
||||
|
||||
// reset the velocity covariance terms
|
||||
zeroRows(P,4,5);
|
||||
zeroCols(P,4,5);
|
||||
|
||||
// reset the horizontal velocity variance using the optical flow noise variance
|
||||
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
|
||||
|
||||
if (!_in_air) {
|
||||
// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
|
||||
_state.pos(0) = 0.0f;
|
||||
_state.pos(1) = 0.0f;
|
||||
|
||||
// reset the coresponding covariances
|
||||
// we are by definition at the origin at commencement so variances are also zeroed
|
||||
zeroRows(P,7,8);
|
||||
zeroCols(P,7,8);
|
||||
|
||||
// align the output observer to the EKF states
|
||||
alignOutputFilter();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -295,18 +311,12 @@ void Ekf::controlFusionModes()
|
||||
// check if baro data is available
|
||||
baroSample baro_init = _baro_buffer.get_newest();
|
||||
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
||||
// check if baro data is consistent
|
||||
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
|
||||
bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
|
||||
|
||||
// reset to baro if data is available and we have no range data
|
||||
// reset to baro if we have no range data and baro data is available
|
||||
bool reset_to_baro = !rng_data_available && baro_data_available;
|
||||
|
||||
// reset to baro if data is acceptable
|
||||
reset_to_baro = reset_to_baro || (baro_data_consistent && baro_data_available && !_baro_hgt_faulty);
|
||||
|
||||
// reset to range data if it is available and we cannot switch to baro
|
||||
bool reset_to_rng = !reset_to_baro && rng_data_available;
|
||||
// reset to range data if it is available
|
||||
bool reset_to_rng = rng_data_available;
|
||||
|
||||
if (reset_to_baro) {
|
||||
// set height sensor health
|
||||
@ -363,7 +373,6 @@ void Ekf::controlFusionModes()
|
||||
if (!_control_status.flags.armed) {
|
||||
// use heading fusion for initial startup
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else {
|
||||
@ -375,13 +384,11 @@ void Ekf::controlFusionModes()
|
||||
|
||||
// use 3D mag fusion when airborne
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// use heading fusion when on the ground
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
}
|
||||
@ -389,13 +396,6 @@ void Ekf::controlFusionModes()
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
|
||||
// always use heading fusion
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {
|
||||
// always use 2D mag fusion
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
|
||||
@ -406,13 +406,11 @@ void Ekf::controlFusionModes()
|
||||
|
||||
// always use 3-axis mag fusion
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// do no magnetometer fusion at all
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_2D = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
81
EKF/ekf.cpp
81
EKF/ekf.cpp
@ -73,6 +73,7 @@ Ekf::Ekf():
|
||||
_time_last_of_fuse(0),
|
||||
_time_last_arsp_fuse(0),
|
||||
_last_disarmed_posD(0.0f),
|
||||
_last_dt_overrun(0.0f),
|
||||
_airspeed_innov(0.0f),
|
||||
_airspeed_innov_var(0.0f),
|
||||
_heading_innov(0.0f),
|
||||
@ -138,14 +139,10 @@ Ekf::~Ekf()
|
||||
bool Ekf::init(uint64_t timestamp)
|
||||
{
|
||||
bool ret = initialise_interface(timestamp);
|
||||
_state.ang_error.setZero();
|
||||
_state.vel.setZero();
|
||||
_state.pos.setZero();
|
||||
_state.gyro_bias.setZero();
|
||||
_state.gyro_scale(0) = 1.0f;
|
||||
_state.gyro_scale(1) = 1.0f;
|
||||
_state.gyro_scale(2) = 1.0f;
|
||||
_state.accel_z_bias = 0.0f;
|
||||
_state.accel_bias.setZero();
|
||||
_state.mag_I.setZero();
|
||||
_state.mag_B.setZero();
|
||||
_state.wind_vel.setZero();
|
||||
@ -182,6 +179,8 @@ bool Ekf::init(uint64_t timestamp)
|
||||
_control_status.value = 0;
|
||||
_control_status_prev.value = 0;
|
||||
|
||||
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERRIOD_MS);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -225,9 +224,6 @@ bool Ekf::update()
|
||||
fuseDeclination();
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
|
||||
fuseMag2D();
|
||||
|
||||
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
|
||||
// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
|
||||
fuseHeading();
|
||||
@ -267,14 +263,18 @@ bool Ekf::update()
|
||||
}
|
||||
|
||||
// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
|
||||
uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
|
||||
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
_fuse_height = true;
|
||||
|
||||
} else {
|
||||
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
|
||||
float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset;
|
||||
_baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f);
|
||||
float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
|
||||
local_time_step = math::constrain(local_time_step,0.0f,1.0f);
|
||||
last_baro_time_us = _baro_sample_delayed.time_us;
|
||||
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
|
||||
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
|
||||
}
|
||||
}
|
||||
|
||||
@ -446,12 +446,10 @@ bool Ekf::initialiseFilter(void)
|
||||
_gps_alt_ref = 0.0f;
|
||||
|
||||
// Zero all of the states
|
||||
_state.ang_error.setZero();
|
||||
_state.vel.setZero();
|
||||
_state.pos.setZero();
|
||||
_state.gyro_bias.setZero();
|
||||
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
|
||||
_state.accel_z_bias = 0.0f;
|
||||
_state.accel_bias.setZero();
|
||||
_state.mag_I.setZero();
|
||||
_state.mag_B.setZero();
|
||||
_state.wind_vel.setZero();
|
||||
@ -477,9 +475,6 @@ bool Ekf::initialiseFilter(void)
|
||||
// update transformation matrix from body to world frame
|
||||
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
|
||||
|
||||
// initialise the filtered alignment error estimate to a larger starting value
|
||||
_tilt_err_length_filt = 1.0f;
|
||||
|
||||
// calculate the averaged magnetometer reading
|
||||
Vector3f mag_init = _mag_filt_state;
|
||||
|
||||
@ -520,18 +515,12 @@ void Ekf::predictState()
|
||||
}
|
||||
}
|
||||
|
||||
// correct delta angles for bias offsets and scale factors
|
||||
Vector3f corrected_delta_ang;
|
||||
corrected_delta_ang(0) = _imu_sample_delayed.delta_ang(0) * _state.gyro_scale(0) - _state.gyro_bias(0);
|
||||
corrected_delta_ang(1) = _imu_sample_delayed.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1);
|
||||
corrected_delta_ang(2) = _imu_sample_delayed.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2);
|
||||
|
||||
// correct delta velocity for bias offsets
|
||||
Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel;
|
||||
corrected_delta_vel(2) -= _state.accel_z_bias;
|
||||
// apply imu bias corrections
|
||||
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.gyro_bias;
|
||||
Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.accel_bias;
|
||||
|
||||
// correct delta angles for earth rotation rate
|
||||
corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
|
||||
corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
|
||||
|
||||
// convert the delta angle to a delta quaternion
|
||||
Quaternion dq;
|
||||
@ -560,30 +549,52 @@ void Ekf::predictState()
|
||||
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
|
||||
|
||||
constrainStates();
|
||||
|
||||
// calculate an average filter update time
|
||||
float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
|
||||
|
||||
// filter and limit input between -50% and +100% of nominal value
|
||||
input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS));
|
||||
_dt_ekf_avg = 0.99f*_dt_ekf_avg + 0.005f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
|
||||
}
|
||||
|
||||
bool Ekf::collect_imu(imuSample &imu)
|
||||
{
|
||||
// accumulate and downsample IMU data across a period FILTER_UPDATE_PERRIOD_MS long
|
||||
|
||||
// copy imu data to local variables
|
||||
_imu_sample_new.delta_ang = imu.delta_ang;
|
||||
_imu_sample_new.delta_vel = imu.delta_vel;
|
||||
_imu_sample_new.delta_ang_dt = imu.delta_ang_dt;
|
||||
_imu_sample_new.delta_vel_dt = imu.delta_vel_dt;
|
||||
_imu_sample_new.time_us = imu.time_us;
|
||||
_imu_sample_new.time_us = imu.time_us;
|
||||
|
||||
// accumulate the time deltas
|
||||
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
|
||||
_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
|
||||
|
||||
// use a quaternion to accumulate delta angle data
|
||||
// this quaternion represents the rotation from the start to end of the accumulation period
|
||||
Quaternion delta_q;
|
||||
delta_q.rotate(imu.delta_ang);
|
||||
_q_down_sampled = _q_down_sampled * delta_q;
|
||||
_q_down_sampled.normalize();
|
||||
|
||||
// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
|
||||
matrix::Dcm<float> delta_R(delta_q.inversed());
|
||||
_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
|
||||
_imu_down_sampled.delta_vel += imu.delta_vel;
|
||||
|
||||
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
|
||||
_dt_imu_avg * _imu_ticks >= 0.02f) {
|
||||
// accumulate the most recent delta velocity data at the updated rotation frame
|
||||
// assume effective sample time is halfway between the previous and current rotation frame
|
||||
_imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f;
|
||||
|
||||
// if the target time delta between filter prediction steps has been exceeded
|
||||
// write the accumulated IMU data to the ring buffer
|
||||
float target_dt = (float)(FILTER_UPDATE_PERRIOD_MS) / 1000;
|
||||
if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) {
|
||||
|
||||
// store the amount we have over-run the target update rate by
|
||||
_last_dt_overrun = _imu_down_sampled.delta_ang_dt - target_dt;
|
||||
|
||||
imu.delta_ang = _q_down_sampled.to_axis_angle();
|
||||
imu.delta_vel = _imu_down_sampled.delta_vel;
|
||||
@ -620,13 +631,13 @@ void Ekf::calculateOutputStates()
|
||||
|
||||
// correct delta angles for bias offsets and scale factors
|
||||
Vector3f delta_angle;
|
||||
delta_angle(0) = _imu_sample_new.delta_ang(0) * _state.gyro_scale(0) - _state.gyro_bias(0);
|
||||
delta_angle(1) = _imu_sample_new.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1);
|
||||
delta_angle(2) = _imu_sample_new.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2);
|
||||
float dt_scale_correction = _dt_imu_avg/_dt_ekf_avg;
|
||||
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0)*dt_scale_correction;
|
||||
delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1)*dt_scale_correction;
|
||||
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2)*dt_scale_correction;
|
||||
|
||||
// correct delta velocity for bias offsets
|
||||
Vector3f delta_vel = _imu_sample_new.delta_vel;
|
||||
delta_vel(2) -= _state.accel_z_bias;
|
||||
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias*dt_scale_correction;
|
||||
|
||||
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
|
||||
delta_angle += _delta_angle_corr;
|
||||
|
||||
24
EKF/ekf.h
24
EKF/ekf.h
@ -121,7 +121,8 @@ public:
|
||||
// return the estimated terrain vertical position relative to the NED origin
|
||||
bool get_terrain_vert_pos(float *ret);
|
||||
|
||||
void get_accel_bias(float *bias) {*bias = _state.accel_z_bias;}
|
||||
// get the accerometer bias in m/s/s
|
||||
void get_accel_bias(float bias[3]);
|
||||
|
||||
// get GPS check status
|
||||
void get_gps_check_status(uint16_t *_gps_check_fail_status);
|
||||
@ -135,6 +136,8 @@ private:
|
||||
const float _k_earth_rate = 0.000072921f;
|
||||
const float _gravity_mss = 9.80665f;
|
||||
|
||||
float _dt_ekf_avg; // average update rate of the ekf
|
||||
|
||||
stateSample _state; // state struct of the ekf running at the delayed time horizon
|
||||
|
||||
bool _filter_initialised; // true when the EKF sttes and covariances been initialised
|
||||
@ -156,6 +159,7 @@ private:
|
||||
uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec)
|
||||
Vector2f _last_known_posNE; // last known local NE position vector (m)
|
||||
float _last_disarmed_posD; // vertical position recorded at arming (m)
|
||||
float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERRIOD_MS (sec)
|
||||
|
||||
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
|
||||
|
||||
@ -177,9 +181,6 @@ private:
|
||||
float _heading_innov; // heading measurement innovation
|
||||
float _heading_innov_var; // heading measurement innovation variance
|
||||
|
||||
Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion
|
||||
float _tilt_err_length_filt; // filtered length of _tilt_err_vec
|
||||
|
||||
// optical flow processing
|
||||
float _flow_innov[2]; // flow measurement innovation
|
||||
float _flow_innov_var[2]; // flow innovation variance
|
||||
@ -265,9 +266,6 @@ private:
|
||||
// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
|
||||
void fuseHeading();
|
||||
|
||||
// fuse projecton of magnetometer onto horizontal plane
|
||||
void fuseMag2D();
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
void fuseDeclination();
|
||||
|
||||
@ -309,13 +307,14 @@ private:
|
||||
// reset height state of the ekf
|
||||
void resetHeight();
|
||||
|
||||
void makeCovSymetrical();
|
||||
// modify output filter to match the the EKF state at the fusion time horizon
|
||||
void alignOutputFilter();
|
||||
|
||||
// limit the diagonal of the covariance matrix
|
||||
void limitCov();
|
||||
void fixCovarianceErrors();
|
||||
|
||||
// make ekf covariance matrix symmetric
|
||||
void makeSymmetrical();
|
||||
// make ekf covariance matrix symmetric between a nominated state indexe range
|
||||
void makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
|
||||
|
||||
// constrain the ekf states
|
||||
void constrainStates();
|
||||
@ -348,4 +347,7 @@ private:
|
||||
// zero the specified range of columns in the state covariance matrix
|
||||
void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor
|
||||
float calcOptFlowMeasVar();
|
||||
|
||||
};
|
||||
|
||||
@ -108,11 +108,11 @@ void Ekf::resetHeight()
|
||||
_state.pos(2) = new_pos_down;
|
||||
|
||||
// reset the associated covariance values
|
||||
zeroRows(P, 8, 8);
|
||||
zeroCols(P, 8, 8);
|
||||
zeroRows(P, 9, 9);
|
||||
zeroCols(P, 9, 9);
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P[8][8] = sq(_params.range_noise);
|
||||
P[9][9] = sq(_params.range_noise);
|
||||
|
||||
vert_pos_reset = true;
|
||||
|
||||
@ -132,11 +132,11 @@ void Ekf::resetHeight()
|
||||
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
|
||||
|
||||
// reset the associated covariance values
|
||||
zeroRows(P, 8, 8);
|
||||
zeroCols(P, 8, 8);
|
||||
zeroRows(P, 9, 9);
|
||||
zeroCols(P, 9, 9);
|
||||
|
||||
// the state variance is th esame as the observation
|
||||
P[8][8] = sq(_params.baro_noise);
|
||||
// the state variance is the same as the observation
|
||||
P[9][9] = sq(_params.baro_noise);
|
||||
|
||||
vert_pos_reset = true;
|
||||
|
||||
@ -150,11 +150,11 @@ void Ekf::resetHeight()
|
||||
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
|
||||
|
||||
// reset the associated covarince values
|
||||
zeroRows(P, 8, 8);
|
||||
zeroCols(P, 8, 8);
|
||||
zeroRows(P, 9, 9);
|
||||
zeroCols(P, 9, 9);
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P[8][8] = sq(gps_newest.hacc);
|
||||
P[9][9] = sq(gps_newest.hacc);
|
||||
|
||||
vert_pos_reset = true;
|
||||
|
||||
@ -168,8 +168,8 @@ void Ekf::resetHeight()
|
||||
}
|
||||
|
||||
// reset the vertical velocity covariance values
|
||||
zeroRows(P, 5, 5);
|
||||
zeroCols(P, 5, 5);
|
||||
zeroRows(P, 6, 6);
|
||||
zeroCols(P, 6, 6);
|
||||
|
||||
// reset the vertical velocity state
|
||||
if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
|
||||
@ -177,7 +177,7 @@ void Ekf::resetHeight()
|
||||
_state.vel(2) = gps_newest.vel(2);
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P[5][5] = sq(1.5f * gps_newest.sacc);
|
||||
P[6][6] = sq(1.5f * gps_newest.sacc);
|
||||
|
||||
} else {
|
||||
// we don't know what the vertical velocity is, so set it to zero
|
||||
@ -185,7 +185,7 @@ void Ekf::resetHeight()
|
||||
|
||||
// Set the variance to a value large enough to allow the state to converge quickly
|
||||
// that does not destabilise the filter
|
||||
P[5][5] = fminf(sq(_state.vel(2)),100.0f);
|
||||
P[6][6] = 10.0f;
|
||||
|
||||
}
|
||||
vert_vel_reset = true;
|
||||
@ -225,6 +225,31 @@ void Ekf::resetHeight()
|
||||
|
||||
}
|
||||
|
||||
// align output filter states to match EKF states at the fusion time horizon
|
||||
void Ekf::alignOutputFilter()
|
||||
{
|
||||
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
|
||||
Quaternion quat_inv = _state.quat_nominal.inversed();
|
||||
Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv;
|
||||
q_delta.normalize();
|
||||
|
||||
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
|
||||
Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
|
||||
Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
|
||||
|
||||
// loop through the output filter state history and add the deltas
|
||||
outputSample output_states;
|
||||
unsigned output_length = _output_buffer.get_length();
|
||||
for (unsigned i=0; i < output_length; i++) {
|
||||
output_states = _output_buffer.get_from_index(i);
|
||||
output_states.quat_nominal *= q_delta;
|
||||
output_states.quat_nominal.normalize();
|
||||
output_states.vel += vel_delta;
|
||||
output_states.pos += pos_delta;
|
||||
_output_buffer.push_to_index(i,output_states);
|
||||
}
|
||||
}
|
||||
|
||||
// Reset heading and magnetic field states
|
||||
bool Ekf::resetMagHeading(Vector3f &mag_init)
|
||||
{
|
||||
@ -248,11 +273,11 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
|
||||
// we don't change the output attitude to avoid jumps
|
||||
_state.quat_nominal = Quaternion(euler_init);
|
||||
|
||||
// reset the angle error variances because the yaw angle could have changed by a significant amount
|
||||
// reset the quaternion variances because the yaw angle could have changed by a significant amount
|
||||
// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
|
||||
// will grow them again.
|
||||
zeroRows(P, 0, 2);
|
||||
zeroCols(P, 0, 2);
|
||||
zeroRows(P, 0, 3);
|
||||
zeroCols(P, 0, 3);
|
||||
|
||||
// calculate initial earth magnetic field states
|
||||
matrix::Dcm<float> R_to_earth(euler_init);
|
||||
@ -292,21 +317,21 @@ void Ekf::calcMagDeclination()
|
||||
}
|
||||
|
||||
// This function forces the covariance matrix to be symmetric
|
||||
void Ekf::makeSymmetrical()
|
||||
void Ekf::makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
|
||||
{
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned row = first; row <= last; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
float tmp = (P[row][column] + P[column][row]) / 2;
|
||||
P[row][column] = tmp;
|
||||
P[column][row] = tmp;
|
||||
float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2;
|
||||
cov_mat[row][column] = tmp;
|
||||
cov_mat[column][row] = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::constrainStates()
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.ang_error(i) = math::constrain(_state.ang_error(i), -1.0f, 1.0f);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
_state.quat_nominal(i) = math::constrain(_state.quat_nominal(i), -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
@ -322,11 +347,9 @@ void Ekf::constrainStates()
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.gyro_scale(i) = math::constrain(_state.gyro_scale(i), 0.95f, 1.05f);
|
||||
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg);
|
||||
}
|
||||
|
||||
_state.accel_z_bias = math::constrain(_state.accel_z_bias, -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f);
|
||||
}
|
||||
@ -407,28 +430,26 @@ void Ekf::get_gps_check_status(uint16_t *val)
|
||||
// get the state vector at the delayed time horizon
|
||||
void Ekf::get_state_delayed(float *state)
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i] = _state.ang_error(i);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
state[i] = _state.quat_nominal(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 3] = _state.vel(i);
|
||||
state[i + 4] = _state.vel(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 6] = _state.pos(i);
|
||||
state[i + 7] = _state.pos(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 9] = _state.gyro_bias(i);
|
||||
state[i + 10] = _state.gyro_bias(i);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 12] = _state.gyro_scale(i);
|
||||
state[i + 13] = _state.accel_bias(i);
|
||||
}
|
||||
|
||||
state[15] = _state.accel_z_bias;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
state[i + 16] = _state.mag_I(i);
|
||||
}
|
||||
@ -442,6 +463,16 @@ void Ekf::get_state_delayed(float *state)
|
||||
}
|
||||
}
|
||||
|
||||
// get the accelerometer bias
|
||||
void Ekf::get_accel_bias(float bias[3])
|
||||
{
|
||||
float temp[3];
|
||||
temp[0] = _state.accel_bias(0) /_dt_ekf_avg;
|
||||
temp[1] = _state.accel_bias(1) /_dt_ekf_avg;
|
||||
temp[2] = _state.accel_bias(2) /_dt_ekf_avg;
|
||||
memcpy(bias, temp, 3 * sizeof(float));
|
||||
}
|
||||
|
||||
// get the diagonal elements of the covariance matrix
|
||||
void Ekf::get_covariances(float *covariances)
|
||||
{
|
||||
@ -463,8 +494,8 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
|
||||
{
|
||||
// report absolute accuracy taking into account the uncertainty in location of the origin
|
||||
// TODO we a need a way to allow for baro drift error
|
||||
float temp1 = sqrtf(P[6][6] + P[7][7] + sq(_gps_origin_eph));
|
||||
float temp2 = sqrtf(P[8][8] + sq(_gps_origin_epv));
|
||||
float temp1 = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
|
||||
float temp2 = sqrtf(P[9][9] + sq(_gps_origin_epv));
|
||||
memcpy(ekf_eph, &temp1, sizeof(float));
|
||||
memcpy(ekf_epv, &temp2, sizeof(float));
|
||||
|
||||
@ -476,28 +507,27 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
|
||||
// fuse measurement
|
||||
void Ekf::fuse(float *K, float innovation)
|
||||
{
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
_state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation;
|
||||
}
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.ang_error(i) = _state.ang_error(i) - K[i] * innovation;
|
||||
_state.vel(i) = _state.vel(i) - K[i + 4] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.vel(i) = _state.vel(i) - K[i + 3] * innovation;
|
||||
_state.pos(i) = _state.pos(i) - K[i + 7] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.pos(i) = _state.pos(i) - K[i + 6] * innovation;
|
||||
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 10] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation;
|
||||
_state.accel_bias(i) = _state.accel_bias(i) - K[i + 13] * innovation;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation;
|
||||
}
|
||||
|
||||
_state.accel_z_bias -= K[15] * innovation;
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
|
||||
}
|
||||
|
||||
@ -346,7 +346,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
_time_last_airspeed = 0;
|
||||
_time_last_optflow = 0;
|
||||
|
||||
memset(&_fault_status, 0, sizeof(_fault_status));
|
||||
memset(&_fault_status.flags, 0, sizeof(_fault_status.flags));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -209,7 +209,7 @@ public:
|
||||
*val = _mag_declination_to_save_deg;
|
||||
}
|
||||
|
||||
virtual void get_accel_bias(float *bias) = 0;
|
||||
virtual void get_accel_bias(float bias[3]) = 0;
|
||||
|
||||
// get EKF mode status
|
||||
void get_control_mode(uint16_t *val)
|
||||
@ -217,6 +217,12 @@ public:
|
||||
*val = _control_status.value;
|
||||
}
|
||||
|
||||
// get EKF internal fault status
|
||||
void get_filter_fault_status(uint16_t *val)
|
||||
{
|
||||
*val = _fault_status.value;
|
||||
}
|
||||
|
||||
// get GPS check status
|
||||
virtual void get_gps_check_status(uint16_t *val) = 0;
|
||||
|
||||
@ -286,7 +292,7 @@ protected:
|
||||
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
|
||||
uint64_t _time_last_optflow;
|
||||
|
||||
fault_status_t _fault_status;
|
||||
fault_status_u _fault_status;
|
||||
|
||||
// allocate data buffers and intialise interface variables
|
||||
bool initialise_interface(uint64_t timestamp);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -59,23 +59,8 @@ void Ekf::fuseOptFlow()
|
||||
float ve = _state.vel(1);
|
||||
float vd = _state.vel(2);
|
||||
|
||||
// calculate the observation noise variance - scaling noise linearly across flow quality range
|
||||
float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
|
||||
float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
|
||||
|
||||
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
|
||||
float weighting = (255.0f - (float)_params.flow_qual_min);
|
||||
|
||||
if (weighting >= 1.0f) {
|
||||
weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
|
||||
1.0f);
|
||||
|
||||
} else {
|
||||
weighting = 0.0f;
|
||||
}
|
||||
|
||||
// take the weighted average of the observation noie for the best and wort flow quality
|
||||
float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
|
||||
// calculate the optical flow observation variance
|
||||
float R_LOS = calcOptFlowMeasVar();
|
||||
|
||||
float H_LOS[2][24] = {}; // Optical flow observation Jacobians
|
||||
float Kfusion[24][2] = {}; // Optical flow Kalman gains
|
||||
@ -126,92 +111,111 @@ void Ekf::fuseOptFlow()
|
||||
|
||||
// calculate X axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
float t3 = q0 * q0;
|
||||
float t4 = q1 * q1;
|
||||
float t5 = q2 * q2;
|
||||
float t6 = q3 * q3;
|
||||
float t7 = q0 * q2 * 2.0f;
|
||||
float t8 = q1 * q3 * 2.0f;
|
||||
float t9 = q0 * q3 * 2.0f;
|
||||
float t10 = q1 * q2 * 2.0f;
|
||||
float t11 = q0 * q1 * 2.0f;
|
||||
H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f));
|
||||
H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6));
|
||||
H_LOS[0][3] = -t2 * (t9 - t10);
|
||||
H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6);
|
||||
H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and klmna gains
|
||||
t2 = 1.0f / range;
|
||||
t3 = q0 * q1 * 2.0f;
|
||||
t4 = q2 * q3 * 2.0f;
|
||||
t5 = q0 * q0;
|
||||
t6 = q1 * q1;
|
||||
t7 = q2 * q2;
|
||||
t8 = q3 * q3;
|
||||
t9 = q0 * q2 * 2.0f;
|
||||
t10 = q1 * q3 * 2.0f;
|
||||
t11 = q0 * q3 * 2.0f;
|
||||
float t12 = q1 * q2 * 2.0f;
|
||||
float t13 = t11 - t12;
|
||||
float t14 = t3 + t4;
|
||||
float t15 = t5 - t6 - t7 + t8;
|
||||
float t16 = t15 * vd;
|
||||
float t17 = t3 - t4;
|
||||
float t18 = t9 + t10;
|
||||
float t19 = t18 * vn;
|
||||
float t28 = t17 * ve;
|
||||
float t20 = t16 + t19 - t28;
|
||||
float t21 = t5 + t6 - t7 - t8;
|
||||
float t22 = t21 * vn;
|
||||
float t23 = t9 - t10;
|
||||
float t24 = t11 + t12;
|
||||
float t25 = t24 * ve;
|
||||
float t29 = t23 * vd;
|
||||
float t26 = t22 + t25 - t29;
|
||||
float t27 = t5 - t6 + t7 - t8;
|
||||
float t30 = P[0][0] * t2 * t20;
|
||||
float t31 = P[5][3] * t2 * t14;
|
||||
float t32 = P[0][3] * t2 * t20;
|
||||
float t33 = P[4][3] * t2 * t27;
|
||||
float t56 = P[3][3] * t2 * t13;
|
||||
float t57 = P[2][3] * t2 * t26;
|
||||
float t34 = t31 + t32 + t33 - t56 - t57;
|
||||
float t35 = P[5][5] * t2 * t14;
|
||||
float t36 = P[0][5] * t2 * t20;
|
||||
float t37 = P[4][5] * t2 * t27;
|
||||
float t59 = P[3][5] * t2 * t13;
|
||||
float t60 = P[2][5] * t2 * t26;
|
||||
float t38 = t35 + t36 + t37 - t59 - t60;
|
||||
float t39 = t2 * t14 * t38;
|
||||
float t40 = P[5][0] * t2 * t14;
|
||||
float t41 = P[4][0] * t2 * t27;
|
||||
float t61 = P[3][0] * t2 * t13;
|
||||
float t62 = P[2][0] * t2 * t26;
|
||||
float t42 = t30 + t40 + t41 - t61 - t62;
|
||||
float t43 = t2 * t20 * t42;
|
||||
float t44 = P[5][2] * t2 * t14;
|
||||
float t45 = P[0][2] * t2 * t20;
|
||||
float t46 = P[4][2] * t2 * t27;
|
||||
float t55 = P[2][2] * t2 * t26;
|
||||
float t63 = P[3][2] * t2 * t13;
|
||||
float t47 = t44 + t45 + t46 - t55 - t63;
|
||||
float t48 = P[5][4] * t2 * t14;
|
||||
float t49 = P[0][4] * t2 * t20;
|
||||
float t50 = P[4][4] * t2 * t27;
|
||||
float t65 = P[3][4] * t2 * t13;
|
||||
float t66 = P[2][4] * t2 * t26;
|
||||
float t51 = t48 + t49 + t50 - t65 - t66;
|
||||
float t52 = t2 * t27 * t51;
|
||||
float t58 = t2 * t13 * t34;
|
||||
float t64 = t2 * t26 * t47;
|
||||
float t53 = R_LOS + t39 + t43 + t52 - t58 - t64;
|
||||
float t54;
|
||||
H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
|
||||
H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t7 = t6-t29;
|
||||
float t8 = q0*q1*2.0f;
|
||||
float t9 = q2*q3*2.0f;
|
||||
float t10 = t8+t9;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q0*vd*2.0f;
|
||||
float t14 = q2*vn*2.0f;
|
||||
float t28 = q1*ve*2.0f;
|
||||
float t15 = t13+t14-t28;
|
||||
float t16 = q3*vd*2.0f;
|
||||
float t17 = q2*ve*2.0f;
|
||||
float t18 = q1*vn*2.0f;
|
||||
float t19 = t16+t17+t18;
|
||||
float t20 = q3*ve*2.0f;
|
||||
float t21 = q0*vn*2.0f;
|
||||
float t30 = q2*vd*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23-t24+t25-t26;
|
||||
float t31 = P[1][1]*t2*t15;
|
||||
float t32 = P[6][0]*t2*t10;
|
||||
float t33 = P[1][0]*t2*t15;
|
||||
float t34 = P[2][0]*t2*t19;
|
||||
float t35 = P[5][0]*t2*t27;
|
||||
float t79 = P[4][0]*t2*t7;
|
||||
float t80 = P[3][0]*t2*t22;
|
||||
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[6][1]*t2*t10;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[2][1]*t2*t19;
|
||||
float t41 = P[5][1]*t2*t27;
|
||||
float t81 = P[4][1]*t2*t7;
|
||||
float t82 = P[3][1]*t2*t22;
|
||||
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t2*t15*t42;
|
||||
float t44 = P[6][2]*t2*t10;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t15;
|
||||
float t47 = P[2][2]*t2*t19;
|
||||
float t48 = P[5][2]*t2*t27;
|
||||
float t83 = P[4][2]*t2*t7;
|
||||
float t84 = P[3][2]*t2*t22;
|
||||
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
float t50 = t2*t19*t49;
|
||||
float t51 = P[6][3]*t2*t10;
|
||||
float t52 = P[0][3]*t2*t5;
|
||||
float t53 = P[1][3]*t2*t15;
|
||||
float t54 = P[2][3]*t2*t19;
|
||||
float t55 = P[5][3]*t2*t27;
|
||||
float t85 = P[4][3]*t2*t7;
|
||||
float t86 = P[3][3]*t2*t22;
|
||||
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
float t57 = P[6][5]*t2*t10;
|
||||
float t58 = P[0][5]*t2*t5;
|
||||
float t59 = P[1][5]*t2*t15;
|
||||
float t60 = P[2][5]*t2*t19;
|
||||
float t61 = P[5][5]*t2*t27;
|
||||
float t88 = P[4][5]*t2*t7;
|
||||
float t89 = P[3][5]*t2*t22;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[6][4]*t2*t10;
|
||||
float t65 = P[0][4]*t2*t5;
|
||||
float t66 = P[1][4]*t2*t15;
|
||||
float t67 = P[2][4]*t2*t19;
|
||||
float t68 = P[5][4]*t2*t27;
|
||||
float t90 = P[4][4]*t2*t7;
|
||||
float t91 = P[3][4]*t2*t22;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = P[6][6]*t2*t10;
|
||||
float t71 = P[0][6]*t2*t5;
|
||||
float t72 = P[1][6]*t2*t15;
|
||||
float t73 = P[2][6]*t2*t19;
|
||||
float t74 = P[5][6]*t2*t27;
|
||||
float t93 = P[4][6]*t2*t7;
|
||||
float t94 = P[3][6]*t2*t22;
|
||||
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
float t76 = t2*t10*t75;
|
||||
float t87 = t2*t22*t56;
|
||||
float t92 = t2*t7*t69;
|
||||
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
float t78;
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
if (t53 >= R_LOS) {
|
||||
t54 = 1.0f / t53;
|
||||
_flow_innov_var[0] = t53;
|
||||
if (t77 >= R_LOS) {
|
||||
t78 = 1.0f / t77;
|
||||
_flow_innov_var[0] = t77;
|
||||
|
||||
} else {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
@ -220,56 +224,30 @@ void Ekf::fuseOptFlow()
|
||||
}
|
||||
|
||||
// calculate Kalman gains for X-axis observation
|
||||
Kfusion[0][0] = t54 * (t30 - P[0][3] * t2 * (t11 - q1 * q2 * 2.0f) + P[0][5] * t2 * t14 - P[0][2] * t2 * t26 + P[0][4] *
|
||||
t2 * t27);
|
||||
Kfusion[1][0] = t54 * (-P[1][3] * t2 * t13 + P[1][5] * t2 * t14 + P[1][0] * t2 * t20 - P[1][2] * t2 * t26 + P[1][4] * t2
|
||||
* t27);
|
||||
Kfusion[2][0] = t54 * (-t55 - P[2][3] * t2 * t13 + P[2][5] * t2 * t14 + P[2][0] * t2 * t20 + P[2][4] * t2 * t27);
|
||||
Kfusion[3][0] = t54 * (-t56 + P[3][5] * t2 * t14 + P[3][0] * t2 * t20 - P[3][2] * t2 * t26 + P[3][4] * t2 * t27);
|
||||
Kfusion[4][0] = t54 * (t50 - P[4][3] * t2 * t13 + P[4][5] * t2 * t14 + P[4][0] * t2 * t20 - P[4][2] * t2 * t26);
|
||||
Kfusion[5][0] = t54 * (t35 - P[5][3] * t2 * t13 + P[5][0] * t2 * t20 - P[5][2] * t2 * t26 + P[5][4] * t2 * t27);
|
||||
Kfusion[6][0] = t54 * (-P[6][3] * t2 * t13 + P[6][5] * t2 * t14 + P[6][0] * t2 * t20 - P[6][2] * t2 * t26 + P[6][4] * t2
|
||||
* t27);
|
||||
Kfusion[7][0] = t54 * (-P[7][3] * t2 * t13 + P[7][5] * t2 * t14 + P[7][0] * t2 * t20 - P[7][2] * t2 * t26 + P[7][4] * t2
|
||||
* t27);
|
||||
Kfusion[8][0] = t54 * (-P[8][3] * t2 * t13 + P[8][5] * t2 * t14 + P[8][0] * t2 * t20 - P[8][2] * t2 * t26 + P[8][4] * t2
|
||||
* t27);
|
||||
Kfusion[9][0] = t54 * (-P[9][3] * t2 * t13 + P[9][5] * t2 * t14 + P[9][0] * t2 * t20 - P[9][2] * t2 * t26 + P[9][4] * t2
|
||||
* t27);
|
||||
Kfusion[10][0] = t54 * (-P[10][3] * t2 * t13 + P[10][5] * t2 * t14 + P[10][0] * t2 * t20 - P[10][2] * t2 * t26 +
|
||||
P[10][4] * t2 * t27);
|
||||
Kfusion[11][0] = t54 * (-P[11][3] * t2 * t13 + P[11][5] * t2 * t14 + P[11][0] * t2 * t20 - P[11][2] * t2 * t26 +
|
||||
P[11][4] * t2 * t27);
|
||||
Kfusion[12][0] = t54 * (-P[12][3] * t2 * t13 + P[12][5] * t2 * t14 + P[12][0] * t2 * t20 - P[12][2] * t2 * t26 +
|
||||
P[12][4] * t2 * t27);
|
||||
Kfusion[13][0] = t54 * (-P[13][3] * t2 * t13 + P[13][5] * t2 * t14 + P[13][0] * t2 * t20 - P[13][2] * t2 * t26 +
|
||||
P[13][4] * t2 * t27);
|
||||
Kfusion[14][0] = t54 * (-P[14][3] * t2 * t13 + P[14][5] * t2 * t14 + P[14][0] * t2 * t20 - P[14][2] * t2 * t26 +
|
||||
P[14][4] * t2 * t27);
|
||||
Kfusion[15][0] = t54 * (-P[15][3] * t2 * t13 + P[15][5] * t2 * t14 + P[15][0] * t2 * t20 - P[15][2] * t2 * t26 +
|
||||
P[15][4] * t2 * t27);
|
||||
|
||||
if (_control_status.flags.mag_3D) {
|
||||
Kfusion[16][0] = t54 * (-P[16][3] * t2 * t13 + P[16][5] * t2 * t14 + P[16][0] * t2 * t20 - P[16][2] * t2 * t26 +
|
||||
P[16][4] * t2 * t27);
|
||||
Kfusion[17][0] = t54 * (-P[17][3] * t2 * t13 + P[17][5] * t2 * t14 + P[17][0] * t2 * t20 - P[17][2] * t2 * t26 +
|
||||
P[17][4] * t2 * t27);
|
||||
Kfusion[18][0] = t54 * (-P[18][3] * t2 * t13 + P[18][5] * t2 * t14 + P[18][0] * t2 * t20 - P[18][2] * t2 * t26 +
|
||||
P[18][4] * t2 * t27);
|
||||
Kfusion[19][0] = t54 * (-P[19][3] * t2 * t13 + P[19][5] * t2 * t14 + P[19][0] * t2 * t20 - P[19][2] * t2 * t26 +
|
||||
P[19][4] * t2 * t27);
|
||||
Kfusion[20][0] = t54 * (-P[20][3] * t2 * t13 + P[20][5] * t2 * t14 + P[20][0] * t2 * t20 - P[20][2] * t2 * t26 +
|
||||
P[20][4] * t2 * t27);
|
||||
Kfusion[21][0] = t54 * (-P[21][3] * t2 * t13 + P[21][5] * t2 * t14 + P[21][0] * t2 * t20 - P[21][2] * t2 * t26 +
|
||||
P[21][4] * t2 * t27);
|
||||
}
|
||||
|
||||
if (_control_status.flags.wind) {
|
||||
Kfusion[22][0] = t54 * (-P[22][3] * t2 * t13 + P[22][5] * t2 * t14 + P[22][0] * t2 * t20 - P[22][2] * t2 * t26 +
|
||||
P[22][4] * t2 * t27);
|
||||
Kfusion[23][0] = t54 * (-P[23][3] * t2 * t13 + P[23][5] * t2 * t14 + P[23][0] * t2 * t20 - P[23][2] * t2 * t26 +
|
||||
P[23][4] * t2 * t27);
|
||||
}
|
||||
Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
||||
Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
||||
Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
||||
Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
||||
Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
||||
Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
||||
Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
||||
Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
||||
Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
||||
Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
||||
Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
||||
Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
||||
Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
||||
Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
||||
Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
||||
Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
||||
Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
||||
Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
||||
Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
|
||||
// run innovation consistency checks
|
||||
optflow_test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]);
|
||||
@ -278,91 +256,111 @@ void Ekf::fuseOptFlow()
|
||||
|
||||
// calculate Y axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
float t3 = q0 * q0;
|
||||
float t4 = q1 * q1;
|
||||
float t5 = q2 * q2;
|
||||
float t6 = q3 * q3;
|
||||
float t7 = q0 * q1 * 2.0f;
|
||||
float t8 = q0 * q3 * 2.0f;
|
||||
float t9 = q0 * q2 * 2.0f;
|
||||
float t10 = q1 * q3 * 2.0f;
|
||||
H_LOS[1][1] = t2 * (vn * (t9 + t10) + vd * (t3 - t4 - t5 + t6) - ve * (t7 - q2 * q3 * 2.0f));
|
||||
H_LOS[1][2] = -t2 * (ve * (t3 - t4 + t5 - t6) + vd * (t7 + q2 * q3 * 2.0f) - vn * (t8 - q1 * q2 * 2.0f));
|
||||
H_LOS[1][3] = -t2 * (t3 + t4 - t5 - t6);
|
||||
H_LOS[1][4] = -t2 * (t8 + q1 * q2 * 2.0f);
|
||||
H_LOS[1][5] = t2 * (t9 - t10);
|
||||
H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
||||
H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and klmna gains
|
||||
t2 = 1.0f / range;
|
||||
t3 = q0 * q2 * 2.0f;
|
||||
t4 = q0 * q0;
|
||||
t5 = q1 * q1;
|
||||
t6 = q2 * q2;
|
||||
t7 = q3 * q3;
|
||||
t8 = q0 * q1 * 2.0f;
|
||||
t9 = q0 * q3 * 2.0f;
|
||||
t10 = q1 * q2 * 2.0f;
|
||||
float t11 = t9 + t10;
|
||||
float t12 = q1 * q3 * 2.0f;
|
||||
float t13 = t4 - t5 - t6 + t7;
|
||||
float t14 = t13 * vd;
|
||||
float t15 = q2 * q3 * 2.0f;
|
||||
float t16 = t3 + t12;
|
||||
float t17 = t16 * vn;
|
||||
float t18 = t4 - t5 + t6 - t7;
|
||||
float t19 = t18 * ve;
|
||||
float t20 = t8 + t15;
|
||||
float t21 = t20 * vd;
|
||||
float t22 = t9 - t10;
|
||||
float t28 = t22 * vn;
|
||||
float t23 = t19 + t21 - t28;
|
||||
float t24 = t4 + t5 - t6 - t7;
|
||||
float t25 = t3 - t12;
|
||||
float t26 = t8 - t15;
|
||||
float t29 = t26 * ve;
|
||||
float t27 = t14 + t17 - t29;
|
||||
float t30 = P[4][4] * t2 * t11;
|
||||
float t31 = P[2][4] * t2 * t23;
|
||||
float t32 = P[3][4] * t2 * t24;
|
||||
float t56 = P[5][4] * t2 * t25;
|
||||
float t57 = P[1][4] * t2 * t27;
|
||||
float t33 = t30 + t31 + t32 - t56 - t57;
|
||||
float t34 = t2 * t11 * t33;
|
||||
float t35 = P[4][5] * t2 * t11;
|
||||
float t36 = P[2][5] * t2 * t23;
|
||||
float t37 = P[3][5] * t2 * t24;
|
||||
float t58 = P[5][5] * t2 * t25;
|
||||
float t59 = P[1][5] * t2 * t27;
|
||||
float t38 = t35 + t36 + t37 - t58 - t59;
|
||||
float t39 = P[4][1] * t2 * t11;
|
||||
float t40 = P[2][1] * t2 * t23;
|
||||
float t41 = P[3][1] * t2 * t24;
|
||||
float t55 = P[1][1] * t2 * t27;
|
||||
float t61 = P[5][1] * t2 * t25;
|
||||
float t42 = t39 + t40 + t41 - t55 - t61;
|
||||
float t43 = P[4][2] * t2 * t11;
|
||||
float t44 = P[2][2] * t2 * t23;
|
||||
float t45 = P[3][2] * t2 * t24;
|
||||
float t63 = P[5][2] * t2 * t25;
|
||||
float t64 = P[1][2] * t2 * t27;
|
||||
float t46 = t43 + t44 + t45 - t63 - t64;
|
||||
float t47 = t2 * t23 * t46;
|
||||
float t48 = P[4][3] * t2 * t11;
|
||||
float t49 = P[2][3] * t2 * t23;
|
||||
float t50 = P[3][3] * t2 * t24;
|
||||
float t65 = P[5][3] * t2 * t25;
|
||||
float t66 = P[1][3] * t2 * t27;
|
||||
float t51 = t48 + t49 + t50 - t65 - t66;
|
||||
float t52 = t2 * t24 * t51;
|
||||
float t60 = t2 * t25 * t38;
|
||||
float t62 = t2 * t27 * t42;
|
||||
float t53 = R_LOS + t34 + t47 + t52 - t60 - t62;
|
||||
float t54;
|
||||
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
if (t53 >= R_LOS) {
|
||||
t54 = 1.0f / t53;
|
||||
_flow_innov_var[1] = t53;
|
||||
// calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t7 = q1*q2*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q2*2.0f;
|
||||
float t28 = q1*q3*2.0f;
|
||||
float t10 = t9-t28;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q3*vd*2.0f;
|
||||
float t14 = q2*ve*2.0f;
|
||||
float t15 = q1*vn*2.0f;
|
||||
float t16 = t13+t14+t15;
|
||||
float t17 = q0*vd*2.0f;
|
||||
float t18 = q2*vn*2.0f;
|
||||
float t29 = q1*ve*2.0f;
|
||||
float t19 = t17+t18-t29;
|
||||
float t20 = q1*vd*2.0f;
|
||||
float t21 = q0*ve*2.0f;
|
||||
float t30 = q3*vn*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23+t24-t25-t26;
|
||||
float t31 = P[1][1]*t2*t16;
|
||||
float t32 = P[5][0]*t2*t8;
|
||||
float t33 = P[1][0]*t2*t16;
|
||||
float t34 = P[3][0]*t2*t22;
|
||||
float t35 = P[4][0]*t2*t27;
|
||||
float t80 = P[6][0]*t2*t10;
|
||||
float t81 = P[2][0]*t2*t19;
|
||||
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[5][1]*t2*t8;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[3][1]*t2*t22;
|
||||
float t41 = P[4][1]*t2*t27;
|
||||
float t82 = P[6][1]*t2*t10;
|
||||
float t83 = P[2][1]*t2*t19;
|
||||
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
float t43 = t2*t16*t42;
|
||||
float t44 = P[5][2]*t2*t8;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t16;
|
||||
float t47 = P[3][2]*t2*t22;
|
||||
float t48 = P[4][2]*t2*t27;
|
||||
float t79 = P[2][2]*t2*t19;
|
||||
float t84 = P[6][2]*t2*t10;
|
||||
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
float t50 = P[5][3]*t2*t8;
|
||||
float t51 = P[0][3]*t2*t5;
|
||||
float t52 = P[1][3]*t2*t16;
|
||||
float t53 = P[3][3]*t2*t22;
|
||||
float t54 = P[4][3]*t2*t27;
|
||||
float t86 = P[6][3]*t2*t10;
|
||||
float t87 = P[2][3]*t2*t19;
|
||||
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
float t56 = t2*t22*t55;
|
||||
float t57 = P[5][4]*t2*t8;
|
||||
float t58 = P[0][4]*t2*t5;
|
||||
float t59 = P[1][4]*t2*t16;
|
||||
float t60 = P[3][4]*t2*t22;
|
||||
float t61 = P[4][4]*t2*t27;
|
||||
float t88 = P[6][4]*t2*t10;
|
||||
float t89 = P[2][4]*t2*t19;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[5][5]*t2*t8;
|
||||
float t65 = P[0][5]*t2*t5;
|
||||
float t66 = P[1][5]*t2*t16;
|
||||
float t67 = P[3][5]*t2*t22;
|
||||
float t68 = P[4][5]*t2*t27;
|
||||
float t90 = P[6][5]*t2*t10;
|
||||
float t91 = P[2][5]*t2*t19;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = t2*t8*t69;
|
||||
float t71 = P[5][6]*t2*t8;
|
||||
float t72 = P[0][6]*t2*t5;
|
||||
float t73 = P[1][6]*t2*t16;
|
||||
float t74 = P[3][6]*t2*t22;
|
||||
float t75 = P[4][6]*t2*t27;
|
||||
float t92 = P[6][6]*t2*t10;
|
||||
float t93 = P[2][6]*t2*t19;
|
||||
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
float t85 = t2*t19*t49;
|
||||
float t94 = t2*t10*t76;
|
||||
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
float t78;
|
||||
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
||||
if (t77 >= R_LOS) {
|
||||
t78 = 1.0f / t77;
|
||||
_flow_innov_var[1] = t77;
|
||||
|
||||
} else {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
@ -370,56 +368,31 @@ void Ekf::fuseOptFlow()
|
||||
return;
|
||||
}
|
||||
|
||||
// calculate Kalman gains for X-axis observation
|
||||
Kfusion[0][1] = -t54 * (P[0][4] * t2 * t11 + P[0][2] * t2 * t23 + P[0][3] * t2 * t24 - P[0][1] * t2 * t27 - P[0][5] * t2
|
||||
* t25);
|
||||
Kfusion[1][1] = -t54 * (-t55 + P[1][4] * t2 * t11 + P[1][2] * t2 * t23 + P[1][3] * t2 * t24 - P[1][5] * t2 * t25);
|
||||
Kfusion[2][1] = -t54 * (t44 + P[2][4] * t2 * t11 + P[2][3] * t2 * t24 - P[2][1] * t2 * t27 - P[2][5] * t2 * t25);
|
||||
Kfusion[3][1] = -t54 * (t50 + P[3][4] * t2 * t11 + P[3][2] * t2 * t23 - P[3][1] * t2 * t27 - P[3][5] * t2 * t25);
|
||||
Kfusion[4][1] = -t54 * (t30 + P[4][2] * t2 * t23 + P[4][3] * t2 * t24 - P[4][1] * t2 * t27 - P[4][5] * t2 * t25);
|
||||
Kfusion[5][1] = -t54 * (-t58 + P[5][4] * t2 * t11 + P[5][2] * t2 * t23 + P[5][3] * t2 * t24 - P[5][1] * t2 * t27);
|
||||
Kfusion[6][1] = -t54 * (P[6][4] * t2 * t11 + P[6][2] * t2 * t23 + P[6][3] * t2 * t24 - P[6][1] * t2 * t27 - P[6][5] * t2
|
||||
* t25);
|
||||
Kfusion[7][1] = -t54 * (P[7][4] * t2 * t11 + P[7][2] * t2 * t23 + P[7][3] * t2 * t24 - P[7][1] * t2 * t27 - P[7][5] * t2
|
||||
* t25);
|
||||
Kfusion[8][1] = -t54 * (P[8][4] * t2 * t11 + P[8][2] * t2 * t23 + P[8][3] * t2 * t24 - P[8][1] * t2 * t27 - P[8][5] * t2
|
||||
* t25);
|
||||
Kfusion[9][1] = -t54 * (P[9][4] * t2 * t11 + P[9][2] * t2 * t23 + P[9][3] * t2 * t24 - P[9][1] * t2 * t27 - P[9][5] * t2
|
||||
* t25);
|
||||
Kfusion[10][1] = -t54 * (P[10][4] * t2 * t11 + P[10][2] * t2 * t23 + P[10][3] * t2 * t24 - P[10][1] * t2 * t27 -
|
||||
P[10][5] * t2 * t25);
|
||||
Kfusion[11][1] = -t54 * (P[11][4] * t2 * t11 + P[11][2] * t2 * t23 + P[11][3] * t2 * t24 - P[11][1] * t2 * t27 -
|
||||
P[11][5] * t2 * t25);
|
||||
Kfusion[12][1] = -t54 * (P[12][4] * t2 * t11 + P[12][2] * t2 * t23 + P[12][3] * t2 * t24 - P[12][1] * t2 * t27 -
|
||||
P[12][5] * t2 * t25);
|
||||
Kfusion[13][1] = -t54 * (P[13][4] * t2 * t11 + P[13][2] * t2 * t23 + P[13][3] * t2 * t24 - P[13][1] * t2 * t27 -
|
||||
P[13][5] * t2 * t25);
|
||||
Kfusion[14][1] = -t54 * (P[14][4] * t2 * t11 + P[14][2] * t2 * t23 + P[14][3] * t2 * t24 - P[14][1] * t2 * t27 -
|
||||
P[14][5] * t2 * t25);
|
||||
Kfusion[15][1] = -t54 * (P[15][4] * t2 * t11 + P[15][2] * t2 * t23 + P[15][3] * t2 * t24 - P[15][1] * t2 * t27 -
|
||||
P[15][5] * t2 * t25);
|
||||
|
||||
if (_control_status.flags.mag_3D) {
|
||||
Kfusion[16][1] = -t54 * (P[16][4] * t2 * t11 + P[16][2] * t2 * t23 + P[16][3] * t2 * t24 - P[16][1] * t2 * t27 -
|
||||
P[16][5] * t2 * t25);
|
||||
Kfusion[17][1] = -t54 * (P[17][4] * t2 * t11 + P[17][2] * t2 * t23 + P[17][3] * t2 * t24 - P[17][1] * t2 * t27 -
|
||||
P[17][5] * t2 * t25);
|
||||
Kfusion[18][1] = -t54 * (P[18][4] * t2 * t11 + P[18][2] * t2 * t23 + P[18][3] * t2 * t24 - P[18][1] * t2 * t27 -
|
||||
P[18][5] * t2 * t25);
|
||||
Kfusion[19][1] = -t54 * (P[19][4] * t2 * t11 + P[19][2] * t2 * t23 + P[19][3] * t2 * t24 - P[19][1] * t2 * t27 -
|
||||
P[19][5] * t2 * t25);
|
||||
Kfusion[20][1] = -t54 * (P[20][4] * t2 * t11 + P[20][2] * t2 * t23 + P[20][3] * t2 * t24 - P[20][1] * t2 * t27 -
|
||||
P[20][5] * t2 * t25);
|
||||
Kfusion[21][1] = -t54 * (P[21][4] * t2 * t11 + P[21][2] * t2 * t23 + P[21][3] * t2 * t24 - P[21][1] * t2 * t27 -
|
||||
P[21][5] * t2 * t25);
|
||||
}
|
||||
|
||||
if (_control_status.flags.wind) {
|
||||
Kfusion[22][1] = -t54 * (P[22][4] * t2 * t11 + P[22][2] * t2 * t23 + P[22][3] * t2 * t24 - P[22][1] * t2 * t27 -
|
||||
P[22][5] * t2 * t25);
|
||||
Kfusion[23][1] = -t54 * (P[23][4] * t2 * t11 + P[23][2] * t2 * t23 + P[23][3] * t2 * t24 - P[23][1] * t2 * t27 -
|
||||
P[23][5] * t2 * t25);
|
||||
}
|
||||
// calculate Kalman gains for Y-axis observation
|
||||
Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
||||
Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
||||
Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
||||
Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
||||
Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
||||
Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
||||
Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
||||
Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
||||
Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
||||
Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
||||
Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
||||
Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
||||
Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
||||
Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
||||
Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
||||
Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
||||
Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
||||
Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
||||
Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
|
||||
// run innovation consistency check
|
||||
optflow_test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]);
|
||||
@ -436,9 +409,6 @@ void Ekf::fuseOptFlow()
|
||||
|
||||
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
|
||||
|
||||
// by definition our error state is zero at the time of fusion
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// copy the Kalman gain vector for the axis we are fusing
|
||||
float gain[24];
|
||||
|
||||
@ -446,23 +416,11 @@ void Ekf::fuseOptFlow()
|
||||
gain[row] = Kfusion[row][obs_index];
|
||||
}
|
||||
|
||||
// Update the state vector
|
||||
fuse(gain, _flow_innov[obs_index]);
|
||||
|
||||
// correct the quaternion using the attitude error state
|
||||
Quaternion q_correction;
|
||||
q_correction.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = q_correction * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
// reset attitude error to zero after the correction has been applied
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// apply covariance correction via P_new = (I -K*H)*P
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column <= 5; column++) {
|
||||
for (unsigned column = 0; column <= 6; column++) {
|
||||
KH[row][column] = gain[row] * H_LOS[obs_index][column];
|
||||
}
|
||||
}
|
||||
@ -475,20 +433,52 @@ void Ekf::fuseOptFlow()
|
||||
tmp += KH[row][3] * P[3][column];
|
||||
tmp += KH[row][4] * P[4][column];
|
||||
tmp += KH[row][5] * P[5][column];
|
||||
tmp += KH[row][6] * P[6][column];
|
||||
KHP[row][column] = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] -= KHP[row][column];
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
_fault_status.flags.bad_optflow_X = false;
|
||||
_fault_status.flags.bad_optflow_Y = false;
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
if (P[i][i] < KHP[i][i]) {
|
||||
// zero rows and columns
|
||||
zeroRows(P,i,i);
|
||||
zeroCols(P,i,i);
|
||||
|
||||
//flag as unhealthy
|
||||
healthy = false;
|
||||
|
||||
// update individual measurement health status
|
||||
if (obs_index == 0) {
|
||||
_fault_status.flags.bad_optflow_X = true;
|
||||
} else if (obs_index == 1) {
|
||||
_fault_status.flags.bad_optflow_Y = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
_gps_check_fail_status.value = 0;
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
// only apply covariance and state corrrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] = P[row][column] - KHP[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
fuse(gain, _flow_innov[obs_index]);
|
||||
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
_gps_check_fail_status.value = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -543,3 +533,27 @@ void Ekf::calcOptFlowBias()
|
||||
_delta_time_of = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
|
||||
float Ekf::calcOptFlowMeasVar()
|
||||
{
|
||||
// calculate the observation noise variance - scaling noise linearly across flow quality range
|
||||
float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
|
||||
float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
|
||||
|
||||
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
|
||||
float weighting = (255.0f - (float)_params.flow_qual_min);
|
||||
|
||||
if (weighting >= 1.0f) {
|
||||
weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
|
||||
1.0f);
|
||||
|
||||
} else {
|
||||
weighting = 0.0f;
|
||||
}
|
||||
|
||||
// take the weighted average of the observation noie for the best and wort flow quality
|
||||
float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
|
||||
|
||||
return R_LOS;
|
||||
}
|
||||
|
||||
@ -153,7 +153,7 @@ void Ekf::fuseVelPosHeight()
|
||||
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
|
||||
if (fuse_map[obs_index]) {
|
||||
// compute the innovation variance SK = HPH + R
|
||||
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
|
||||
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
|
||||
_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
|
||||
// Compute the ratio of innovation to gate size
|
||||
_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) *
|
||||
@ -163,26 +163,25 @@ void Ekf::fuseVelPosHeight()
|
||||
|
||||
// check position, velocity and height innovations
|
||||
// treat 3D velocity, 2D position and height as separate sensors
|
||||
// always pass position checks if using synthetic position measurements
|
||||
// always pass position checks if using synthetic position measurements or yet to complete tilt alignment
|
||||
// always pass height checks if yet to complete tilt alignment
|
||||
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
|
||||
&& (_vel_pos_test_ratio[2] <= 1.0f);
|
||||
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
|
||||
bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
|
||||
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f))
|
||||
|| using_synthetic_measurements;
|
||||
|| using_synthetic_measurements || !_control_status.flags.tilt_align;
|
||||
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
|
||||
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
|
||||
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
|
||||
|
||||
// record the successful velocity fusion time
|
||||
if (vel_check_pass && _fuse_hor_vel) {
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
_tilt_err_vec.setZero();
|
||||
}
|
||||
|
||||
// record the successful position fusion time
|
||||
if (pos_check_pass && _fuse_pos) {
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_tilt_err_vec.setZero();
|
||||
}
|
||||
|
||||
// record the successful height fusion time
|
||||
@ -196,55 +195,13 @@ void Ekf::fuseVelPosHeight()
|
||||
continue;
|
||||
}
|
||||
|
||||
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
|
||||
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
|
||||
|
||||
// calculate kalman gain K = PHS, where S = 1/innovation variance
|
||||
for (int row = 0; row <= 15; row++) {
|
||||
for (int row = 0; row < _k_num_states; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
|
||||
// only update magnetic field states if we are fusing 3-axis observations
|
||||
if (_control_status.flags.mag_3D) {
|
||||
for (int row = 16; row <= 21; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
|
||||
} else {
|
||||
for (int row = 16; row <= 21; row++) {
|
||||
Kfusion[row] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// only update wind states if we are doing wind estimation
|
||||
if (_control_status.flags.wind) {
|
||||
for (int row = 22; row <= 23; row++) {
|
||||
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
|
||||
}
|
||||
|
||||
} else {
|
||||
for (int row = 22; row <= 23; row++) {
|
||||
Kfusion[row] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// sum the attitude error from velocity and position fusion only
|
||||
// used as a metric for convergence monitoring
|
||||
if (obs_index != 5) {
|
||||
_tilt_err_vec += _state.ang_error;
|
||||
}
|
||||
|
||||
// by definition the angle error state is zero at the fusion time
|
||||
_state.ang_error.setZero();
|
||||
|
||||
// fuse the observation
|
||||
fuse(Kfusion, _vel_pos_innov[obs_index]);
|
||||
|
||||
// correct the nominal quaternion
|
||||
Quaternion dq;
|
||||
dq.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = dq * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
|
||||
// update covarinace matrix via Pnew = (I - KH)P
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
@ -252,13 +209,64 @@ void Ekf::fuseVelPosHeight()
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] = P[row][column] - KHP[row][column];
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance marix is unhealthy and must be corrected
|
||||
bool healthy = true;
|
||||
for (int i = 0; i < _k_num_states; i++) {
|
||||
if (P[i][i] < KHP[i][i]) {
|
||||
// zero rows and columns
|
||||
zeroRows(P,i,i);
|
||||
zeroCols(P,i,i);
|
||||
|
||||
//flag as unhealthy
|
||||
healthy = false;
|
||||
|
||||
// update individual measurement health status
|
||||
if (obs_index == 0) {
|
||||
_fault_status.flags.bad_vel_N = true;
|
||||
} else if (obs_index == 1) {
|
||||
_fault_status.flags.bad_vel_E = true;
|
||||
} else if (obs_index == 2) {
|
||||
_fault_status.flags.bad_vel_D = true;
|
||||
} else if (obs_index == 3) {
|
||||
_fault_status.flags.bad_pos_N = true;
|
||||
} else if (obs_index == 4) {
|
||||
_fault_status.flags.bad_pos_E = true;
|
||||
} else if (obs_index == 5) {
|
||||
_fault_status.flags.bad_pos_D = true;
|
||||
}
|
||||
} else {
|
||||
// update individual measurement health status
|
||||
if (obs_index == 0) {
|
||||
_fault_status.flags.bad_vel_N = false;
|
||||
} else if (obs_index == 1) {
|
||||
_fault_status.flags.bad_vel_E = false;
|
||||
} else if (obs_index == 2) {
|
||||
_fault_status.flags.bad_vel_D = false;
|
||||
} else if (obs_index == 3) {
|
||||
_fault_status.flags.bad_pos_N = false;
|
||||
} else if (obs_index == 4) {
|
||||
_fault_status.flags.bad_pos_E = false;
|
||||
} else if (obs_index == 5) {
|
||||
_fault_status.flags.bad_pos_D = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
// only apply covariance and state corrrections if healthy
|
||||
if (healthy) {
|
||||
// apply the covariance corrections
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] = P[row][column] - KHP[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
// correct the covariance marix for gross errors
|
||||
fixCovarianceErrors();
|
||||
|
||||
// apply the state corrections
|
||||
fuse(Kfusion, _vel_pos_innov[obs_index]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -4,15 +4,15 @@
|
||||
|
||||
// intermediate variable from algebraic optimisation
|
||||
float SH_TAS[3];
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
|
||||
SH_TAS[0] = 1.0f/v_tas_pred;
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
|
||||
|
||||
// observation jacobian
|
||||
float H_TAS[24];
|
||||
H_TAS[3] = SH_TAS[2];
|
||||
H_TAS[4] = SH_TAS[1];
|
||||
H_TAS[5] = vd*SH_TAS[0];
|
||||
H_TAS[4] = SH_TAS[2];
|
||||
H_TAS[5] = SH_TAS[1];
|
||||
H_TAS[6] = vd*SH_TAS[0];
|
||||
H_TAS[22] = -SH_TAS[2];
|
||||
H_TAS[23] = -SH_TAS[1];
|
||||
|
||||
@ -20,31 +20,31 @@ H_TAS[23] = -SH_TAS[1];
|
||||
|
||||
// intermediate variables - note SK_TAS[0] is 1/(innovation variance)
|
||||
float SK_TAS[2];
|
||||
SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]));
|
||||
SK_TAS[0] = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
|
||||
SK_TAS[1] = SH_TAS[1];
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_TAS[0]*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]);
|
||||
Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]);
|
||||
Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]);
|
||||
Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]);
|
||||
Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]);
|
||||
Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]);
|
||||
Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]);
|
||||
Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]);
|
||||
Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]);
|
||||
Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]);
|
||||
Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]);
|
||||
Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]);
|
||||
Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]);
|
||||
Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]);
|
||||
Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]);
|
||||
Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]);
|
||||
Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]);
|
||||
Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]);
|
||||
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
|
||||
Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
|
||||
Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
|
||||
Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
|
||||
Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
|
||||
Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
|
||||
Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
|
||||
Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
|
||||
Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
|
||||
Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
|
||||
Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
|
||||
Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
|
||||
Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
|
||||
Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
|
||||
Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
|
||||
Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]);
|
||||
Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
|
||||
|
||||
@ -1,228 +1,216 @@
|
||||
// Auto code for covariance prediction
|
||||
|
||||
// Intermediate expressions obtained from algebraic optimisation
|
||||
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 SF[21] = {};
|
||||
SF[0] = dvz - dvz_b;
|
||||
SF[1] = dvy - dvy_b;
|
||||
SF[2] = dvx - dvx_b;
|
||||
SF[3] = 2.0f*q1*SF[2] + 2.0f*q2*SF[1] + 2.0f*q3*SF[0];
|
||||
SF[4] = 2.0f*q0*SF[1] - 2.0f*q1*SF[0] + 2.0f*q3*SF[2];
|
||||
SF[5] = 2.0f*q0*SF[2] + 2.0f*q2*SF[0] - 2.0f*q3*SF[1];
|
||||
SF[6] = day*0.5f - day_b*0.5f;
|
||||
SF[7] = daz*0.5f - daz_b*0.5f;
|
||||
SF[8] = dax*0.5f - dax_b*0.5f;
|
||||
SF[9] = dax_b*0.5f - dax*0.5f;
|
||||
SF[10] = daz_b*0.5f - daz*0.5f;
|
||||
SF[11] = day_b*0.5f - day*0.5f;
|
||||
SF[12] = 2.0f*q1*SF[1];
|
||||
SF[13] = 2.0f*q0*SF[0];
|
||||
SF[14] = q1*0.5f;
|
||||
SF[15] = q2*0.5f;
|
||||
SF[16] = q3*0.5f;
|
||||
SF[17] = sq(q3);
|
||||
SF[18] = sq(q2);
|
||||
SF[19] = sq(q1);
|
||||
SF[20] = sq(q0);
|
||||
|
||||
float SG[5];
|
||||
SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
|
||||
float SG[8] = {};
|
||||
SG[0] = q0*0.5f;
|
||||
SG[1] = sq(q3);
|
||||
SG[2] = sq(q2);
|
||||
SG[3] = sq(q1);
|
||||
SG[4] = sq(q0);
|
||||
SG[5] = 2.0f*q2*q3;
|
||||
SG[6] = 2.0f*q1*q3;
|
||||
SG[7] = 2.0f*q1*q2;
|
||||
|
||||
float SQ[10];
|
||||
SQ[0] = - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
|
||||
SQ[1] = sq(dvxNoise)*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + sq(dvzNoise)*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - sq(dvyNoise)*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
|
||||
SQ[2] = sq(dvyNoise)*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - sq(dvxNoise)*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - sq(dvzNoise)*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
|
||||
SQ[3] = sq(SG[0]);
|
||||
SQ[4] = sq(dvyNoise);
|
||||
SQ[5] = sq(dvzNoise);
|
||||
SQ[6] = sq(dvxNoise);
|
||||
SQ[7] = 2*q2*q3;
|
||||
SQ[8] = 2*q1*q3;
|
||||
SQ[9] = 2*q1*q2;
|
||||
float SQ[11] = {};
|
||||
SQ[0] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[7] + 2.0f*q0*q3);
|
||||
SQ[1] = dvzVar*(SG[6] + 2.0f*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[7] - 2.0f*q0*q3);
|
||||
SQ[2] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[6] + 2.0f*q0*q2) - dvyVar*(SG[7] - 2.0f*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2.0f*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f;
|
||||
SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f;
|
||||
SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f;
|
||||
SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f;
|
||||
SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f;
|
||||
SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f;
|
||||
SQ[9] = sq(SG[0]);
|
||||
SQ[10] = sq(q1);
|
||||
|
||||
float SPP[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];
|
||||
float SPP[11] = {};
|
||||
SPP[0] = SF[12] + SF[13] - 2.0f*q2*SF[2];
|
||||
SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
|
||||
SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
|
||||
SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
|
||||
SPP[4] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SPP[5] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||
SPP[6] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SPP[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
SPP[8] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
SPP[10] = SF[16];
|
||||
|
||||
// Calculate uppder diagonal elements of the predicted covariance matrix
|
||||
// Use symmetry to assign value to lower diagonal
|
||||
// Note: this matrix does not include the process noise for stationary states, it only includes the effect of noise on the inertial measurements.
|
||||
// Process noise for stationary states must be added later.
|
||||
float nextP[24][24];
|
||||
nextP[0][0] = 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]) + sq(daxNoise)*SQ[3];
|
||||
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] = 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[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[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]) + sq(dayNoise)*SQ[3];
|
||||
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] = 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[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[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]) + sq(dazNoise)*SQ[3];
|
||||
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] + SQ[5]*sq(SQ[8] + 2*q0*q2) + SQ[4]*sq(SQ[9] - 2*q0*q3) + 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]) + SQ[6]*sq(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
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] + SQ[5]*sq(SQ[7] - 2*q0*q1) + SQ[6]*sq(SQ[9] + 2*q0*q3) + 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]) + SQ[4]*sq(SG[1] - SG[2] + SG[3] - SG[4]);
|
||||
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] + SQ[4]*sq(SQ[7] + 2*q0*q1) + SQ[6]*sq(SQ[8] - 2*q0*q2) + 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]) + SQ[5]*sq(SG[1] - SG[2] - SG[3] + SG[4]);
|
||||
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[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f;
|
||||
nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f;
|
||||
nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f;
|
||||
nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))*0.5f;
|
||||
nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)*0.5f + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))*0.5f;
|
||||
nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P[11][2]*q0)*0.5f + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))*0.25f - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))*0.5f;
|
||||
nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))*0.5f;
|
||||
nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)*0.5f + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))*0.5f;
|
||||
nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)*0.5f + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))*0.5f;
|
||||
nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P[12][3]*q0)*0.5f + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))*0.25f - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))*0.5f;
|
||||
nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
|
||||
nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)*0.5f + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
|
||||
nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)*0.5f + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
|
||||
nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)*0.5f + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
|
||||
nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2.0f*q0*q3) + dvzVar*sq(SG[6] + 2.0f*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
|
||||
nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
|
||||
nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)*0.5f + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
|
||||
nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)*0.5f + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
|
||||
nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)*0.5f + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
|
||||
nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
|
||||
nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2.0f*q0*q3) + dvzVar*sq(SG[5] - 2.0f*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
|
||||
nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
|
||||
nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)*0.5f + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
|
||||
nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)*0.5f + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
|
||||
nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)*0.5f + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
|
||||
nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
|
||||
nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]);
|
||||
nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2.0f*q0*q2) + dvyVar*sq(SG[5] + 2.0f*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
|
||||
nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]);
|
||||
nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)*0.5f + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2);
|
||||
nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)*0.5f + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2);
|
||||
nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)*0.5f + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2);
|
||||
nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]);
|
||||
nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]);
|
||||
nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]);
|
||||
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
|
||||
nextP[0][8] = P[0][8]*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[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]);
|
||||
nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)*0.5f + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2);
|
||||
nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)*0.5f + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2);
|
||||
nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)*0.5f + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2);
|
||||
nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]);
|
||||
nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]);
|
||||
nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]);
|
||||
nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
|
||||
nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
|
||||
nextP[0][9] = P[0][9]*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[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]);
|
||||
nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)*0.5f + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2);
|
||||
nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)*0.5f + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2);
|
||||
nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)*0.5f + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2);
|
||||
nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]);
|
||||
nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]);
|
||||
nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]);
|
||||
nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
|
||||
nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
|
||||
nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
|
||||
nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10];
|
||||
nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f;
|
||||
nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f;
|
||||
nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)*0.5f;
|
||||
nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9];
|
||||
nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5];
|
||||
nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1];
|
||||
nextP[7][10] = P[7][10] + P[4][10]*dt;
|
||||
nextP[8][10] = P[8][10] + P[5][10]*dt;
|
||||
nextP[9][10] = P[9][10];
|
||||
nextP[9][10] = P[9][10] + P[6][10]*dt;
|
||||
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[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10];
|
||||
nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)*0.5f;
|
||||
nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)*0.5f;
|
||||
nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)*0.5f;
|
||||
nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9];
|
||||
nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5];
|
||||
nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1];
|
||||
nextP[7][11] = P[7][11] + P[4][11]*dt;
|
||||
nextP[8][11] = P[8][11] + P[5][11]*dt;
|
||||
nextP[9][11] = P[9][11];
|
||||
nextP[9][11] = P[9][11] + P[6][11]*dt;
|
||||
nextP[10][11] = P[10][11];
|
||||
nextP[11][11] = P[11][11];
|
||||
nextP[0][12] = P[0][12]*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[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10];
|
||||
nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)*0.5f;
|
||||
nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)*0.5f;
|
||||
nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)*0.5f;
|
||||
nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9];
|
||||
nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5];
|
||||
nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1];
|
||||
nextP[7][12] = P[7][12] + P[4][12]*dt;
|
||||
nextP[8][12] = P[8][12] + P[5][12]*dt;
|
||||
nextP[9][12] = P[9][12];
|
||||
nextP[9][12] = P[9][12] + P[6][12]*dt;
|
||||
nextP[10][12] = P[10][12];
|
||||
nextP[11][12] = P[11][12];
|
||||
nextP[12][12] = P[12][12];
|
||||
nextP[0][13] = P[0][13]*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[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10];
|
||||
nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f;
|
||||
nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f;
|
||||
nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)*0.5f;
|
||||
nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9];
|
||||
nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5];
|
||||
nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1];
|
||||
nextP[7][13] = P[7][13] + P[4][13]*dt;
|
||||
nextP[8][13] = P[8][13] + P[5][13]*dt;
|
||||
nextP[9][13] = P[9][13];
|
||||
nextP[9][13] = P[9][13] + P[6][13]*dt;
|
||||
nextP[10][13] = P[10][13];
|
||||
nextP[11][13] = P[11][13];
|
||||
nextP[12][13] = P[12][13];
|
||||
nextP[13][13] = P[13][13];
|
||||
nextP[0][14] = P[0][14]*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[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10];
|
||||
nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)*0.5f;
|
||||
nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)*0.5f;
|
||||
nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)*0.5f;
|
||||
nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9];
|
||||
nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5];
|
||||
nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1];
|
||||
nextP[7][14] = P[7][14] + P[4][14]*dt;
|
||||
nextP[8][14] = P[8][14] + P[5][14]*dt;
|
||||
nextP[9][14] = P[9][14];
|
||||
nextP[9][14] = P[9][14] + P[6][14]*dt;
|
||||
nextP[10][14] = P[10][14];
|
||||
nextP[11][14] = P[11][14];
|
||||
nextP[12][14] = P[12][14];
|
||||
nextP[13][14] = P[13][14];
|
||||
nextP[14][14] = P[14][14];
|
||||
nextP[0][15] = P[0][15]*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[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10];
|
||||
nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f;
|
||||
nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f;
|
||||
nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)*0.5f;
|
||||
nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9];
|
||||
nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5];
|
||||
nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1];
|
||||
nextP[7][15] = P[7][15] + P[4][15]*dt;
|
||||
nextP[8][15] = P[8][15] + P[5][15]*dt;
|
||||
nextP[9][15] = P[9][15];
|
||||
nextP[9][15] = P[9][15] + P[6][15]*dt;
|
||||
nextP[10][15] = P[10][15];
|
||||
nextP[11][15] = P[11][15];
|
||||
nextP[12][15] = P[12][15];
|
||||
nextP[13][15] = P[13][15];
|
||||
nextP[14][15] = P[14][15];
|
||||
nextP[15][15] = P[15][15];
|
||||
nextP[0][16] = P[0][16]*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[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10];
|
||||
nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)*0.5f;
|
||||
nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)*0.5f;
|
||||
nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)*0.5f;
|
||||
nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9];
|
||||
nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5];
|
||||
nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1];
|
||||
nextP[7][16] = P[7][16] + P[4][16]*dt;
|
||||
nextP[8][16] = P[8][16] + P[5][16]*dt;
|
||||
nextP[9][16] = P[9][16];
|
||||
nextP[9][16] = P[9][16] + P[6][16]*dt;
|
||||
nextP[10][16] = P[10][16];
|
||||
nextP[11][16] = P[11][16];
|
||||
nextP[12][16] = P[12][16];
|
||||
@ -230,16 +218,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[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10];
|
||||
nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)*0.5f;
|
||||
nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)*0.5f;
|
||||
nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)*0.5f;
|
||||
nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9];
|
||||
nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5];
|
||||
nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1];
|
||||
nextP[7][17] = P[7][17] + P[4][17]*dt;
|
||||
nextP[8][17] = P[8][17] + P[5][17]*dt;
|
||||
nextP[9][17] = P[9][17];
|
||||
nextP[9][17] = P[9][17] + P[6][17]*dt;
|
||||
nextP[10][17] = P[10][17];
|
||||
nextP[11][17] = P[11][17];
|
||||
nextP[12][17] = P[12][17];
|
||||
@ -248,16 +236,16 @@ 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[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10];
|
||||
nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)*0.5f;
|
||||
nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)*0.5f;
|
||||
nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)*0.5f;
|
||||
nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9];
|
||||
nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5];
|
||||
nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1];
|
||||
nextP[7][18] = P[7][18] + P[4][18]*dt;
|
||||
nextP[8][18] = P[8][18] + P[5][18]*dt;
|
||||
nextP[9][18] = P[9][18];
|
||||
nextP[9][18] = P[9][18] + P[6][18]*dt;
|
||||
nextP[10][18] = P[10][18];
|
||||
nextP[11][18] = P[11][18];
|
||||
nextP[12][18] = P[12][18];
|
||||
@ -267,16 +255,16 @@ 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[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10];
|
||||
nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)*0.5f;
|
||||
nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)*0.5f;
|
||||
nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)*0.5f;
|
||||
nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9];
|
||||
nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5];
|
||||
nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1];
|
||||
nextP[7][19] = P[7][19] + P[4][19]*dt;
|
||||
nextP[8][19] = P[8][19] + P[5][19]*dt;
|
||||
nextP[9][19] = P[9][19];
|
||||
nextP[9][19] = P[9][19] + P[6][19]*dt;
|
||||
nextP[10][19] = P[10][19];
|
||||
nextP[11][19] = P[11][19];
|
||||
nextP[12][19] = P[12][19];
|
||||
@ -287,16 +275,16 @@ 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[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10];
|
||||
nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)*0.5f;
|
||||
nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)*0.5f;
|
||||
nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)*0.5f;
|
||||
nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9];
|
||||
nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5];
|
||||
nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1];
|
||||
nextP[7][20] = P[7][20] + P[4][20]*dt;
|
||||
nextP[8][20] = P[8][20] + P[5][20]*dt;
|
||||
nextP[9][20] = P[9][20];
|
||||
nextP[9][20] = P[9][20] + P[6][20]*dt;
|
||||
nextP[10][20] = P[10][20];
|
||||
nextP[11][20] = P[11][20];
|
||||
nextP[12][20] = P[12][20];
|
||||
@ -308,16 +296,16 @@ 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[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10];
|
||||
nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)*0.5f;
|
||||
nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)*0.5f;
|
||||
nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)*0.5f;
|
||||
nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9];
|
||||
nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5];
|
||||
nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1];
|
||||
nextP[7][21] = P[7][21] + P[4][21]*dt;
|
||||
nextP[8][21] = P[8][21] + P[5][21]*dt;
|
||||
nextP[9][21] = P[9][21];
|
||||
nextP[9][21] = P[9][21] + P[6][21]*dt;
|
||||
nextP[10][21] = P[10][21];
|
||||
nextP[11][21] = P[11][21];
|
||||
nextP[12][21] = P[12][21];
|
||||
@ -330,16 +318,16 @@ 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[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10];
|
||||
nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)*0.5f;
|
||||
nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)*0.5f;
|
||||
nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)*0.5f;
|
||||
nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9];
|
||||
nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5];
|
||||
nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1];
|
||||
nextP[7][22] = P[7][22] + P[4][22]*dt;
|
||||
nextP[8][22] = P[8][22] + P[5][22]*dt;
|
||||
nextP[9][22] = P[9][22];
|
||||
nextP[9][22] = P[9][22] + P[6][22]*dt;
|
||||
nextP[10][22] = P[10][22];
|
||||
nextP[11][22] = P[11][22];
|
||||
nextP[12][22] = P[12][22];
|
||||
@ -353,16 +341,16 @@ 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[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10];
|
||||
nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)*0.5f;
|
||||
nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)*0.5f;
|
||||
nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)*0.5f;
|
||||
nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9];
|
||||
nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5];
|
||||
nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1];
|
||||
nextP[7][23] = P[7][23] + P[4][23]*dt;
|
||||
nextP[8][23] = P[8][23] + P[5][23]*dt;
|
||||
nextP[9][23] = P[9][23];
|
||||
nextP[9][23] = P[9][23] + P[6][23]*dt;
|
||||
nextP[10][23] = P[10][23];
|
||||
nextP[11][23] = P[11][23];
|
||||
nextP[12][23] = P[12][23];
|
||||
|
||||
@ -17,48 +17,51 @@ Divide by zero protection and protection against a badly conditioned covariance
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
float t5 = 1.0f/t4;
|
||||
float t22 = magE*t5;
|
||||
float t23 = magN*t5;
|
||||
float t6 = P[16][16]*t22;
|
||||
float t13 = P[17][16]*t23;
|
||||
float t7 = t6-t13;
|
||||
float t8 = t22*t7;
|
||||
float t9 = P[16][17]*t22;
|
||||
float t14 = P[17][17]*t23;
|
||||
float t10 = t9-t14;
|
||||
float t15 = t23*t10;
|
||||
float t11 = R_DECL+t8-t15; // innovation variance
|
||||
float t12 = 1.0f/t11;
|
||||
float t5 = P[16][16]*t2;
|
||||
float t6 = P[17][17]*t3;
|
||||
float t7 = t2*t2;
|
||||
float t8 = R_DECL*t7;
|
||||
float t9 = t3*t3;
|
||||
float t10 = R_DECL*t9;
|
||||
float t11 = R_DECL*t2*t3*2.0f;
|
||||
float t14 = P[16][17]*magE*magN;
|
||||
float t15 = P[17][16]*magE*magN;
|
||||
float t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
float t13 = 1.0f / t12;
|
||||
float t16 = magE;
|
||||
float t17 = magN;
|
||||
float t18 = t16*t16;
|
||||
float t19 = t17*t17;
|
||||
float t20 = t18+t19;
|
||||
float t21 = 1.0f/t20;
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
H_DECL[16] = -magE*t5;
|
||||
H_DECL[17] = magN*t5;
|
||||
H_DECL[16] = -t16*t21;
|
||||
H_DECL[17] = t17*t21;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23);
|
||||
Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23);
|
||||
Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23);
|
||||
Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23);
|
||||
Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23);
|
||||
Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23);
|
||||
Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23);
|
||||
Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23);
|
||||
Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23);
|
||||
Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23);
|
||||
Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23);
|
||||
Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23);
|
||||
Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23);
|
||||
Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23);
|
||||
Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23);
|
||||
Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23);
|
||||
Kfusion[16] = -t12*(t6-P[16][17]*t23);
|
||||
Kfusion[17] = t12*(t14-P[17][16]*t22);
|
||||
Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23);
|
||||
Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23);
|
||||
Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23);
|
||||
Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23);
|
||||
Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23);
|
||||
Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23);
|
||||
|
||||
Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN);
|
||||
Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN);
|
||||
Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN);
|
||||
Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN);
|
||||
Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN);
|
||||
Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN);
|
||||
Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN);
|
||||
Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN);
|
||||
Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN);
|
||||
Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN);
|
||||
Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN);
|
||||
Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
|
||||
Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
|
||||
Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN);
|
||||
Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN);
|
||||
Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN);
|
||||
Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN);
|
||||
Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN);
|
||||
Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN);
|
||||
Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN);
|
||||
Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
|
||||
Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
|
||||
Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
|
||||
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
|
||||
|
||||
@ -1,146 +1,142 @@
|
||||
// Auto code for fusion of XYZ magnetometer vector measurement
|
||||
// Sequential fusion of each axis is used (assumes uncorrrelated observation errors)
|
||||
|
||||
// intermediate variables from algebraic optimisation
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_MAG[3] = 2*q0*q1 + 2*q2*q3;
|
||||
SH_MAG[4] = 2*q0*q3 + 2*q1*q2;
|
||||
SH_MAG[5] = 2*q0*q2 + 2*q1*q3;
|
||||
SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3);
|
||||
SH_MAG[7] = 2*q1*q3 - 2*q0*q2;
|
||||
SH_MAG[8] = 2*q0*q3;
|
||||
// common expressions for XYZ axis observation jacobians
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
|
||||
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2.0f*magN*q0;
|
||||
SH_MAG[8] = 2.0f*magE*q3;
|
||||
|
||||
// Calculate the observation jacobian for the X component
|
||||
float H_MAG[24];
|
||||
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
|
||||
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||||
H_MAG[16] = SH_MAG[1];
|
||||
H_MAG[17] = SH_MAG[4];
|
||||
H_MAG[18] = SH_MAG[7];
|
||||
H_MAG[19] = 1;
|
||||
// X axis observation jacobians
|
||||
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG[1] = SH_MAG[0];
|
||||
H_MAG[2] = -SH_MAG[1];
|
||||
H_MAG[3] = SH_MAG[2];
|
||||
H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||||
H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2;
|
||||
H_MAG[19] = 1.0f;
|
||||
|
||||
// calculate the Kalman gain matrix for the X component
|
||||
// common expressions for X axis Kalman gains
|
||||
SK_MX[0] = 1.0f/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
|
||||
// intermediate variables - note SK_MX[0] is 1/(innovation variance)
|
||||
float SK_MX[4];
|
||||
SK_MX[0] = 1/(P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))));
|
||||
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||||
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||
SK_MX[3] = SH_MAG[7];
|
||||
// X axis Kalman gains
|
||||
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]);
|
||||
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]);
|
||||
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]);
|
||||
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]);
|
||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]);
|
||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]);
|
||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]);
|
||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]);
|
||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]);
|
||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]);
|
||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]);
|
||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]);
|
||||
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]);
|
||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]);
|
||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]);
|
||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]);
|
||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]);
|
||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]);
|
||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]);
|
||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
|
||||
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
|
||||
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
|
||||
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
|
||||
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
|
||||
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
|
||||
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
|
||||
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
|
||||
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
|
||||
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
|
||||
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
|
||||
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
|
||||
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
|
||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
|
||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
|
||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
|
||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
|
||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
|
||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
|
||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
|
||||
|
||||
// Calculate the observation jacobian for the Y component
|
||||
float H_MAG[24];
|
||||
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
|
||||
H_MAG[16] = 2*q1*q2 - SH_MAG[8];
|
||||
H_MAG[17] = SH_MAG[0];
|
||||
H_MAG[18] = SH_MAG[3];
|
||||
// Y axis observation Jacobians
|
||||
H_MAG[0] = SH_MAG[2];
|
||||
H_MAG[1] = SH_MAG[1];
|
||||
H_MAG[2] = SH_MAG[0];
|
||||
H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||||
H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3;
|
||||
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
H_MAG[20] = 1;
|
||||
|
||||
// calculate the Kalman gain matrix for the Y component
|
||||
// Common expressions for Y axis Kalamn gains
|
||||
SK_MY[0] = 1.0f/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
|
||||
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
|
||||
// intermediate variables - note SK_MY[0] is 1/(innovation variance)
|
||||
float SK_MY[4];
|
||||
SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2)));
|
||||
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
|
||||
SK_MY[3] = SH_MAG[8] - 2*q1*q2;
|
||||
// Y axis Kalmna gains
|
||||
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
|
||||
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
|
||||
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
|
||||
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
|
||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
|
||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
|
||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
|
||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
|
||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
|
||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
|
||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
|
||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
|
||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
|
||||
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
|
||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
|
||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
|
||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
|
||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
|
||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
|
||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
|
||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
|
||||
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
|
||||
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
|
||||
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
|
||||
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
|
||||
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
|
||||
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
|
||||
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
|
||||
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
|
||||
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
|
||||
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
|
||||
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
|
||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
|
||||
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
|
||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
|
||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
|
||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
|
||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
|
||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
|
||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
|
||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
|
||||
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
|
||||
|
||||
// Calculate the observation jacobian for the Z component
|
||||
float H_MAG[24];
|
||||
H_MAG[0] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
|
||||
H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||
H_MAG[16] = SH_MAG[5];
|
||||
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
|
||||
H_MAG[18] = SH_MAG[2];
|
||||
// Z axis observation Jacobians
|
||||
H_MAG[0] = SH_MAG[1];
|
||||
H_MAG[1] = -SH_MAG[2];
|
||||
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG[3] = SH_MAG[0];
|
||||
H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
|
||||
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG[21] = 1;
|
||||
|
||||
// calculate the Kalman gain matrix for the Z component
|
||||
// Common expressions for Z axis Kalman gains
|
||||
SK_MZ[0] = 1.0f/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||
SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
|
||||
// intermediate variables - note SK_MZ[0] is 1/(innovation variance)
|
||||
float SK_MZ[4][1];
|
||||
SK_MZ[0] = 1/(P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2*q0*q1 - 2*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2*q0*q1 - 2*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2*q0*q1 - 2*q2*q3)) - P[17][21]*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2*q0*q1 - 2*q2*q3)));
|
||||
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2);
|
||||
SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
|
||||
SK_MZ[3] = 2*q0*q1 - 2*q2*q3;
|
||||
|
||||
float Kfusion[24];
|
||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
|
||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
|
||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
|
||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
|
||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
|
||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
|
||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
|
||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
|
||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
|
||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
|
||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
|
||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
|
||||
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
|
||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
|
||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
|
||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
|
||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
|
||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
|
||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
|
||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
|
||||
// Z axis Kalman gains
|
||||
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]);
|
||||
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]);
|
||||
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]);
|
||||
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]);
|
||||
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]);
|
||||
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]);
|
||||
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]);
|
||||
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]);
|
||||
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]);
|
||||
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]);
|
||||
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]);
|
||||
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]);
|
||||
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]);
|
||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]);
|
||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]);
|
||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]);
|
||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]);
|
||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]);
|
||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]);
|
||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
|
||||
|
||||
@ -15,36 +15,8 @@ It is important that maximum useage of the sparsity in the H matrix be taken adv
|
||||
otherwise the matrix operations should need unrolled with inclusion of conditional statements to improve efficiency. Examples of this technique can
|
||||
be found in the existing att_pos_ekf_estimator library
|
||||
|
||||
It is important that the attitude error state vector (first three states) be zeroed prior to fusion of a new observation. This is becasue it is assumed
|
||||
that the predicted quaternion attitude is corrected using the attitude error estimate each time an observation is fused.
|
||||
|
||||
Measurement fusion steps:
|
||||
|
||||
1) Calculate the innovation
|
||||
|
||||
2) Calculate the observation partial derivative (Jacobian) vector. For direct state observations this has not been provided as it is trivial,
|
||||
eg [0 0 0 1 0 .....] for a north velocity measurement
|
||||
|
||||
3) Calculate the innovation variance and apply an innovatino consistency check to determine if he observation should be fused. BTW, if
|
||||
(H*P*transpose(H) + R) < R, then the covariance matrix has become ill conditioned and should probably be reset. Certainly fusion of data
|
||||
should not be performed if H*P*transpose(H) < 0
|
||||
|
||||
4) Calculate the Kalman gain vector
|
||||
|
||||
5) Zero the first three states in the state vector
|
||||
|
||||
6) Calculate and apply a correction to the states which is the product of the Kalman gain matrix and innovation
|
||||
|
||||
7) The first three states represent the estimated angular misalignment vector. Rotate the predicted quaternion from the last INS calculation through
|
||||
the reciprocal rotation to remove the error
|
||||
|
||||
8) Update the covariance using the standard P = (I - K*H)*P operator, taking advatage of sparseness in the H and I matrices. This has been implemented
|
||||
in the code as a P - KHP operation. Note, the use of the short rather thn Joseph or other mumerically more accurate form of the equation and use of single
|
||||
precision operations means that numerical stability can be an issue, so symmetry and non-negative diagonal elements must be enforced after every
|
||||
covariance update.
|
||||
|
||||
NOTES FOR COVARIANCE PREDICTION
|
||||
|
||||
Only expression for the upper diagonal is provided. The values will need to be copied across to the lower diagnal elements assuming symmetry
|
||||
Process noise for time invariant states, eg Gyro bias, has not been included in the auto-code. The process noise variance for these states will
|
||||
need to be added in a separate operation.
|
||||
need to be added in a separate operation.
|
||||
|
||||
@ -3,219 +3,265 @@
|
||||
// Observations are body modtion compensated optica flow rates about the X and Y body axis
|
||||
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
|
||||
|
||||
float H_LOS[2][24];
|
||||
float H_LOS[2][24]; // Optical flow observation Jacobians
|
||||
float Kfusion[24][2]; // Optical flow Kalman gains
|
||||
|
||||
// calculate X axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
float t3 = q0 * q0;
|
||||
float t4 = q1 * q1;
|
||||
float t5 = q2 * q2;
|
||||
float t6 = q3 * q3;
|
||||
float t7 = q0 * q2 * 2.0f;
|
||||
float t8 = q1 * q3 * 2.0f;
|
||||
float t9 = q0 * q3 * 2.0f;
|
||||
float t10 = q1 * q2 * 2.0f;
|
||||
float t11 = q0 * q1 * 2.0f;
|
||||
H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f));
|
||||
H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6));
|
||||
H_LOS[0][3] = -t2 * (t9 - t10);
|
||||
H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6);
|
||||
H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f);
|
||||
H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
|
||||
H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
|
||||
|
||||
// calculate X axis Kalman gains
|
||||
t2 = 1.0/range;
|
||||
t3 = q0*q1*2.0;
|
||||
t4 = q2*q3*2.0;
|
||||
t5 = q0*q0;
|
||||
t6 = q1*q1;
|
||||
t7 = q2*q2;
|
||||
t8 = q3*q3;
|
||||
t9 = q0*q2*2.0;
|
||||
t10 = q1*q3*2.0;
|
||||
t11 = q0*q3*2.0;
|
||||
float t12 = q1*q2*2.0;
|
||||
float t13 = t11-t12;
|
||||
float t14 = t3+t4;
|
||||
float t15 = t5-t6-t7+t8;
|
||||
float t16 = t15*vd;
|
||||
float t17 = t3-t4;
|
||||
float t18 = t9+t10;
|
||||
float t19 = t18*vn;
|
||||
float t28 = t17*ve;
|
||||
float t20 = t16+t19-t28;
|
||||
float t21 = t5+t6-t7-t8;
|
||||
float t22 = t21*vn;
|
||||
float t23 = t9-t10;
|
||||
float t24 = t11+t12;
|
||||
float t25 = t24*ve;
|
||||
float t29 = t23*vd;
|
||||
float t26 = t22+t25-t29;
|
||||
float t27 = t5-t6+t7-t8;
|
||||
float t30 = P[0][0]*t2*t20;
|
||||
float t31 = P[5][3]*t2*t14;
|
||||
float t32 = P[0][3]*t2*t20;
|
||||
float t33 = P[4][3]*t2*t27;
|
||||
float t56 = P[3][3]*t2*t13;
|
||||
float t57 = P[2][3]*t2*t26;
|
||||
float t34 = t31+t32+t33-t56-t57;
|
||||
float t35 = P[5][5]*t2*t14;
|
||||
float t36 = P[0][5]*t2*t20;
|
||||
float t37 = P[4][5]*t2*t27;
|
||||
float t59 = P[3][5]*t2*t13;
|
||||
float t60 = P[2][5]*t2*t26;
|
||||
float t38 = t35+t36+t37-t59-t60;
|
||||
float t39 = t2*t14*t38;
|
||||
float t40 = P[5][0]*t2*t14;
|
||||
float t41 = P[4][0]*t2*t27;
|
||||
float t61 = P[3][0]*t2*t13;
|
||||
float t62 = P[2][0]*t2*t26;
|
||||
float t42 = t30+t40+t41-t61-t62;
|
||||
float t43 = t2*t20*t42;
|
||||
float t44 = P[5][2]*t2*t14;
|
||||
float t45 = P[0][2]*t2*t20;
|
||||
float t46 = P[4][2]*t2*t27;
|
||||
float t55 = P[2][2]*t2*t26;
|
||||
float t63 = P[3][2]*t2*t13;
|
||||
float t47 = t44+t45+t46-t55-t63;
|
||||
float t48 = P[5][4]*t2*t14;
|
||||
float t49 = P[0][4]*t2*t20;
|
||||
float t50 = P[4][4]*t2*t27;
|
||||
float t65 = P[3][4]*t2*t13;
|
||||
float t66 = P[2][4]*t2*t26;
|
||||
float t51 = t48+t49+t50-t65-t66;
|
||||
float t52 = t2*t27*t51;
|
||||
float t58 = t2*t13*t34;
|
||||
float t64 = t2*t26*t47;
|
||||
float t53 = R_LOS+t39+t43+t52-t58-t64;
|
||||
float t54 = 1.0/t53;
|
||||
Kfusion[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27);
|
||||
Kfusion[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27);
|
||||
Kfusion[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27);
|
||||
Kfusion[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27);
|
||||
Kfusion[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26);
|
||||
Kfusion[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27);
|
||||
Kfusion[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27);
|
||||
Kfusion[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27);
|
||||
Kfusion[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27);
|
||||
Kfusion[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27);
|
||||
Kfusion[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27);
|
||||
Kfusion[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27);
|
||||
Kfusion[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27);
|
||||
Kfusion[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27);
|
||||
Kfusion[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27);
|
||||
Kfusion[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27);
|
||||
Kfusion[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27);
|
||||
Kfusion[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27);
|
||||
Kfusion[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27);
|
||||
Kfusion[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27);
|
||||
Kfusion[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27);
|
||||
Kfusion[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27);
|
||||
Kfusion[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27);
|
||||
Kfusion[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27);
|
||||
// calculate intermediate variables for the X observaton innovatoin variance and Kalman gains
|
||||
float t3 = q1*vd*2.0f;
|
||||
float t4 = q0*ve*2.0f;
|
||||
float t11 = q3*vn*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t29 = q1*q2*2.0f;
|
||||
float t7 = t6-t29;
|
||||
float t8 = q0*q1*2.0f;
|
||||
float t9 = q2*q3*2.0f;
|
||||
float t10 = t8+t9;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q0*vd*2.0f;
|
||||
float t14 = q2*vn*2.0f;
|
||||
float t28 = q1*ve*2.0f;
|
||||
float t15 = t13+t14-t28;
|
||||
float t16 = q3*vd*2.0f;
|
||||
float t17 = q2*ve*2.0f;
|
||||
float t18 = q1*vn*2.0f;
|
||||
float t19 = t16+t17+t18;
|
||||
float t20 = q3*ve*2.0f;
|
||||
float t21 = q0*vn*2.0f;
|
||||
float t30 = q2*vd*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23-t24+t25-t26;
|
||||
float t31 = P[1][1]*t2*t15;
|
||||
float t32 = P[6][0]*t2*t10;
|
||||
float t33 = P[1][0]*t2*t15;
|
||||
float t34 = P[2][0]*t2*t19;
|
||||
float t35 = P[5][0]*t2*t27;
|
||||
float t79 = P[4][0]*t2*t7;
|
||||
float t80 = P[3][0]*t2*t22;
|
||||
float t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[6][1]*t2*t10;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[2][1]*t2*t19;
|
||||
float t41 = P[5][1]*t2*t27;
|
||||
float t81 = P[4][1]*t2*t7;
|
||||
float t82 = P[3][1]*t2*t22;
|
||||
float t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
float t43 = t2*t15*t42;
|
||||
float t44 = P[6][2]*t2*t10;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t15;
|
||||
float t47 = P[2][2]*t2*t19;
|
||||
float t48 = P[5][2]*t2*t27;
|
||||
float t83 = P[4][2]*t2*t7;
|
||||
float t84 = P[3][2]*t2*t22;
|
||||
float t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
float t50 = t2*t19*t49;
|
||||
float t51 = P[6][3]*t2*t10;
|
||||
float t52 = P[0][3]*t2*t5;
|
||||
float t53 = P[1][3]*t2*t15;
|
||||
float t54 = P[2][3]*t2*t19;
|
||||
float t55 = P[5][3]*t2*t27;
|
||||
float t85 = P[4][3]*t2*t7;
|
||||
float t86 = P[3][3]*t2*t22;
|
||||
float t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
float t57 = P[6][5]*t2*t10;
|
||||
float t58 = P[0][5]*t2*t5;
|
||||
float t59 = P[1][5]*t2*t15;
|
||||
float t60 = P[2][5]*t2*t19;
|
||||
float t61 = P[5][5]*t2*t27;
|
||||
float t88 = P[4][5]*t2*t7;
|
||||
float t89 = P[3][5]*t2*t22;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[6][4]*t2*t10;
|
||||
float t65 = P[0][4]*t2*t5;
|
||||
float t66 = P[1][4]*t2*t15;
|
||||
float t67 = P[2][4]*t2*t19;
|
||||
float t68 = P[5][4]*t2*t27;
|
||||
float t90 = P[4][4]*t2*t7;
|
||||
float t91 = P[3][4]*t2*t22;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = P[6][6]*t2*t10;
|
||||
float t71 = P[0][6]*t2*t5;
|
||||
float t72 = P[1][6]*t2*t15;
|
||||
float t73 = P[2][6]*t2*t19;
|
||||
float t74 = P[5][6]*t2*t27;
|
||||
float t93 = P[4][6]*t2*t7;
|
||||
float t94 = P[3][6]*t2*t22;
|
||||
float t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
float t76 = t2*t10*t75;
|
||||
float t87 = t2*t22*t56;
|
||||
float t92 = t2*t7*t69;
|
||||
float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
float t78 = 1.0f / t77;
|
||||
|
||||
// calculate Y axis observation jacobian
|
||||
float t2 = 1.0f/range;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = q0*q1*2.0f;
|
||||
float t8 = q0*q3*2.0f;
|
||||
// calculate Kalman gains for X-axis observation
|
||||
Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
||||
Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
||||
Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
||||
Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
||||
Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
||||
Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
||||
Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
||||
Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
||||
Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
||||
Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
||||
Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
||||
Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
||||
Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
||||
Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
||||
Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
||||
Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
||||
Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
||||
Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
||||
Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
|
||||
// calculate Y axis observation Jacobian
|
||||
float t2 = 1.0f / range;
|
||||
H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
|
||||
H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
|
||||
H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
|
||||
H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
|
||||
H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
|
||||
H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
|
||||
|
||||
// calculate intermediate variables for the Y axis observaton innovation variance and Kalman gains
|
||||
float t3 = q3*ve*2.0f;
|
||||
float t4 = q0*vn*2.0f;
|
||||
float t11 = q2*vd*2.0f;
|
||||
float t5 = t3+t4-t11;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t7 = q1*q2*2.0f;
|
||||
float t8 = t6+t7;
|
||||
float t9 = q0*q2*2.0f;
|
||||
float t10 = q1*q3*2.0f;
|
||||
H_LOS[1][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0f));
|
||||
H_LOS[1][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0f)-vn*(t8-q1*q2*2.0f));
|
||||
H_LOS[1][3] = -t2*(t3+t4-t5-t6);
|
||||
H_LOS[1][4] = -t2*(t8+q1*q2*2.0f);
|
||||
H_LOS[1][5] = t2*(t9-t10);
|
||||
float t28 = q1*q3*2.0f;
|
||||
float t10 = t9-t28;
|
||||
float t12 = P[0][0]*t2*t5;
|
||||
float t13 = q3*vd*2.0f;
|
||||
float t14 = q2*ve*2.0f;
|
||||
float t15 = q1*vn*2.0f;
|
||||
float t16 = t13+t14+t15;
|
||||
float t17 = q0*vd*2.0f;
|
||||
float t18 = q2*vn*2.0f;
|
||||
float t29 = q1*ve*2.0f;
|
||||
float t19 = t17+t18-t29;
|
||||
float t20 = q1*vd*2.0f;
|
||||
float t21 = q0*ve*2.0f;
|
||||
float t30 = q3*vn*2.0f;
|
||||
float t22 = t20+t21-t30;
|
||||
float t23 = q0*q0;
|
||||
float t24 = q1*q1;
|
||||
float t25 = q2*q2;
|
||||
float t26 = q3*q3;
|
||||
float t27 = t23+t24-t25-t26;
|
||||
float t31 = P[1][1]*t2*t16;
|
||||
float t32 = P[5][0]*t2*t8;
|
||||
float t33 = P[1][0]*t2*t16;
|
||||
float t34 = P[3][0]*t2*t22;
|
||||
float t35 = P[4][0]*t2*t27;
|
||||
float t80 = P[6][0]*t2*t10;
|
||||
float t81 = P[2][0]*t2*t19;
|
||||
float t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
float t37 = t2*t5*t36;
|
||||
float t38 = P[5][1]*t2*t8;
|
||||
float t39 = P[0][1]*t2*t5;
|
||||
float t40 = P[3][1]*t2*t22;
|
||||
float t41 = P[4][1]*t2*t27;
|
||||
float t82 = P[6][1]*t2*t10;
|
||||
float t83 = P[2][1]*t2*t19;
|
||||
float t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
float t43 = t2*t16*t42;
|
||||
float t44 = P[5][2]*t2*t8;
|
||||
float t45 = P[0][2]*t2*t5;
|
||||
float t46 = P[1][2]*t2*t16;
|
||||
float t47 = P[3][2]*t2*t22;
|
||||
float t48 = P[4][2]*t2*t27;
|
||||
float t79 = P[2][2]*t2*t19;
|
||||
float t84 = P[6][2]*t2*t10;
|
||||
float t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
float t50 = P[5][3]*t2*t8;
|
||||
float t51 = P[0][3]*t2*t5;
|
||||
float t52 = P[1][3]*t2*t16;
|
||||
float t53 = P[3][3]*t2*t22;
|
||||
float t54 = P[4][3]*t2*t27;
|
||||
float t86 = P[6][3]*t2*t10;
|
||||
float t87 = P[2][3]*t2*t19;
|
||||
float t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
float t56 = t2*t22*t55;
|
||||
float t57 = P[5][4]*t2*t8;
|
||||
float t58 = P[0][4]*t2*t5;
|
||||
float t59 = P[1][4]*t2*t16;
|
||||
float t60 = P[3][4]*t2*t22;
|
||||
float t61 = P[4][4]*t2*t27;
|
||||
float t88 = P[6][4]*t2*t10;
|
||||
float t89 = P[2][4]*t2*t19;
|
||||
float t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
float t63 = t2*t27*t62;
|
||||
float t64 = P[5][5]*t2*t8;
|
||||
float t65 = P[0][5]*t2*t5;
|
||||
float t66 = P[1][5]*t2*t16;
|
||||
float t67 = P[3][5]*t2*t22;
|
||||
float t68 = P[4][5]*t2*t27;
|
||||
float t90 = P[6][5]*t2*t10;
|
||||
float t91 = P[2][5]*t2*t19;
|
||||
float t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
float t70 = t2*t8*t69;
|
||||
float t71 = P[5][6]*t2*t8;
|
||||
float t72 = P[0][6]*t2*t5;
|
||||
float t73 = P[1][6]*t2*t16;
|
||||
float t74 = P[3][6]*t2*t22;
|
||||
float t75 = P[4][6]*t2*t27;
|
||||
float t92 = P[6][6]*t2*t10;
|
||||
float t93 = P[2][6]*t2*t19;
|
||||
float t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
float t85 = t2*t19*t49;
|
||||
float t94 = t2*t10*t76;
|
||||
float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
float t78 = 1.0f / t77;
|
||||
|
||||
// calculate Y axis Kalman gains
|
||||
t2 = 1.0f/range;
|
||||
t3 = q0*q2*2.0f;
|
||||
t4 = q0*q0;
|
||||
t5 = q1*q1;
|
||||
t6 = q2*q2;
|
||||
t7 = q3*q3;
|
||||
t8 = q0*q1*2.0f;
|
||||
t9 = q0*q3*2.0f;
|
||||
t10 = q1*q2*2.0f;
|
||||
float t11 = t9+t10;
|
||||
float t12 = q1*q3*2.0f;
|
||||
float t13 = t4-t5-t6+t7;
|
||||
float t14 = t13*vd;
|
||||
float t15 = q2*q3*2.0f;
|
||||
float t16 = t3+t12;
|
||||
float t17 = t16*vn;
|
||||
float t18 = t4-t5+t6-t7;
|
||||
float t19 = t18*ve;
|
||||
float t20 = t8+t15;
|
||||
float t21 = t20*vd;
|
||||
float t22 = t9-t10;
|
||||
float t28 = t22*vn;
|
||||
float t23 = t19+t21-t28;
|
||||
float t24 = t4+t5-t6-t7;
|
||||
float t25 = t3-t12;
|
||||
float t26 = t8-t15;
|
||||
float t29 = t26*ve;
|
||||
float t27 = t14+t17-t29;
|
||||
float t30 = P[4][4]*t2*t11;
|
||||
float t31 = P[2][4]*t2*t23;
|
||||
float t32 = P[3][4]*t2*t24;
|
||||
float t56 = P[5][4]*t2*t25;
|
||||
float t57 = P[1][4]*t2*t27;
|
||||
float t33 = t30+t31+t32-t56-t57;
|
||||
float t34 = t2*t11*t33;
|
||||
float t35 = P[4][5]*t2*t11;
|
||||
float t36 = P[2][5]*t2*t23;
|
||||
float t37 = P[3][5]*t2*t24;
|
||||
float t58 = P[5][5]*t2*t25;
|
||||
float t59 = P[1][5]*t2*t27;
|
||||
float t38 = t35+t36+t37-t58-t59;
|
||||
float t39 = P[4][1]*t2*t11;
|
||||
float t40 = P[2][1]*t2*t23;
|
||||
float t41 = P[3][1]*t2*t24;
|
||||
float t55 = P[1][1]*t2*t27;
|
||||
float t61 = P[5][1]*t2*t25;
|
||||
float t42 = t39+t40+t41-t55-t61;
|
||||
float t43 = P[4][2]*t2*t11;
|
||||
float t44 = P[2][2]*t2*t23;
|
||||
float t45 = P[3][2]*t2*t24;
|
||||
float t63 = P[5][2]*t2*t25;
|
||||
float t64 = P[1][2]*t2*t27;
|
||||
float t46 = t43+t44+t45-t63-t64;
|
||||
float t47 = t2*t23*t46;
|
||||
float t48 = P[4][3]*t2*t11;
|
||||
float t49 = P[2][3]*t2*t23;
|
||||
float t50 = P[3][3]*t2*t24;
|
||||
float t65 = P[5][3]*t2*t25;
|
||||
float t66 = P[1][3]*t2*t27;
|
||||
float t51 = t48+t49+t50-t65-t66;
|
||||
float t52 = t2*t24*t51;
|
||||
float t60 = t2*t25*t38;
|
||||
float t62 = t2*t27*t42;
|
||||
float t53 = R_LOS+t34+t47+t52-t60-t62;
|
||||
float t54 = 1.0f/t53;
|
||||
Kfusion[0][1] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25);
|
||||
Kfusion[1][1] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25);
|
||||
Kfusion[2][1] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25);
|
||||
Kfusion[3][1] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25);
|
||||
Kfusion[4][1] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25);
|
||||
Kfusion[5][1] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27);
|
||||
Kfusion[6][1] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25);
|
||||
Kfusion[7][1] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25);
|
||||
Kfusion[8][1] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25);
|
||||
Kfusion[9][1] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25);
|
||||
Kfusion[10][1] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25);
|
||||
Kfusion[11][1] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25);
|
||||
Kfusion[12][1] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25);
|
||||
Kfusion[13][1] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25);
|
||||
Kfusion[14][1] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25);
|
||||
Kfusion[15][1] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25);
|
||||
Kfusion[16][1] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25);
|
||||
Kfusion[17][1] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25);
|
||||
Kfusion[18][1] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25);
|
||||
Kfusion[19][1] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25);
|
||||
Kfusion[20][1] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25);
|
||||
Kfusion[21][1] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25);
|
||||
Kfusion[22][1] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25);
|
||||
Kfusion[23][1] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25);
|
||||
// calculate Kalman gains for Y-axis observation
|
||||
Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
||||
Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
||||
Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
||||
Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
||||
Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
||||
Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
||||
Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
||||
Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
||||
Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
||||
Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
||||
Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
||||
Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
||||
Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
||||
Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
||||
Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
||||
Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
||||
Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
||||
Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
||||
Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -10,48 +10,33 @@ Divide by zero protection hs been added
|
||||
*/
|
||||
|
||||
// calculate intermediate variables for observation jacobian
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = q0*q3*2.0f;
|
||||
float t8 = t2-t3+t4-t5;
|
||||
float t9 = q0*q1*2.0f;
|
||||
float t10 = q2*q3*2.0f;
|
||||
float t11 = t9-t10;
|
||||
float t14 = q1*q2*2.0f;
|
||||
float t21 = magY*t8;
|
||||
float t22 = t6+t14;
|
||||
float t23 = magX*t22;
|
||||
float t24 = magZ*t11;
|
||||
float t7 = t21+t23-t24;
|
||||
float t12 = t2+t3-t4-t5;
|
||||
float t13 = magX*t12;
|
||||
float t15 = q0*q2*2.0f;
|
||||
float t16 = q1*q3*2.0f;
|
||||
float t17 = t15+t16;
|
||||
float t18 = magZ*t17;
|
||||
float t19 = t6-t14;
|
||||
float t25 = magY*t19;
|
||||
float t20 = t13+t18-t25;
|
||||
if (fabsf(t20) < 1e-6f) {
|
||||
return;
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
float t8 = t7*t7;
|
||||
if (t8 > 1e-6f) {
|
||||
t8 = 1.0f/t8;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t26 = 1.0f/(t20*t20);
|
||||
float t27 = t7*t7;
|
||||
float t28 = t26*t27;
|
||||
float t29 = t28+1.0;
|
||||
if (fabsf(t29) < 1e-12f) {
|
||||
return;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14;
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f/t13;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t30 = 1.0f/t29;
|
||||
if (fabsf(t20) < 1e-12f) {
|
||||
return;
|
||||
}
|
||||
float t31 = 1.0f/t20;
|
||||
|
||||
// calculate observation jacobian
|
||||
float H_DECL[3] = {};
|
||||
H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
|
||||
H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
|
||||
H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
|
||||
float H_DECL[4] = {};
|
||||
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
|
||||
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
|
||||
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
|
||||
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
|
||||
|
||||
@ -2,44 +2,66 @@
|
||||
Code fragments for fusion of an Euler yaw measurement from a 321 sequence.
|
||||
*/
|
||||
|
||||
// calculate observation jacobians
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = t2+t3-t4-t5;
|
||||
float t7 = q0*q3*2.0f;
|
||||
float t8 = q1*q2*2.0f;
|
||||
float t9 = t7+t8;
|
||||
float t10 = 1.0f/(t6*t6);
|
||||
float t11 = t9*t9;
|
||||
float t12 = t10*t11;
|
||||
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
float t8 = t7*t7;
|
||||
if (t8 > 1e-6f) {
|
||||
t8 = 1.0f/t8;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
float t15 = 1.0f/t6;
|
||||
H_YAW[0] = 0.0f;
|
||||
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
|
||||
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
|
||||
float t14;
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f/t13;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
|
||||
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
|
||||
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
|
||||
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;
|
||||
|
||||
/*
|
||||
Code fragments for fusion of an Euler yaw measurement from a 312 sequence.
|
||||
*/
|
||||
|
||||
// calculate observation jacobian
|
||||
float t2 = q0*q0;
|
||||
float t3 = q1*q1;
|
||||
float t4 = q2*q2;
|
||||
float t5 = q3*q3;
|
||||
float t6 = t2-t3+t4-t5;
|
||||
float t7 = q0*q3*2.0f;
|
||||
float t10 = q1*q2*2.0f;
|
||||
float t8 = t7-t10;
|
||||
float t9 = 1.0f/(t6*t6);
|
||||
float t11 = t8*t8;
|
||||
float t12 = t9*t11;
|
||||
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9-t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3-t4+t5-t6;
|
||||
float t8 = t7*t7;
|
||||
if (t8 > 1e-6f) {
|
||||
t8 = 1.0f/t8;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
float t15 = 1.0f/t6;
|
||||
H_YAW[0] = -t14*(t15*(q0*q2*2.0f+q1*q3*2.0f)-t8*t9*(q0*q1*2.0f-q2*q3*2.0f));
|
||||
H_YAW[1] = 0.0f;
|
||||
H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
|
||||
float t14;
|
||||
if (fabsf(t13) > 1e-6f) {
|
||||
t14 = 1.0f/t13;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f;
|
||||
H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f;
|
||||
H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f;
|
||||
H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f;
|
||||
|
||||
Binary file not shown.
File diff suppressed because one or more lines are too long
Binary file not shown.
@ -12,20 +12,12 @@
|
||||
|
||||
% Author: Paul Riseborough
|
||||
|
||||
% Based on use of a rotation vector for attitude estimation as described
|
||||
% here:
|
||||
|
||||
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
|
||||
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
|
||||
% pp. 855-860.
|
||||
|
||||
% State vector:
|
||||
% error rotation vector in body frame (X,Y,Z)
|
||||
% attitude quaternion
|
||||
% Velocity - m/sec (North, East, Down)
|
||||
% Position - m (North, East, Down)
|
||||
% Delta Angle bias - rad (X,Y,Z)
|
||||
% Delta Angle scale factor (X,Y,Z)
|
||||
% Delta Velocity bias - m/s (Z)
|
||||
% Delta Velocity bias - m/s (X,Y,Z)
|
||||
% Earth Magnetic Field Vector - (North, East, Down)
|
||||
% Body Magnetic Field Vector - (X,Y,Z)
|
||||
% Wind Vector - m/sec (North,East)
|
||||
@ -51,11 +43,10 @@ syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to l
|
||||
syms vn ve vd real % NED velocity - m/sec
|
||||
syms pn pe pd real % NED position - m
|
||||
syms dax_b day_b daz_b real % delta angle bias - rad
|
||||
syms dax_s day_s daz_s real % delta angle scale factor
|
||||
syms dvz_b dvy_b dvz_b real % delta velocity bias - m/sec
|
||||
syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
|
||||
syms dt real % IMU time step - sec
|
||||
syms gravity real % gravity - m/sec^2
|
||||
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise
|
||||
syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
|
||||
syms vwn vwe real; % NE wind velocity - m/sec
|
||||
syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss
|
||||
syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
|
||||
@ -66,7 +57,6 @@ syms R_MAG real % variance for magnetic flux measurements - milligauss^2
|
||||
syms R_BETA real % variance of sidelsip measurements rad^2
|
||||
syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
|
||||
syms ptd real % location of terrain in D axis
|
||||
syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
|
||||
syms decl real; % earth magnetic field declination from true north
|
||||
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
|
||||
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
|
||||
@ -82,28 +72,23 @@ dVelMeas = [dvx; dvy; dvz];
|
||||
|
||||
% define the IMU bias errors and scale factor
|
||||
dAngBias = [dax_b; day_b; daz_b];
|
||||
dAngScale = [dax_s; day_s; daz_s];
|
||||
dVelBias = [0;0;dvz_b];
|
||||
dVelBias = [dvx_b; dvy_b; dvz_b];
|
||||
|
||||
% define the quaternion rotation vector for the state estimate
|
||||
estQuat = [q0;q1;q2;q3];
|
||||
|
||||
% define the attitude error rotation vector, where error = truth - estimate
|
||||
errRotVec = [rotErrX;rotErrY;rotErrZ];
|
||||
|
||||
% define the attitude error quaternion using a first order linearisation
|
||||
errQuat = [1;0.5*errRotVec];
|
||||
|
||||
% Define the truth quaternion as the estimate + error
|
||||
truthQuat = QuatMult(estQuat, errQuat);
|
||||
quat = [q0;q1;q2;q3];
|
||||
|
||||
% derive the truth body to nav direction cosine matrix
|
||||
Tbn = Quat2Tbn(truthQuat);
|
||||
Tbn = Quat2Tbn(quat);
|
||||
|
||||
% define the truth delta angle
|
||||
% ignore coning compensation as these effects are negligible in terms of
|
||||
% covariance growth for our application and grade of sensor
|
||||
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
|
||||
dAngTruth = dAngMeas - dAngBias;
|
||||
|
||||
% Define the truth delta velocity -ignore sculling and transport rate
|
||||
% corrections as these negligible are in terms of covariance growth for our
|
||||
% application and grade of sensor
|
||||
dVelTruth = dVelMeas - dVelBias;
|
||||
|
||||
% define the attitude update equations
|
||||
% use a first order expansion of rotation to calculate the quaternion increment
|
||||
@ -113,16 +98,7 @@ deltaQuat = [1;
|
||||
0.5*dAngTruth(2);
|
||||
0.5*dAngTruth(3);
|
||||
];
|
||||
truthQuatNew = QuatMult(truthQuat,deltaQuat);
|
||||
% calculate the updated attitude error quaternion with respect to the previous estimate
|
||||
errQuatNew = QuatDivide(truthQuatNew,estQuat);
|
||||
% change to a rotaton vector - this is the error rotation vector updated state
|
||||
errRotNew = 2 * [errQuatNew(2);errQuatNew(3);errQuatNew(4)];
|
||||
|
||||
% Define the truth delta velocity -ignore sculling and transport rate
|
||||
% corrections as these negligible are in terms of covariance growth for our
|
||||
% application and grade of sensor
|
||||
dVelTruth = dVelMeas - dVelBias - [dvxNoise;dvyNoise;dvzNoise];
|
||||
quatNew = QuatMult(quat,deltaQuat);
|
||||
|
||||
% define the velocity update equations
|
||||
% ignore coriolis terms for linearisation purposes
|
||||
@ -132,9 +108,8 @@ vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
|
||||
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
|
||||
|
||||
% define the IMU error update equations
|
||||
dabNew = [dax_b; day_b; daz_b];
|
||||
dasNew = [dax_s; day_s; daz_s];
|
||||
dvbNew = dvz_b;
|
||||
dAngBiasNew = dAngBias;
|
||||
dVelBiasNew = dVelBias;
|
||||
|
||||
% define the wind velocity update equations
|
||||
vwnNew = vwn;
|
||||
@ -151,16 +126,15 @@ magYnew = magY;
|
||||
magZnew = magZ;
|
||||
|
||||
% Define the state vector & number of states
|
||||
stateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b;magN;magE;magD;magX;magY;magZ;vwn;vwe];
|
||||
stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
|
||||
nStates=numel(stateVector);
|
||||
|
||||
% Define vector of process equations
|
||||
newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
|
||||
newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
|
||||
|
||||
% derive the state transition matrix
|
||||
F = jacobian(newStateVector, stateVector);
|
||||
% set the rotation error states to zero
|
||||
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
@ -181,27 +155,19 @@ save 'StatePrediction.mat';
|
||||
% 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];
|
||||
% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
|
||||
% velocities, after bias effects have been removed.
|
||||
|
||||
% derive the control(disturbance) influence matrix
|
||||
G = jacobian(newStateVector, distVector);
|
||||
G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
% derive the control(disturbance) influence matrix from IMu noise to state
|
||||
% noise
|
||||
G = jacobian(newStateVector, [dAngMeas;dVelMeas]);
|
||||
[G,SG]=OptimiseAlgebra(G,'SG');
|
||||
|
||||
% derive the state error matrix
|
||||
distMatrix = diag(distVector.^2);
|
||||
distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]);
|
||||
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;
|
||||
|
||||
@ -216,7 +182,6 @@ reset(symengine);
|
||||
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
|
||||
@ -230,11 +195,10 @@ reset(symengine);
|
||||
load('StatePrediction.mat');
|
||||
|
||||
% calculate wind relative velocities in nav frame and rotate into body frame
|
||||
Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd];
|
||||
Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd];
|
||||
% calculate predicted angle of sideslip using small angle assumption
|
||||
BetaPred = Vbw(2)/Vbw(1);
|
||||
H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
|
||||
H_BETA = 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
|
||||
|
||||
@ -248,7 +212,6 @@ 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
|
||||
@ -281,7 +244,6 @@ 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');
|
||||
@ -294,7 +256,6 @@ 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');
|
||||
@ -308,7 +269,6 @@ 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');
|
||||
@ -321,7 +281,6 @@ 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');
|
||||
@ -336,7 +295,6 @@ 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');
|
||||
@ -351,7 +309,6 @@ 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');
|
||||
@ -367,7 +324,6 @@ load('StatePrediction.mat');
|
||||
% 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);
|
||||
@ -404,7 +360,6 @@ 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);
|
||||
@ -412,7 +367,6 @@ K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
|
||||
|
||||
% 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);
|
||||
@ -436,4 +390,4 @@ fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
save(fileName);
|
||||
SaveScriptCode(nStates);
|
||||
ConvertToM(nStates);
|
||||
ConvertToC(nStates);
|
||||
ConvertToC(nStates);
|
||||
|
||||
@ -1,15 +1,8 @@
|
||||
t2 = 1.0/range;
|
||||
t3 = q0*q0;
|
||||
t4 = q1*q1;
|
||||
t5 = q2*q2;
|
||||
t6 = q3*q3;
|
||||
t7 = q0*q2*2.0;
|
||||
t8 = q1*q3*2.0;
|
||||
t9 = q0*q3*2.0;
|
||||
t10 = q1*q2*2.0;
|
||||
t11 = q0*q1*2.0;
|
||||
A0[0][0] = t2*(vn*(t7+t8)+vd*(t3-t4-t5+t6)-ve*(t11-q2*q3*2.0));
|
||||
A0[0][2] = -t2*(ve*(t9+t10)-vd*(t7-t8)+vn*(t3+t4-t5-t6));
|
||||
A0[0][3] = -t2*(t9-t10);
|
||||
A0[0][4] = t2*(t3-t4+t5-t6);
|
||||
A0[0][5] = t2*(t11+q2*q3*2.0);
|
||||
A0[0][0] = t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
|
||||
A0[0][1] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
|
||||
A0[0][2] = t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
|
||||
A0[0][3] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
|
||||
A0[0][4] = -t2*(q0*q3*2.0-q1*q2*2.0);
|
||||
A0[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
|
||||
A0[0][6] = t2*(q0*q1*2.0+q2*q3*2.0);
|
||||
|
||||
@ -1,14 +1,8 @@
|
||||
t2 = 1.0/range;
|
||||
t3 = q0*q0;
|
||||
t4 = q1*q1;
|
||||
t5 = q2*q2;
|
||||
t6 = q3*q3;
|
||||
t7 = q0*q1*2.0;
|
||||
t8 = q0*q3*2.0;
|
||||
t9 = q0*q2*2.0;
|
||||
t10 = q1*q3*2.0;
|
||||
A0[0][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0));
|
||||
A0[0][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0)-vn*(t8-q1*q2*2.0));
|
||||
A0[0][3] = -t2*(t3+t4-t5-t6);
|
||||
A0[0][4] = -t2*(t8+q1*q2*2.0);
|
||||
A0[0][5] = t2*(t9-t10);
|
||||
A0[0][0] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
|
||||
A0[0][1] = -t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
|
||||
A0[0][2] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
|
||||
A0[0][3] = -t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
|
||||
A0[0][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
|
||||
A0[0][5] = -t2*(q0*q3*2.0+q1*q2*2.0);
|
||||
A0[0][6] = t2*(q0*q2*2.0-q1*q3*2.0);
|
||||
|
||||
@ -1,89 +1,117 @@
|
||||
t2 = 1.0/range;
|
||||
t3 = q0*q1*2.0;
|
||||
t4 = q2*q3*2.0;
|
||||
t5 = q0*q0;
|
||||
t6 = q1*q1;
|
||||
t7 = q2*q2;
|
||||
t8 = q3*q3;
|
||||
t9 = q0*q2*2.0;
|
||||
t10 = q1*q3*2.0;
|
||||
t11 = q0*q3*2.0;
|
||||
t12 = q1*q2*2.0;
|
||||
t13 = t11-t12;
|
||||
t14 = t3+t4;
|
||||
t15 = t5-t6-t7+t8;
|
||||
t16 = t15*vd;
|
||||
t17 = t3-t4;
|
||||
t18 = t9+t10;
|
||||
t19 = t18*vn;
|
||||
t28 = t17*ve;
|
||||
t20 = t16+t19-t28;
|
||||
t21 = t5+t6-t7-t8;
|
||||
t22 = t21*vn;
|
||||
t23 = t9-t10;
|
||||
t24 = t11+t12;
|
||||
t25 = t24*ve;
|
||||
t29 = t23*vd;
|
||||
t26 = t22+t25-t29;
|
||||
t27 = t5-t6+t7-t8;
|
||||
t30 = P[0][0]*t2*t20;
|
||||
t31 = P[5][3]*t2*t14;
|
||||
t32 = P[0][3]*t2*t20;
|
||||
t33 = P[4][3]*t2*t27;
|
||||
t56 = P[3][3]*t2*t13;
|
||||
t57 = P[2][3]*t2*t26;
|
||||
t34 = t31+t32+t33-t56-t57;
|
||||
t35 = P[5][5]*t2*t14;
|
||||
t36 = P[0][5]*t2*t20;
|
||||
t37 = P[4][5]*t2*t27;
|
||||
t59 = P[3][5]*t2*t13;
|
||||
t60 = P[2][5]*t2*t26;
|
||||
t38 = t35+t36+t37-t59-t60;
|
||||
t39 = t2*t14*t38;
|
||||
t40 = P[5][0]*t2*t14;
|
||||
t41 = P[4][0]*t2*t27;
|
||||
t61 = P[3][0]*t2*t13;
|
||||
t62 = P[2][0]*t2*t26;
|
||||
t42 = t30+t40+t41-t61-t62;
|
||||
t43 = t2*t20*t42;
|
||||
t44 = P[5][2]*t2*t14;
|
||||
t45 = P[0][2]*t2*t20;
|
||||
t46 = P[4][2]*t2*t27;
|
||||
t55 = P[2][2]*t2*t26;
|
||||
t63 = P[3][2]*t2*t13;
|
||||
t47 = t44+t45+t46-t55-t63;
|
||||
t48 = P[5][4]*t2*t14;
|
||||
t49 = P[0][4]*t2*t20;
|
||||
t50 = P[4][4]*t2*t27;
|
||||
t65 = P[3][4]*t2*t13;
|
||||
t66 = P[2][4]*t2*t26;
|
||||
t51 = t48+t49+t50-t65-t66;
|
||||
t52 = t2*t27*t51;
|
||||
t58 = t2*t13*t34;
|
||||
t64 = t2*t26*t47;
|
||||
t53 = R_LOS+t39+t43+t52-t58-t64;
|
||||
t54 = 1.0/t53;
|
||||
A0[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27);
|
||||
A0[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27);
|
||||
A0[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27);
|
||||
A0[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27);
|
||||
A0[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26);
|
||||
A0[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27);
|
||||
A0[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27);
|
||||
A0[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27);
|
||||
A0[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27);
|
||||
A0[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27);
|
||||
A0[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27);
|
||||
A0[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27);
|
||||
A0[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27);
|
||||
A0[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27);
|
||||
A0[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27);
|
||||
A0[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27);
|
||||
A0[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27);
|
||||
A0[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27);
|
||||
A0[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27);
|
||||
A0[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27);
|
||||
A0[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27);
|
||||
A0[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27);
|
||||
A0[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27);
|
||||
A0[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27);
|
||||
t3 = q1*vd*2.0;
|
||||
t4 = q0*ve*2.0;
|
||||
t11 = q3*vn*2.0;
|
||||
t5 = t3+t4-t11;
|
||||
t6 = q0*q3*2.0;
|
||||
t29 = q1*q2*2.0;
|
||||
t7 = t6-t29;
|
||||
t8 = q0*q1*2.0;
|
||||
t9 = q2*q3*2.0;
|
||||
t10 = t8+t9;
|
||||
t12 = P[0][0]*t2*t5;
|
||||
t13 = q0*vd*2.0;
|
||||
t14 = q2*vn*2.0;
|
||||
t28 = q1*ve*2.0;
|
||||
t15 = t13+t14-t28;
|
||||
t16 = q3*vd*2.0;
|
||||
t17 = q2*ve*2.0;
|
||||
t18 = q1*vn*2.0;
|
||||
t19 = t16+t17+t18;
|
||||
t20 = q3*ve*2.0;
|
||||
t21 = q0*vn*2.0;
|
||||
t30 = q2*vd*2.0;
|
||||
t22 = t20+t21-t30;
|
||||
t23 = q0*q0;
|
||||
t24 = q1*q1;
|
||||
t25 = q2*q2;
|
||||
t26 = q3*q3;
|
||||
t27 = t23-t24+t25-t26;
|
||||
t31 = P[1][1]*t2*t15;
|
||||
t32 = P[6][0]*t2*t10;
|
||||
t33 = P[1][0]*t2*t15;
|
||||
t34 = P[2][0]*t2*t19;
|
||||
t35 = P[5][0]*t2*t27;
|
||||
t79 = P[4][0]*t2*t7;
|
||||
t80 = P[3][0]*t2*t22;
|
||||
t36 = t12+t32+t33+t34+t35-t79-t80;
|
||||
t37 = t2*t5*t36;
|
||||
t38 = P[6][1]*t2*t10;
|
||||
t39 = P[0][1]*t2*t5;
|
||||
t40 = P[2][1]*t2*t19;
|
||||
t41 = P[5][1]*t2*t27;
|
||||
t81 = P[4][1]*t2*t7;
|
||||
t82 = P[3][1]*t2*t22;
|
||||
t42 = t31+t38+t39+t40+t41-t81-t82;
|
||||
t43 = t2*t15*t42;
|
||||
t44 = P[6][2]*t2*t10;
|
||||
t45 = P[0][2]*t2*t5;
|
||||
t46 = P[1][2]*t2*t15;
|
||||
t47 = P[2][2]*t2*t19;
|
||||
t48 = P[5][2]*t2*t27;
|
||||
t83 = P[4][2]*t2*t7;
|
||||
t84 = P[3][2]*t2*t22;
|
||||
t49 = t44+t45+t46+t47+t48-t83-t84;
|
||||
t50 = t2*t19*t49;
|
||||
t51 = P[6][3]*t2*t10;
|
||||
t52 = P[0][3]*t2*t5;
|
||||
t53 = P[1][3]*t2*t15;
|
||||
t54 = P[2][3]*t2*t19;
|
||||
t55 = P[5][3]*t2*t27;
|
||||
t85 = P[4][3]*t2*t7;
|
||||
t86 = P[3][3]*t2*t22;
|
||||
t56 = t51+t52+t53+t54+t55-t85-t86;
|
||||
t57 = P[6][5]*t2*t10;
|
||||
t58 = P[0][5]*t2*t5;
|
||||
t59 = P[1][5]*t2*t15;
|
||||
t60 = P[2][5]*t2*t19;
|
||||
t61 = P[5][5]*t2*t27;
|
||||
t88 = P[4][5]*t2*t7;
|
||||
t89 = P[3][5]*t2*t22;
|
||||
t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
t63 = t2*t27*t62;
|
||||
t64 = P[6][4]*t2*t10;
|
||||
t65 = P[0][4]*t2*t5;
|
||||
t66 = P[1][4]*t2*t15;
|
||||
t67 = P[2][4]*t2*t19;
|
||||
t68 = P[5][4]*t2*t27;
|
||||
t90 = P[4][4]*t2*t7;
|
||||
t91 = P[3][4]*t2*t22;
|
||||
t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
t70 = P[6][6]*t2*t10;
|
||||
t71 = P[0][6]*t2*t5;
|
||||
t72 = P[1][6]*t2*t15;
|
||||
t73 = P[2][6]*t2*t19;
|
||||
t74 = P[5][6]*t2*t27;
|
||||
t93 = P[4][6]*t2*t7;
|
||||
t94 = P[3][6]*t2*t22;
|
||||
t75 = t70+t71+t72+t73+t74-t93-t94;
|
||||
t76 = t2*t10*t75;
|
||||
t87 = t2*t22*t56;
|
||||
t92 = t2*t7*t69;
|
||||
t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
|
||||
t78 = 1.0/t77;
|
||||
A0[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
|
||||
A0[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
|
||||
A0[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
|
||||
A0[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
|
||||
A0[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
|
||||
A0[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
|
||||
A0[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
|
||||
A0[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
|
||||
A0[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
|
||||
A0[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
|
||||
A0[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
|
||||
A0[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
|
||||
A0[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
|
||||
A0[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
|
||||
A0[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
|
||||
A0[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
|
||||
A0[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
|
||||
A0[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
|
||||
A0[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
|
||||
A0[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
|
||||
A0[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
|
||||
A0[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
|
||||
A0[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
|
||||
A0[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
|
||||
|
||||
@ -1,89 +1,117 @@
|
||||
t2 = 1.0/range;
|
||||
t3 = q0*q2*2.0;
|
||||
t4 = q0*q0;
|
||||
t5 = q1*q1;
|
||||
t6 = q2*q2;
|
||||
t7 = q3*q3;
|
||||
t8 = q0*q1*2.0;
|
||||
t9 = q0*q3*2.0;
|
||||
t10 = q1*q2*2.0;
|
||||
t11 = t9+t10;
|
||||
t12 = q1*q3*2.0;
|
||||
t13 = t4-t5-t6+t7;
|
||||
t14 = t13*vd;
|
||||
t15 = q2*q3*2.0;
|
||||
t16 = t3+t12;
|
||||
t17 = t16*vn;
|
||||
t18 = t4-t5+t6-t7;
|
||||
t19 = t18*ve;
|
||||
t20 = t8+t15;
|
||||
t21 = t20*vd;
|
||||
t22 = t9-t10;
|
||||
t28 = t22*vn;
|
||||
t23 = t19+t21-t28;
|
||||
t24 = t4+t5-t6-t7;
|
||||
t25 = t3-t12;
|
||||
t26 = t8-t15;
|
||||
t29 = t26*ve;
|
||||
t27 = t14+t17-t29;
|
||||
t30 = P[4][4]*t2*t11;
|
||||
t31 = P[2][4]*t2*t23;
|
||||
t32 = P[3][4]*t2*t24;
|
||||
t56 = P[5][4]*t2*t25;
|
||||
t57 = P[1][4]*t2*t27;
|
||||
t33 = t30+t31+t32-t56-t57;
|
||||
t34 = t2*t11*t33;
|
||||
t35 = P[4][5]*t2*t11;
|
||||
t36 = P[2][5]*t2*t23;
|
||||
t37 = P[3][5]*t2*t24;
|
||||
t58 = P[5][5]*t2*t25;
|
||||
t59 = P[1][5]*t2*t27;
|
||||
t38 = t35+t36+t37-t58-t59;
|
||||
t39 = P[4][1]*t2*t11;
|
||||
t40 = P[2][1]*t2*t23;
|
||||
t41 = P[3][1]*t2*t24;
|
||||
t55 = P[1][1]*t2*t27;
|
||||
t61 = P[5][1]*t2*t25;
|
||||
t42 = t39+t40+t41-t55-t61;
|
||||
t43 = P[4][2]*t2*t11;
|
||||
t44 = P[2][2]*t2*t23;
|
||||
t45 = P[3][2]*t2*t24;
|
||||
t63 = P[5][2]*t2*t25;
|
||||
t64 = P[1][2]*t2*t27;
|
||||
t46 = t43+t44+t45-t63-t64;
|
||||
t47 = t2*t23*t46;
|
||||
t48 = P[4][3]*t2*t11;
|
||||
t49 = P[2][3]*t2*t23;
|
||||
t50 = P[3][3]*t2*t24;
|
||||
t65 = P[5][3]*t2*t25;
|
||||
t66 = P[1][3]*t2*t27;
|
||||
t51 = t48+t49+t50-t65-t66;
|
||||
t52 = t2*t24*t51;
|
||||
t60 = t2*t25*t38;
|
||||
t62 = t2*t27*t42;
|
||||
t53 = R_LOS+t34+t47+t52-t60-t62;
|
||||
t54 = 1.0/t53;
|
||||
A0[0][0] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25);
|
||||
A0[1][0] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25);
|
||||
A0[2][0] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25);
|
||||
A0[3][0] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25);
|
||||
A0[4][0] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25);
|
||||
A0[5][0] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27);
|
||||
A0[6][0] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25);
|
||||
A0[7][0] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25);
|
||||
A0[8][0] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25);
|
||||
A0[9][0] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25);
|
||||
A0[10][0] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25);
|
||||
A0[11][0] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25);
|
||||
A0[12][0] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25);
|
||||
A0[13][0] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25);
|
||||
A0[14][0] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25);
|
||||
A0[15][0] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25);
|
||||
A0[16][0] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25);
|
||||
A0[17][0] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25);
|
||||
A0[18][0] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25);
|
||||
A0[19][0] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25);
|
||||
A0[20][0] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25);
|
||||
A0[21][0] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25);
|
||||
A0[22][0] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25);
|
||||
A0[23][0] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25);
|
||||
t3 = q3*ve*2.0;
|
||||
t4 = q0*vn*2.0;
|
||||
t11 = q2*vd*2.0;
|
||||
t5 = t3+t4-t11;
|
||||
t6 = q0*q3*2.0;
|
||||
t7 = q1*q2*2.0;
|
||||
t8 = t6+t7;
|
||||
t9 = q0*q2*2.0;
|
||||
t28 = q1*q3*2.0;
|
||||
t10 = t9-t28;
|
||||
t12 = P[0][0]*t2*t5;
|
||||
t13 = q3*vd*2.0;
|
||||
t14 = q2*ve*2.0;
|
||||
t15 = q1*vn*2.0;
|
||||
t16 = t13+t14+t15;
|
||||
t17 = q0*vd*2.0;
|
||||
t18 = q2*vn*2.0;
|
||||
t29 = q1*ve*2.0;
|
||||
t19 = t17+t18-t29;
|
||||
t20 = q1*vd*2.0;
|
||||
t21 = q0*ve*2.0;
|
||||
t30 = q3*vn*2.0;
|
||||
t22 = t20+t21-t30;
|
||||
t23 = q0*q0;
|
||||
t24 = q1*q1;
|
||||
t25 = q2*q2;
|
||||
t26 = q3*q3;
|
||||
t27 = t23+t24-t25-t26;
|
||||
t31 = P[1][1]*t2*t16;
|
||||
t32 = P[5][0]*t2*t8;
|
||||
t33 = P[1][0]*t2*t16;
|
||||
t34 = P[3][0]*t2*t22;
|
||||
t35 = P[4][0]*t2*t27;
|
||||
t80 = P[6][0]*t2*t10;
|
||||
t81 = P[2][0]*t2*t19;
|
||||
t36 = t12+t32+t33+t34+t35-t80-t81;
|
||||
t37 = t2*t5*t36;
|
||||
t38 = P[5][1]*t2*t8;
|
||||
t39 = P[0][1]*t2*t5;
|
||||
t40 = P[3][1]*t2*t22;
|
||||
t41 = P[4][1]*t2*t27;
|
||||
t82 = P[6][1]*t2*t10;
|
||||
t83 = P[2][1]*t2*t19;
|
||||
t42 = t31+t38+t39+t40+t41-t82-t83;
|
||||
t43 = t2*t16*t42;
|
||||
t44 = P[5][2]*t2*t8;
|
||||
t45 = P[0][2]*t2*t5;
|
||||
t46 = P[1][2]*t2*t16;
|
||||
t47 = P[3][2]*t2*t22;
|
||||
t48 = P[4][2]*t2*t27;
|
||||
t79 = P[2][2]*t2*t19;
|
||||
t84 = P[6][2]*t2*t10;
|
||||
t49 = t44+t45+t46+t47+t48-t79-t84;
|
||||
t50 = P[5][3]*t2*t8;
|
||||
t51 = P[0][3]*t2*t5;
|
||||
t52 = P[1][3]*t2*t16;
|
||||
t53 = P[3][3]*t2*t22;
|
||||
t54 = P[4][3]*t2*t27;
|
||||
t86 = P[6][3]*t2*t10;
|
||||
t87 = P[2][3]*t2*t19;
|
||||
t55 = t50+t51+t52+t53+t54-t86-t87;
|
||||
t56 = t2*t22*t55;
|
||||
t57 = P[5][4]*t2*t8;
|
||||
t58 = P[0][4]*t2*t5;
|
||||
t59 = P[1][4]*t2*t16;
|
||||
t60 = P[3][4]*t2*t22;
|
||||
t61 = P[4][4]*t2*t27;
|
||||
t88 = P[6][4]*t2*t10;
|
||||
t89 = P[2][4]*t2*t19;
|
||||
t62 = t57+t58+t59+t60+t61-t88-t89;
|
||||
t63 = t2*t27*t62;
|
||||
t64 = P[5][5]*t2*t8;
|
||||
t65 = P[0][5]*t2*t5;
|
||||
t66 = P[1][5]*t2*t16;
|
||||
t67 = P[3][5]*t2*t22;
|
||||
t68 = P[4][5]*t2*t27;
|
||||
t90 = P[6][5]*t2*t10;
|
||||
t91 = P[2][5]*t2*t19;
|
||||
t69 = t64+t65+t66+t67+t68-t90-t91;
|
||||
t70 = t2*t8*t69;
|
||||
t71 = P[5][6]*t2*t8;
|
||||
t72 = P[0][6]*t2*t5;
|
||||
t73 = P[1][6]*t2*t16;
|
||||
t74 = P[3][6]*t2*t22;
|
||||
t75 = P[4][6]*t2*t27;
|
||||
t92 = P[6][6]*t2*t10;
|
||||
t93 = P[2][6]*t2*t19;
|
||||
t76 = t71+t72+t73+t74+t75-t92-t93;
|
||||
t85 = t2*t19*t49;
|
||||
t94 = t2*t10*t76;
|
||||
t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
|
||||
t78 = 1.0/t77;
|
||||
A0[0][0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
|
||||
A0[1][0] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
|
||||
A0[2][0] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
|
||||
A0[3][0] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
|
||||
A0[4][0] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
|
||||
A0[5][0] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
|
||||
A0[6][0] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
|
||||
A0[7][0] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
|
||||
A0[8][0] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
|
||||
A0[9][0] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
|
||||
A0[10][0] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
|
||||
A0[11][0] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
|
||||
A0[12][0] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
|
||||
A0[13][0] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
|
||||
A0[14][0] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
|
||||
A0[15][0] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
|
||||
A0[16][0] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
|
||||
A0[17][0] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
|
||||
A0[18][0] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
|
||||
A0[19][0] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
|
||||
A0[20][0] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
|
||||
A0[21][0] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
|
||||
A0[22][0] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
|
||||
A0[23][0] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
|
||||
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
@ -1,16 +1,17 @@
|
||||
t2 = q0*q0;
|
||||
t3 = q1*q1;
|
||||
t4 = q2*q2;
|
||||
t5 = q3*q3;
|
||||
t6 = t2-t3+t4-t5;
|
||||
t7 = q0*q3*2.0;
|
||||
t10 = q1*q2*2.0;
|
||||
t8 = t7-t10;
|
||||
t9 = 1.0/(t6*t6);
|
||||
t11 = t8*t8;
|
||||
t12 = t9*t11;
|
||||
t9 = q0*q3;
|
||||
t10 = q1*q2;
|
||||
t2 = t9-t10;
|
||||
t3 = q0*q0;
|
||||
t4 = q1*q1;
|
||||
t5 = q2*q2;
|
||||
t6 = q3*q3;
|
||||
t7 = t3-t4+t5-t6;
|
||||
t8 = 1.0/(t7*t7);
|
||||
t11 = t2*t2;
|
||||
t12 = t8*t11*4.0;
|
||||
t13 = t12+1.0;
|
||||
t14 = 1.0/t13;
|
||||
t15 = 1.0/t6;
|
||||
A0[0][0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
|
||||
A0[0][2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
|
||||
A0[0][0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0)*-2.0;
|
||||
A0[0][1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0)*-2.0;
|
||||
A0[0][2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0)*2.0;
|
||||
A0[0][3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0)*2.0;
|
||||
|
||||
@ -1,16 +1,17 @@
|
||||
t2 = q0*q0;
|
||||
t3 = q1*q1;
|
||||
t4 = q2*q2;
|
||||
t5 = q3*q3;
|
||||
t6 = t2+t3-t4-t5;
|
||||
t7 = q0*q3*2.0;
|
||||
t8 = q1*q2*2.0;
|
||||
t9 = t7+t8;
|
||||
t10 = 1.0/(t6*t6);
|
||||
t11 = t9*t9;
|
||||
t12 = t10*t11;
|
||||
t9 = q0*q3;
|
||||
t10 = q1*q2;
|
||||
t2 = t9+t10;
|
||||
t3 = q0*q0;
|
||||
t4 = q1*q1;
|
||||
t5 = q2*q2;
|
||||
t6 = q3*q3;
|
||||
t7 = t3+t4-t5-t6;
|
||||
t8 = 1.0/(t7*t7);
|
||||
t11 = t2*t2;
|
||||
t12 = t8*t11*4.0;
|
||||
t13 = t12+1.0;
|
||||
t14 = 1.0/t13;
|
||||
t15 = 1.0/t6;
|
||||
A0[0][1] = t14*(t15*(q0*q1*2.0-q2*q3*2.0)+t9*t10*(q0*q2*2.0+q1*q3*2.0));
|
||||
A0[0][2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
|
||||
A0[0][0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0)*-2.0;
|
||||
A0[0][1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0)*-2.0;
|
||||
A0[0][2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0)*2.0;
|
||||
A0[0][3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0)*2.0;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
x
Reference in New Issue
Block a user