mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
No changes to mainline code.
Three files added to AHRS which add accelerometer biases (16 states) to the INS/GPS. Added for evaluation (give it a try James). git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2182 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
dschin
dschin
parent
4f21bee0d7
commit
f99eb5b0c7
1005
flight/AHRS/ahrs16state.c
Normal file
1005
flight/AHRS/ahrs16state.c
Normal file
@ -0,0 +1,1005 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_Main
|
||||
* @brief Main function which does the hardware dependent stuff
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "ahrs.h"
|
||||
#include "ahrs_adc.h"
|
||||
#include "ahrs_timer.h"
|
||||
#include "ahrs_spi_comm.h"
|
||||
#include "insgps.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
#define MAX_OVERSAMPLING 50 /* cannot have more than 50 samples */
|
||||
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
|
||||
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
|
||||
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */
|
||||
#define INSGPS_MAGLEN 1000
|
||||
#define INSGPS_MAGTOL 0.5 /* error in magnetic vector length to use */
|
||||
|
||||
// For debugging the raw sensors
|
||||
//#define DUMP_RAW
|
||||
//#define DUMP_FRIENDLY
|
||||
#define DUMP_EKF
|
||||
|
||||
volatile int8_t ahrs_algorithm;
|
||||
|
||||
/* INS functions */
|
||||
void ins_outdoor_update();
|
||||
void ins_indoor_update();
|
||||
void simple_update();
|
||||
|
||||
/* Data accessors */
|
||||
void downsample_data(void);
|
||||
void process_mag_data();
|
||||
void reset_values();
|
||||
void calibrate_sensors(void);
|
||||
|
||||
/* Communication functions */
|
||||
void send_calibration(void);
|
||||
void send_attitude(void);
|
||||
void send_velocity(void);
|
||||
void send_position(void);
|
||||
void homelocation_callback(AhrsObjHandle obj);
|
||||
void altitude_callback(AhrsObjHandle obj);
|
||||
void calibration_callback(AhrsObjHandle obj);
|
||||
void gps_callback(AhrsObjHandle obj);
|
||||
void settings_callback(AhrsObjHandle obj);
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_Global_Data AHRS Global Data
|
||||
* @{
|
||||
* Public data. Used by both EKF and the sender
|
||||
*/
|
||||
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
|
||||
int16_t fir_coeffs[MAX_OVERSAMPLING];
|
||||
|
||||
//! Contains the data from the mag sensor chip
|
||||
struct mag_sensor mag_data;
|
||||
|
||||
//! Contains the data from the accelerometer
|
||||
struct accel_sensor accel_data;
|
||||
|
||||
//! Contains the data from the gyro
|
||||
struct gyro_sensor gyro_data;
|
||||
|
||||
//! Conains the current estimate of the attitude
|
||||
struct attitude_solution attitude_data;
|
||||
|
||||
//! Contains data from the altitude sensor
|
||||
struct altitude_sensor altitude_data;
|
||||
|
||||
//! Contains data from the GPS (via the SPI link)
|
||||
struct gps_sensor gps_data;
|
||||
|
||||
//! The oversampling rate, ekf is 2k / this
|
||||
static uint8_t adc_oversampling = 30;
|
||||
|
||||
//! Offset correction of barometric alt, to match gps data
|
||||
static float baro_offset = 0;
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* INS functions */
|
||||
|
||||
/**
|
||||
* @brief Update the EKF when in outdoor mode. The primary difference is using the GPS values.
|
||||
*/
|
||||
void ins_outdoor_update()
|
||||
{
|
||||
float gyro[3], accel[3], vel[3];
|
||||
static uint32_t last_gps_time = 0;
|
||||
uint16_t sensors;
|
||||
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
gyro[1] = gyro_data.filtered.y;
|
||||
gyro[2] = gyro_data.filtered.z;
|
||||
accel[0] = accel_data.filtered.x,
|
||||
accel[1] = accel_data.filtered.y,
|
||||
accel[2] = accel_data.filtered.z,
|
||||
|
||||
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
|
||||
attitude_data.quaternion.q1 = Nav.q[0];
|
||||
attitude_data.quaternion.q2 = Nav.q[1];
|
||||
attitude_data.quaternion.q3 = Nav.q[2];
|
||||
attitude_data.quaternion.q4 = Nav.q[3];
|
||||
send_attitude(); // get message out quickly
|
||||
send_velocity();
|
||||
send_position();
|
||||
INSCovariancePrediction(1 / (float)EKF_RATE);
|
||||
|
||||
sensors = 0;
|
||||
|
||||
/*
|
||||
* Detect if greater than certain time since last gps update and if so
|
||||
* reset EKF to that position since probably drifted too far for safe
|
||||
* update
|
||||
*/
|
||||
uint32_t this_gps_time = timer_count();
|
||||
float gps_delay;
|
||||
|
||||
if (this_gps_time < last_gps_time)
|
||||
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
|
||||
else
|
||||
gps_delay = (this_gps_time - last_gps_time) / timer_rate();
|
||||
last_gps_time = this_gps_time;
|
||||
|
||||
if (gps_data.updated) {
|
||||
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
|
||||
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
|
||||
vel[2] = 0;
|
||||
|
||||
if(gps_delay > INSGPS_GPS_TIMEOUT)
|
||||
INSPosVelReset(gps_data.NED,vel); // position stale, reset
|
||||
else {
|
||||
sensors |= HORIZ_SENSORS | POS_SENSORS;
|
||||
}
|
||||
|
||||
/*
|
||||
* When using gps need to make sure that barometer is brought into NED frame
|
||||
* we should try and see if the altitude from the home location is good enough
|
||||
* to use for the offset but for now starting with this conservative filter
|
||||
*/
|
||||
if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
|
||||
baro_offset = gps_data.NED[2] + altitude_data.altitude;
|
||||
} else {
|
||||
/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
|
||||
baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
|
||||
}
|
||||
gps_data.updated = false;
|
||||
} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
|
||||
vel[0] = 0;
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
sensors |= VERT_SENSORS | HORIZ_SENSORS;
|
||||
}
|
||||
|
||||
if(mag_data.updated) {
|
||||
sensors |= MAG_SENSORS;
|
||||
mag_data.updated = false;
|
||||
}
|
||||
|
||||
if(altitude_data.updated) {
|
||||
sensors |= BARO_SENSOR;
|
||||
altitude_data.updated = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
|
||||
* although probably should occur within INS itself
|
||||
*/
|
||||
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Update the EKF when in indoor mode
|
||||
*/
|
||||
void ins_indoor_update()
|
||||
{
|
||||
float gyro[3], accel[3], vel[3];
|
||||
static uint32_t last_indoor_time = 0;
|
||||
uint16_t sensors = 0;
|
||||
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
gyro[1] = gyro_data.filtered.y;
|
||||
gyro[2] = gyro_data.filtered.z;
|
||||
accel[0] = accel_data.filtered.x,
|
||||
accel[1] = accel_data.filtered.y,
|
||||
accel[2] = accel_data.filtered.z,
|
||||
|
||||
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
|
||||
attitude_data.quaternion.q1 = Nav.q[0];
|
||||
attitude_data.quaternion.q2 = Nav.q[1];
|
||||
attitude_data.quaternion.q3 = Nav.q[2];
|
||||
attitude_data.quaternion.q4 = Nav.q[3];
|
||||
send_attitude(); // get message out quickly
|
||||
send_velocity();
|
||||
send_position();
|
||||
INSCovariancePrediction(1 / (float)EKF_RATE);
|
||||
|
||||
/* Indoors, update with zero position and velocity and high covariance */
|
||||
vel[0] = 0;
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
|
||||
uint32_t this_indoor_time = timer_count();
|
||||
float indoor_delay;
|
||||
|
||||
/*
|
||||
* Detect if greater than certain time since last gps update and if so
|
||||
* reset EKF to that position since probably drifted too far for safe
|
||||
* update
|
||||
*/
|
||||
if (this_indoor_time < last_indoor_time)
|
||||
indoor_delay = ((0xFFFF - last_indoor_time) - this_indoor_time) / timer_rate();
|
||||
else
|
||||
indoor_delay = (this_indoor_time - last_indoor_time) / timer_rate();
|
||||
last_indoor_time = this_indoor_time;
|
||||
|
||||
if(indoor_delay > INSGPS_GPS_TIMEOUT)
|
||||
INSPosVelReset(vel,vel);
|
||||
else
|
||||
sensors = HORIZ_SENSORS | VERT_SENSORS;
|
||||
|
||||
if(mag_data.updated && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
|
||||
sensors |= MAG_SENSORS;
|
||||
mag_data.updated = false;
|
||||
}
|
||||
|
||||
if(altitude_data.updated) {
|
||||
sensors |= BARO_SENSOR;
|
||||
altitude_data.updated = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
|
||||
* although probably should occur within INS itself
|
||||
*/
|
||||
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize the EKF assuming stationary
|
||||
*/
|
||||
void ins_init_algorithm()
|
||||
{
|
||||
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
|
||||
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
|
||||
bool using_mags, using_gps;
|
||||
|
||||
INSGPSInit();
|
||||
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
accels[0]=accel_data.filtered.x;
|
||||
accels[1]=accel_data.filtered.y;
|
||||
accels[2]=accel_data.filtered.z;
|
||||
|
||||
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
|
||||
using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0); /* only use mags when valid home location */
|
||||
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality != 0);
|
||||
|
||||
if (using_mags){
|
||||
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
|
||||
R2Quaternion(Rbe,q);
|
||||
if (using_gps)
|
||||
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
|
||||
else
|
||||
INSSetState(zeros, zeros, q, zeros, zeros);
|
||||
}
|
||||
else{
|
||||
// assume yaw = 0
|
||||
mag = VectorMagnitude(accels);
|
||||
rpy[1] = asinf(-accels[0]/mag);
|
||||
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
|
||||
rpy[2] = 0;
|
||||
RPY2Quaternion(rpy,q);
|
||||
if (using_gps)
|
||||
INSSetState(gps_data.NED, zeros, q, zeros, zeros);
|
||||
else
|
||||
INSSetState(zeros, zeros, q, zeros, zeros);
|
||||
}
|
||||
|
||||
INSResetP(Pdiag);
|
||||
|
||||
// TODO: include initial estimate of gyro bias?
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Simple update using just mag and accel. Yaw biased and big attitude changes.
|
||||
*/
|
||||
void simple_update() {
|
||||
float q[4];
|
||||
float rpy[3];
|
||||
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
|
||||
/* Very simple computation of the heading and attitude from accel. */
|
||||
rpy[2] =
|
||||
atan2((mag_data.raw.axis[0]),
|
||||
(-1 * mag_data.raw.axis[1])) * 180 /
|
||||
M_PI;
|
||||
rpy[1] =
|
||||
atan2(accel_data.filtered.x,
|
||||
accel_data.filtered.z) * 180 / M_PI;
|
||||
rpy[0] =
|
||||
atan2(accel_data.filtered.y,
|
||||
accel_data.filtered.z) * 180 / M_PI;
|
||||
|
||||
RPY2Quaternion(rpy, q);
|
||||
attitude_data.quaternion.q1 = q[0];
|
||||
attitude_data.quaternion.q2 = q[1];
|
||||
attitude_data.quaternion.q3 = q[2];
|
||||
attitude_data.quaternion.q4 = q[3];
|
||||
send_attitude();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Output all the important inputs and states of the ekf through serial port
|
||||
*/
|
||||
#ifdef DUMP_EKF
|
||||
#define NUMX 16 // number of states, X is the state vector
|
||||
#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 7 // number of deterministic inputs, U is the input vector
|
||||
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
|
||||
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
|
||||
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
extern float K[NUMX][NUMV]; // feedback gain matrix
|
||||
void print_ekf_binary()
|
||||
{
|
||||
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
|
||||
// Dump raw buffer
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20)
|
||||
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32)
|
||||
PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (86:149)
|
||||
for(uint8_t i = 0; i < NUMX; i++)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (150:213)
|
||||
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (214:217)
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (218:221)
|
||||
}
|
||||
#else
|
||||
void print_ekf_binary() {}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Debugging function to output all the ADC samples
|
||||
*/
|
||||
void print_ahrs_raw()
|
||||
{
|
||||
int result;
|
||||
static int previous_conversion = 0;
|
||||
|
||||
uint8_t framing[16] =
|
||||
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
|
||||
15 };
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
|
||||
if (total_conversion_blocks != previous_conversion + 1)
|
||||
PIOS_LED_On(LED1); // not keeping up
|
||||
else
|
||||
PIOS_LED_Off(LED1);
|
||||
previous_conversion = total_conversion_blocks;
|
||||
|
||||
downsample_data();
|
||||
ahrs_state = AHRS_IDLE;;
|
||||
|
||||
// Dump raw buffer
|
||||
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
|
||||
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX,
|
||||
(uint8_t *) & valid_data_buffer[0],
|
||||
adc_oversampling *
|
||||
PIOS_ADC_NUM_PINS *
|
||||
sizeof(valid_data_buffer[0]));
|
||||
if (result == 0)
|
||||
PIOS_LED_Off(LED1);
|
||||
else {
|
||||
PIOS_LED_On(LED1);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief AHRS Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
gps_data.quality = -1;
|
||||
uint32_t up_time_real = 0;
|
||||
uint32_t up_time = 0;
|
||||
uint32_t last_up_time = 0;
|
||||
static int8_t last_ahrs_algorithm;
|
||||
uint32_t last_counter_idle_start = 0;
|
||||
uint32_t last_counter_idle_end = 0;
|
||||
uint32_t idle_counts = 0;
|
||||
uint32_t running_counts = 0;
|
||||
uint32_t counter_val = 0;
|
||||
ahrs_algorithm = AHRSSETTINGS_ALGORITHM_SIMPLE;
|
||||
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* Communication system */
|
||||
PIOS_COM_Init();
|
||||
|
||||
/* ADC system */
|
||||
AHRS_ADC_Config(adc_oversampling);
|
||||
|
||||
/* Setup the Accelerometer FS (Full-Scale) GPIO */
|
||||
PIOS_GPIO_Enable(0);
|
||||
SET_ACCEL_6G;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/* Magnetic sensor system */
|
||||
PIOS_I2C_Init();
|
||||
PIOS_HMC5843_Init();
|
||||
// Get 3 ID bytes
|
||||
strcpy((char *)mag_data.id, "ZZZ");
|
||||
PIOS_HMC5843_ReadID(mag_data.id);
|
||||
#endif
|
||||
|
||||
reset_values();
|
||||
|
||||
ahrs_state = AHRS_IDLE;
|
||||
AhrsInitComms();
|
||||
ahrs_state = AHRS_IDLE;
|
||||
while(!AhrsLinkReady()) {
|
||||
AhrsPoll();
|
||||
while(ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
downsample_data();
|
||||
ahrs_state = AHRS_IDLE;
|
||||
if((total_conversion_blocks % 10) == 0)
|
||||
PIOS_LED_Toggle(LED1);
|
||||
}
|
||||
/* we didn't connect the callbacks before because we have to wait
|
||||
for all data to be up to date before doing anything*/
|
||||
|
||||
AHRSCalibrationConnectCallback(calibration_callback);
|
||||
GPSPositionConnectCallback(gps_callback);
|
||||
BaroAltitudeConnectCallback(altitude_callback);
|
||||
AHRSSettingsConnectCallback(settings_callback);
|
||||
HomeLocationConnectCallback(homelocation_callback);
|
||||
|
||||
calibration_callback(AHRSCalibrationHandle()); //force an update
|
||||
|
||||
|
||||
/* Use simple averaging filter for now */
|
||||
for (int i = 0; i < adc_oversampling; i++)
|
||||
fir_coeffs[i] = 1;
|
||||
fir_coeffs[adc_oversampling] = adc_oversampling;
|
||||
|
||||
#ifdef DUMP_RAW
|
||||
while (1) {
|
||||
AhrsPoll();
|
||||
print_ahrs_raw();
|
||||
}
|
||||
#endif
|
||||
|
||||
timer_start();
|
||||
|
||||
/******************* Main EKF loop ****************************/
|
||||
while(1) {
|
||||
|
||||
|
||||
AhrsPoll();
|
||||
AhrsStatusData status;
|
||||
AhrsStatusGet(&status);
|
||||
status.CPULoad = ((float)running_counts /
|
||||
(float)(idle_counts + running_counts)) * 100;
|
||||
status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
|
||||
status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
|
||||
status.DroppedUpdates = ekf_too_slow;
|
||||
up_time = timer_count();
|
||||
if(up_time >= last_up_time) // normal condition
|
||||
up_time_real += ((up_time - last_up_time) * 1000) / timer_rate();
|
||||
else
|
||||
up_time_real += ((0xFFFF - last_up_time + up_time) * 1000) / timer_rate();
|
||||
last_up_time = up_time;
|
||||
status.RunningTime = up_time_real;
|
||||
AhrsStatusSet(&status);
|
||||
|
||||
// Alive signal
|
||||
if ((total_conversion_blocks % 100) == 0)
|
||||
PIOS_LED_Toggle(LED1);
|
||||
|
||||
// Delay for valid data
|
||||
|
||||
counter_val = timer_count();
|
||||
running_counts = counter_val - last_counter_idle_end;
|
||||
last_counter_idle_start = counter_val;
|
||||
|
||||
while (ahrs_state != AHRS_DATA_READY);
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
|
||||
counter_val = timer_count();
|
||||
idle_counts = counter_val - last_counter_idle_start;
|
||||
last_counter_idle_end = counter_val;
|
||||
|
||||
downsample_data();
|
||||
process_mag_data();
|
||||
|
||||
print_ekf_binary();
|
||||
|
||||
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
|
||||
if (ahrs_algorithm != last_ahrs_algorithm)
|
||||
ins_init_algorithm();
|
||||
last_ahrs_algorithm = ahrs_algorithm;
|
||||
|
||||
switch(ahrs_algorithm) {
|
||||
case AHRSSETTINGS_ALGORITHM_SIMPLE:
|
||||
simple_update();
|
||||
break;
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
|
||||
ins_outdoor_update();
|
||||
break;
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
|
||||
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
|
||||
ins_indoor_update();
|
||||
break;
|
||||
}
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Downsample the analog data
|
||||
* @return none
|
||||
*
|
||||
* Tried to make as much of the filtering fixed point when possible. Need to account
|
||||
* for offset for each sample before the multiplication if filter not a boxcar. Could
|
||||
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
|
||||
* data structures @ref accel_data and @ref gyro_data.
|
||||
*
|
||||
* The accel_data values are converted into a coordinate system where X is forwards along
|
||||
* the fuselage, Y is along right the wing, and Z is down.
|
||||
*/
|
||||
void downsample_data()
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
// Get the Y data. Third byte in. Convert to m/s
|
||||
accel_data.filtered.y = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_data.filtered.y += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_data.filtered.y /= (float) fir_coeffs[adc_oversampling];
|
||||
accel_data.filtered.y = (accel_data.filtered.y * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1];
|
||||
|
||||
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
|
||||
accel_data.filtered.x = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_data.filtered.x += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_data.filtered.x /= (float) fir_coeffs[adc_oversampling];
|
||||
accel_data.filtered.x = (accel_data.filtered.x * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0];
|
||||
|
||||
// Get the Z data. Third byte in. Convert to m/s
|
||||
accel_data.filtered.z = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_data.filtered.z += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_data.filtered.z /= (float) fir_coeffs[adc_oversampling];
|
||||
accel_data.filtered.z = (accel_data.filtered.z * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2];
|
||||
|
||||
// Get the X gyro data. Seventh byte in. Convert to deg/s.
|
||||
gyro_data.filtered.x = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
gyro_data.filtered.x += valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
gyro_data.filtered.x /= fir_coeffs[adc_oversampling];
|
||||
gyro_data.filtered.x = (gyro_data.filtered.x * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
|
||||
|
||||
// Get the Y gyro data. Second byte in. Convert to deg/s.
|
||||
gyro_data.filtered.y = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
gyro_data.filtered.y += valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
gyro_data.filtered.y /= fir_coeffs[adc_oversampling];
|
||||
gyro_data.filtered.y = (gyro_data.filtered.y * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
|
||||
|
||||
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
|
||||
gyro_data.filtered.z = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
gyro_data.filtered.z += valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
gyro_data.filtered.z /= fir_coeffs[adc_oversampling];
|
||||
gyro_data.filtered.z = (gyro_data.filtered.z * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
|
||||
|
||||
AttitudeRawData raw;
|
||||
|
||||
raw.gyros[0] = valid_data_buffer[1];
|
||||
raw.gyros[1] = valid_data_buffer[3];
|
||||
raw.gyros[2] = valid_data_buffer[5];
|
||||
raw.gyrotemp[0] = valid_data_buffer[6];
|
||||
raw.gyrotemp[1] = valid_data_buffer[7];
|
||||
|
||||
raw.gyros_filtered[0] = (gyro_data.filtered.x - Nav.gyro_bias[0]) * 180 / M_PI;
|
||||
raw.gyros_filtered[1] = (gyro_data.filtered.y - Nav.gyro_bias[1]) * 180 / M_PI;
|
||||
raw.gyros_filtered[2] = (gyro_data.filtered.z - Nav.gyro_bias[2]) * 180 / M_PI;
|
||||
|
||||
raw.accels[0] = valid_data_buffer[2];
|
||||
raw.accels[1] = valid_data_buffer[0];
|
||||
raw.accels[2] = valid_data_buffer[4];
|
||||
|
||||
raw.accels_filtered[0] = accel_data.filtered.x;
|
||||
raw.accels_filtered[1] = accel_data.filtered.y;
|
||||
raw.accels_filtered[2] = accel_data.filtered.z;
|
||||
|
||||
raw.magnetometers[0] = mag_data.scaled.axis[0];
|
||||
raw.magnetometers[1] = mag_data.scaled.axis[1];
|
||||
raw.magnetometers[2] = mag_data.scaled.axis[2];
|
||||
|
||||
AttitudeRawSet(&raw);
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
/**
|
||||
* @brief Get the mag data from the I2C sensor and load into structure
|
||||
* @return none
|
||||
*
|
||||
* This function also considers if the home location is set and has a valid
|
||||
* magnetic field before updating the mag data to prevent data being used that
|
||||
* cannot be interpreted. In addition the mag data is not used for the first
|
||||
* five seconds to allow the filter to start to converge
|
||||
*/
|
||||
void process_mag_data()
|
||||
{
|
||||
// Get magnetic readings
|
||||
// For now don't use mags until the magnetic field is set AND until 5 seconds
|
||||
// after initialization otherwise it seems to have problems
|
||||
// TODO: Follow up this initialization issue
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
if (PIOS_HMC5843_NewDataAvailable() &&
|
||||
(home.Set == HOMELOCATION_SET_TRUE) &&
|
||||
((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0))) {
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
// Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[1] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[0] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
|
||||
// Only use if magnetic length reasonable
|
||||
float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2));
|
||||
if((Blen < INSGPS_MAGLEN * (1 + INSGPS_MAGTOL)) && (Blen > INSGPS_MAGLEN * (1 - INSGPS_MAGTOL)))
|
||||
mag_data.updated = 1;
|
||||
}
|
||||
}
|
||||
#else
|
||||
void process_mag_data() { }
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Assumes board is not moving computes biases and variances of sensors
|
||||
* @returns None
|
||||
*
|
||||
* All data is stored in global structures. This function should be called from OP when
|
||||
* aircraft is in stable state and then the data stored to SD card.
|
||||
*
|
||||
* After this function the bias for each sensor will be the mean value. This doesn't make
|
||||
* sense for the z accel so make sure 6 point calibration is also run and those values set
|
||||
* after these read.
|
||||
*/
|
||||
#define NBIAS 100
|
||||
#define NVAR 500
|
||||
void calibrate_sensors()
|
||||
{
|
||||
int i,j;
|
||||
float accel_bias[3] = {0, 0, 0};
|
||||
float gyro_bias[3] = {0, 0, 0};
|
||||
float mag_bias[3] = {0, 0, 0};
|
||||
|
||||
|
||||
for (i = 0, j = 0; i < NBIAS; i++) {
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
downsample_data();
|
||||
gyro_bias[0] += gyro_data.filtered.x / NBIAS;
|
||||
gyro_bias[1] += gyro_data.filtered.y / NBIAS;
|
||||
gyro_bias[2] += gyro_data.filtered.z / NBIAS;
|
||||
accel_bias[0] += accel_data.filtered.x / NBIAS;
|
||||
accel_bias[1] += accel_data.filtered.y / NBIAS;
|
||||
accel_bias[2] += accel_data.filtered.z / NBIAS;
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_bias[0] += mag_data.scaled.axis[0];
|
||||
mag_bias[1] += mag_data.scaled.axis[1];
|
||||
mag_bias[2] += mag_data.scaled.axis[2];
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
mag_bias[0] /= j;
|
||||
mag_bias[1] /= j;
|
||||
mag_bias[2] /= j;
|
||||
|
||||
gyro_data.calibration.variance[0] = 0;
|
||||
gyro_data.calibration.variance[1] = 0;
|
||||
gyro_data.calibration.variance[2] = 0;
|
||||
mag_data.calibration.variance[0] = 0;
|
||||
mag_data.calibration.variance[1] = 0;
|
||||
mag_data.calibration.variance[2] = 0;
|
||||
accel_data.calibration.variance[0] = 0;
|
||||
accel_data.calibration.variance[1] = 0;
|
||||
accel_data.calibration.variance[2] = 0;
|
||||
|
||||
for (i = 0, j = 0; j < NVAR; j++) {
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
downsample_data();
|
||||
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
|
||||
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
|
||||
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
|
||||
accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
|
||||
accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
|
||||
accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
|
||||
ahrs_state = AHRS_IDLE;
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
|
||||
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
|
||||
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
mag_data.calibration.variance[0] /= j;
|
||||
mag_data.calibration.variance[1] /= j;
|
||||
mag_data.calibration.variance[2] /= j;
|
||||
|
||||
gyro_data.calibration.bias[0] -= gyro_bias[0];
|
||||
gyro_data.calibration.bias[1] -= gyro_bias[1];
|
||||
gyro_data.calibration.bias[2] -= gyro_bias[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Populate fields with initial values
|
||||
*/
|
||||
void reset_values() {
|
||||
accel_data.calibration.scale[0] = 0.012;
|
||||
accel_data.calibration.scale[1] = 0.012;
|
||||
accel_data.calibration.scale[2] = -0.012;
|
||||
accel_data.calibration.bias[0] = 24;
|
||||
accel_data.calibration.bias[1] = 24;
|
||||
accel_data.calibration.bias[2] = -24;
|
||||
accel_data.calibration.variance[0] = 1e-4;
|
||||
accel_data.calibration.variance[1] = 1e-4;
|
||||
accel_data.calibration.variance[2] = 1e-4;
|
||||
gyro_data.calibration.scale[0] = -0.014;
|
||||
gyro_data.calibration.scale[1] = 0.014;
|
||||
gyro_data.calibration.scale[2] = -0.014;
|
||||
gyro_data.calibration.bias[0] = -24;
|
||||
gyro_data.calibration.bias[1] = -24;
|
||||
gyro_data.calibration.bias[2] = -24;
|
||||
gyro_data.calibration.variance[0] = 1;
|
||||
gyro_data.calibration.variance[1] = 1;
|
||||
gyro_data.calibration.variance[2] = 1;
|
||||
mag_data.calibration.scale[0] = 1;
|
||||
mag_data.calibration.scale[1] = 1;
|
||||
mag_data.calibration.scale[2] = 1;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.variance[0] = 1;
|
||||
mag_data.calibration.variance[1] = 1;
|
||||
mag_data.calibration.variance[2] = 1;
|
||||
}
|
||||
|
||||
|
||||
void send_attitude(void)
|
||||
{
|
||||
AttitudeActualData attitude;
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
attitude.q1 = attitude_data.quaternion.q1;
|
||||
attitude.q2 = attitude_data.quaternion.q2;
|
||||
attitude.q3 = attitude_data.quaternion.q3;
|
||||
attitude.q4 = attitude_data.quaternion.q4;
|
||||
float rpy[3];
|
||||
Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
|
||||
attitude.Roll = rpy[0] + settings.RollBias;
|
||||
attitude.Pitch = rpy[1] + settings.PitchBias;
|
||||
attitude.Yaw = rpy[2] + settings.YawBias;
|
||||
if(attitude.Yaw > 360)
|
||||
attitude.Yaw -= 360;
|
||||
AttitudeActualSet(&attitude);
|
||||
}
|
||||
|
||||
void send_velocity(void)
|
||||
{
|
||||
VelocityActualData velocityActual;
|
||||
VelocityActualGet(&velocityActual);
|
||||
|
||||
// convert into cm
|
||||
velocityActual.North = Nav.Vel[0] * 100;
|
||||
velocityActual.East = Nav.Vel[1] * 100;
|
||||
velocityActual.Down = Nav.Vel[2] * 100;
|
||||
|
||||
VelocityActualSet(&velocityActual);
|
||||
}
|
||||
|
||||
void send_position(void)
|
||||
{
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
// convert into cm
|
||||
positionActual.North = Nav.Pos[0] * 100;
|
||||
positionActual.East = Nav.Pos[1] * 100;
|
||||
positionActual.Down = Nav.Pos[2] * 100;
|
||||
|
||||
PositionActualSet(&positionActual);
|
||||
}
|
||||
|
||||
void send_calibration(void)
|
||||
{
|
||||
AHRSCalibrationData cal;
|
||||
AHRSCalibrationGet(&cal);
|
||||
for(int ct=0; ct<3; ct++)
|
||||
{
|
||||
cal.accel_var[ct] = accel_data.calibration.variance[ct];
|
||||
cal.gyro_bias[ct] = gyro_data.calibration.bias[ct];
|
||||
cal.gyro_var[ct] = gyro_data.calibration.variance[ct];
|
||||
cal.mag_var[ct] = mag_data.calibration.variance[ct];
|
||||
}
|
||||
cal.measure_var = AHRSCALIBRATION_MEASURE_VAR_SET;
|
||||
AHRSCalibrationSet(&cal);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief AHRS calibration callback
|
||||
*
|
||||
* Called when the OP board sets the calibration
|
||||
*/
|
||||
void calibration_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSCalibrationData cal;
|
||||
AHRSCalibrationGet(&cal);
|
||||
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
|
||||
for(int ct=0; ct<3; ct++)
|
||||
{
|
||||
accel_data.calibration.scale[ct] = cal.accel_scale[ct];
|
||||
accel_data.calibration.bias[ct] = cal.accel_bias[ct];
|
||||
accel_data.calibration.variance[ct] = cal.accel_var[ct];
|
||||
gyro_data.calibration.scale[ct] = cal.gyro_scale[ct];
|
||||
gyro_data.calibration.bias[ct] = cal.gyro_bias[ct];
|
||||
gyro_data.calibration.variance[ct] = cal.gyro_var[ct];
|
||||
mag_data.calibration.bias[ct] = cal.mag_bias[ct];
|
||||
mag_data.calibration.scale[ct] = cal.mag_scale[ct];
|
||||
mag_data.calibration.variance[ct] = cal.mag_var[ct];
|
||||
}
|
||||
// Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
|
||||
// one in code
|
||||
float mag_var[3] = {mag_data.calibration.variance[0] / INSGPS_MAGLEN / INSGPS_MAGLEN,
|
||||
mag_data.calibration.variance[1] / INSGPS_MAGLEN / INSGPS_MAGLEN,
|
||||
mag_data.calibration.variance[2] / INSGPS_MAGLEN / INSGPS_MAGLEN};
|
||||
INSSetMagVar(mag_var);
|
||||
INSSetAccelVar(accel_data.calibration.variance);
|
||||
INSSetGyroVar(gyro_data.calibration.variance);
|
||||
}else if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_MEASURE){
|
||||
calibrate_sensors();
|
||||
send_calibration();
|
||||
}
|
||||
|
||||
INSSetPosVelVar(cal.pos_var, cal.vel_var);
|
||||
|
||||
}
|
||||
|
||||
void gps_callback(AhrsObjHandle obj)
|
||||
{
|
||||
GPSPositionData pos;
|
||||
GPSPositionGet(&pos);
|
||||
HomeLocationData home;
|
||||
HomeLocationGet(&home);
|
||||
|
||||
// convert from cm back to meters
|
||||
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
|
||||
// put in local NED frame
|
||||
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
|
||||
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
|
||||
|
||||
gps_data.heading = pos.Heading;
|
||||
gps_data.groundspeed = pos.Groundspeed;
|
||||
gps_data.quality = 1; /* currently unused */
|
||||
gps_data.updated = true;
|
||||
|
||||
// if poor don't use this update
|
||||
if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
|
||||
(pos.Satellites < INSGPS_GPS_MINSAT) ||
|
||||
(pos.PDOP >= INSGPS_GPS_MINPDOP) ||
|
||||
(home.Set == FALSE) ||
|
||||
((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
|
||||
{
|
||||
gps_data.quality = 0;
|
||||
gps_data.updated = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void altitude_callback(AhrsObjHandle obj)
|
||||
{
|
||||
BaroAltitudeData alt;
|
||||
BaroAltitudeGet(&alt);
|
||||
altitude_data.altitude = alt.Altitude;
|
||||
altitude_data.updated = true;
|
||||
}
|
||||
|
||||
void settings_callback(AhrsObjHandle obj)
|
||||
{
|
||||
AHRSSettingsData settings;
|
||||
AHRSSettingsGet(&settings);
|
||||
|
||||
ahrs_algorithm = settings.Algorithm;
|
||||
|
||||
if(settings.Downsampling != adc_oversampling) {
|
||||
adc_oversampling = settings.Downsampling;
|
||||
if(adc_oversampling > MAX_OVERSAMPLING) {
|
||||
adc_oversampling = MAX_OVERSAMPLING;
|
||||
settings.Downsampling = MAX_OVERSAMPLING;
|
||||
AHRSSettingsSet(&settings);
|
||||
}
|
||||
AHRS_ADC_Config(adc_oversampling);
|
||||
|
||||
/* Use simple averaging filter for now */
|
||||
for (int i = 0; i < adc_oversampling; i++)
|
||||
fir_coeffs[i] = 1;
|
||||
fir_coeffs[adc_oversampling] = adc_oversampling;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void homelocation_callback(AhrsObjHandle obj)
|
||||
{
|
||||
HomeLocationData data;
|
||||
HomeLocationGet(&data);
|
||||
|
||||
float Bmag = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
|
||||
float Be[3] = {data.Be[0] / Bmag, data.Be[1] / Bmag, data.Be[2] / Bmag};
|
||||
|
||||
INSSetMagNorth(Be);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
90
flight/AHRS/inc/insgps16state.h
Normal file
90
flight/AHRS/inc/insgps16state.h
Normal file
@ -0,0 +1,90 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS
|
||||
* @{
|
||||
* @addtogroup INSGPS
|
||||
* @{
|
||||
* @brief INSGPS is a joint attitude and position estimation EKF
|
||||
*
|
||||
* @file insgps.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the INSGPS exposed functionality.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef INSGPS_H_
|
||||
#define INSGPS_H_
|
||||
|
||||
#include "stdint.h"
|
||||
|
||||
/**
|
||||
* @addtogroup Constants
|
||||
* @{
|
||||
*/
|
||||
#define POS_SENSORS 0x007
|
||||
#define HORIZ_SENSORS 0x018
|
||||
#define VERT_SENSORS 0x020
|
||||
#define MAG_SENSORS 0x1C0
|
||||
#define BARO_SENSOR 0x200
|
||||
|
||||
#define FULL_SENSORS 0x3FF
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
// Exposed Function Prototypes
|
||||
void INSGPSInit();
|
||||
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
|
||||
void INSCovariancePrediction(float dT);
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
|
||||
|
||||
void INSResetP(float PDiag[13]);
|
||||
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
|
||||
void INSSetPosVelVar(float PosVar, float VelVar);
|
||||
void INSSetGyroBias(float gyro_bias[3]);
|
||||
void INSSetAccelVar(float accel_var[3]);
|
||||
void INSSetGyroVar(float gyro_var[3]);
|
||||
void INSSetMagNorth(float B[3]);
|
||||
void INSSetMagVar(float scaled_mag_var[3]);
|
||||
void INSPosVelReset(float pos[3], float vel[3]);
|
||||
|
||||
void MagCorrection(float mag_data[3]);
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt);
|
||||
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
|
||||
void VelBaroCorrection(float Vel[3], float BaroAlt);
|
||||
|
||||
// Nav structure containing current solution
|
||||
struct NavStruct {
|
||||
float Pos[3]; // Position in meters and relative to a local NED frame
|
||||
float Vel[3]; // Velocity in meters and in NED
|
||||
float q[4]; // unit quaternion rotation relative to NED
|
||||
float gyro_bias[3];
|
||||
} Nav;
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
#endif /* EKF_H_ */
|
||||
854
flight/AHRS/insgps16state.c
Normal file
854
flight/AHRS/insgps16state.c
Normal file
@ -0,0 +1,854 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS
|
||||
* @{
|
||||
* @addtogroup INSGPS
|
||||
* @{
|
||||
* @brief INSGPS is a joint attitude and position estimation EKF
|
||||
*
|
||||
* @file insgps.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief An INS/GPS algorithm implemented with an EKF.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "insgps.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// constants/macros/typdefs
|
||||
#define NUMX 16 // number of states, X is the state vector
|
||||
#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
|
||||
#define NUMV 10 // number of measurements, v is the measurement noise vector
|
||||
#define NUMU 6 // number of deterministic inputs, U is the input vector
|
||||
|
||||
#if defined(GENERAL_COV)
|
||||
// This might trick people so I have a note here. There is a slower but bigger version of the
|
||||
// code here but won't fit when debugging disabled (requires -Os)
|
||||
#define COVARIANCE_PREDICTION_GENERAL
|
||||
#endif
|
||||
|
||||
// Private functions
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX]);
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed);
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW]);
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
|
||||
|
||||
// Private variables
|
||||
float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
|
||||
// global to init to zero and maintain zero elements
|
||||
float Be[3]; // local magnetic unit vector in NED frame
|
||||
float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
|
||||
float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
float K[NUMX][NUMV]; // feedback gain matrix
|
||||
|
||||
// ************* Exposed Functions ****************
|
||||
// *************************************************
|
||||
void INSGPSInit() //pretty much just a place holder for now
|
||||
{
|
||||
Be[0] = 1;
|
||||
Be[1] = 0;
|
||||
Be[2] = 0; // local magnetic unit vector
|
||||
|
||||
for (int i = 0; i < NUMX; i++) {
|
||||
for (int j = 0; j < NUMX; j++) {
|
||||
P[i][j] = 0; // zero all terms
|
||||
}
|
||||
}
|
||||
|
||||
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
|
||||
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
|
||||
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
|
||||
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
|
||||
|
||||
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
|
||||
X[6] = 1;
|
||||
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
|
||||
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
|
||||
|
||||
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
|
||||
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
|
||||
Q[6] = Q[7] = Q[8] = 2e-14; // gyro bias random walk variance (rad/s^2)^2
|
||||
Q[9] = Q[10] = Q[11] = 2e-13; // accel bias random walk variance (m/s^3)^2
|
||||
|
||||
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
|
||||
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
|
||||
R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
|
||||
R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
|
||||
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
|
||||
R[9] = .05; // High freq altimeter noise variance (m^2)
|
||||
}
|
||||
|
||||
void INSResetP(float PDiag[NUMX])
|
||||
{
|
||||
uint8_t i,j;
|
||||
|
||||
// if PDiag[i] nonzero then clear row and column and set diagonal element
|
||||
for (i=0;i<NUMX;i++){
|
||||
if (PDiag != 0){
|
||||
for (j=0;j<NUMX;j++)
|
||||
P[i][j]=P[j][i]=0;
|
||||
P[i][i]=PDiag[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
|
||||
{
|
||||
Nav.Pos[0] = X[0] = pos[0];
|
||||
Nav.Pos[1] = X[1] = pos[1];
|
||||
Nav.Pos[2] = X[2] = pos[2];
|
||||
Nav.Vel[0] = X[3] = vel[0];
|
||||
Nav.Vel[1] = X[4] = vel[1];
|
||||
Nav.Vel[2] = X[5] = vel[2];
|
||||
Nav.q[0] = X[6] = q[0];
|
||||
Nav.q[1] = X[7] = q[1];
|
||||
Nav.q[2] = X[8] = q[2];
|
||||
Nav.q[3] = X[9] = q[3];
|
||||
Nav.gyro_bias[0] = X[10] = gyro_bias[0];
|
||||
Nav.gyro_bias[1] = X[11] = gyro_bias[1];
|
||||
Nav.gyro_bias[2] = X[12] = gyro_bias[2];
|
||||
X[13] = accel_bias[0];
|
||||
X[14] = accel_bias[1];
|
||||
X[15] = accel_bias[2];
|
||||
}
|
||||
|
||||
void INSPosVelReset(float pos[3], float vel[3])
|
||||
{
|
||||
for (int i = 0; i < 6; i++) {
|
||||
for(int j = i; j < NUMX; j++) {
|
||||
P[i][j] = 0; // zero the first 6 rows and columns
|
||||
P[j][i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
|
||||
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
|
||||
|
||||
X[0] = pos[0];
|
||||
X[1] = pos[1];
|
||||
X[2] = pos[2];
|
||||
X[3] = vel[0];
|
||||
X[4] = vel[1];
|
||||
X[5] = vel[2];
|
||||
}
|
||||
|
||||
void INSSetPosVelVar(float PosVar, float VelVar)
|
||||
{
|
||||
R[0] = PosVar;
|
||||
R[1] = PosVar;
|
||||
R[2] = PosVar;
|
||||
R[3] = VelVar;
|
||||
R[4] = VelVar;
|
||||
// R[5] = PosVar; // Don't change vertical velocity, not measured
|
||||
}
|
||||
|
||||
void INSSetGyroBias(float gyro_bias[3])
|
||||
{
|
||||
X[10] = gyro_bias[0];
|
||||
X[11] = gyro_bias[1];
|
||||
X[12] = gyro_bias[2];
|
||||
}
|
||||
|
||||
void INSSetAccelVar(float accel_var[3])
|
||||
{
|
||||
Q[3] = accel_var[0];
|
||||
Q[4] = accel_var[1];
|
||||
Q[5] = accel_var[2];
|
||||
}
|
||||
|
||||
void INSSetGyroVar(float gyro_var[3])
|
||||
{
|
||||
Q[0] = gyro_var[0];
|
||||
Q[1] = gyro_var[1];
|
||||
Q[2] = gyro_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagVar(float scaled_mag_var[3])
|
||||
{
|
||||
R[6] = scaled_mag_var[0];
|
||||
R[7] = scaled_mag_var[1];
|
||||
R[8] = scaled_mag_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagNorth(float B[3])
|
||||
{
|
||||
Be[0] = B[0];
|
||||
Be[1] = B[1];
|
||||
Be[2] = B[2];
|
||||
}
|
||||
|
||||
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
{
|
||||
float U[6];
|
||||
float qmag;
|
||||
|
||||
// rate gyro inputs in units of rad/s
|
||||
U[0] = gyro_data[0];
|
||||
U[1] = gyro_data[1];
|
||||
U[2] = gyro_data[2];
|
||||
|
||||
// accelerometer inputs in units of m/s
|
||||
U[3] = accel_data[0];
|
||||
U[4] = accel_data[1];
|
||||
U[5] = accel_data[2];
|
||||
|
||||
// EKF prediction step
|
||||
LinearizeFG(X, U, F, G);
|
||||
RungeKutta(X, U, dT);
|
||||
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
|
||||
X[6] /= qmag;
|
||||
X[7] /= qmag;
|
||||
X[8] /= qmag;
|
||||
X[9] /= qmag;
|
||||
//CovariancePrediction(F,G,Q,dT,P);
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
Nav.gyro_bias[0] = X[10];
|
||||
Nav.gyro_bias[1] = X[11];
|
||||
Nav.gyro_bias[2] = X[12];
|
||||
}
|
||||
|
||||
void INSCovariancePrediction(float dT)
|
||||
{
|
||||
CovariancePrediction(F, G, Q, dT, P);
|
||||
}
|
||||
|
||||
float zeros[3] = { 0, 0, 0 };
|
||||
|
||||
void MagCorrection(float mag_data[3])
|
||||
{
|
||||
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
|
||||
}
|
||||
|
||||
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, zeros, Vel, BaroAlt,
|
||||
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
|
||||
BARO_SENSOR);
|
||||
}
|
||||
|
||||
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, Pos, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt)
|
||||
{
|
||||
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
|
||||
}
|
||||
|
||||
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
|
||||
{
|
||||
INSCorrection(mag_data, Pos, Vel, zeros[0],
|
||||
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
|
||||
}
|
||||
|
||||
void VelBaroCorrection(float Vel[3], float BaroAlt)
|
||||
{
|
||||
INSCorrection(zeros, zeros, Vel, BaroAlt,
|
||||
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
|
||||
}
|
||||
|
||||
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
|
||||
float BaroAlt, uint16_t SensorsUsed)
|
||||
{
|
||||
float Z[10], Y[10];
|
||||
float Bmag, qmag;
|
||||
|
||||
// GPS Position in meters and in local NED frame
|
||||
Z[0] = Pos[0];
|
||||
Z[1] = Pos[1];
|
||||
Z[2] = Pos[2];
|
||||
|
||||
// GPS Velocity in meters and in local NED frame
|
||||
Z[3] = Vel[0];
|
||||
Z[4] = Vel[1];
|
||||
Z[5] = Vel[2];
|
||||
|
||||
// magnetometer data in any units (use unit vector) and in body frame
|
||||
Bmag =
|
||||
sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
|
||||
mag_data[2] * mag_data[2]);
|
||||
Z[6] = mag_data[0] / Bmag;
|
||||
Z[7] = mag_data[1] / Bmag;
|
||||
Z[8] = mag_data[2] / Bmag;
|
||||
|
||||
// barometric altimeter in meters and in local NED frame
|
||||
Z[9] = BaroAlt;
|
||||
|
||||
// EKF correction step
|
||||
LinearizeH(X, Be, H);
|
||||
MeasurementEq(X, Be, Y);
|
||||
SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
|
||||
qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
|
||||
X[6] /= qmag;
|
||||
X[7] /= qmag;
|
||||
X[8] /= qmag;
|
||||
X[9] /= qmag;
|
||||
|
||||
// Update Nav solution structure
|
||||
Nav.Pos[0] = X[0];
|
||||
Nav.Pos[1] = X[1];
|
||||
Nav.Pos[2] = X[2];
|
||||
Nav.Vel[0] = X[3];
|
||||
Nav.Vel[1] = X[4];
|
||||
Nav.Vel[2] = X[5];
|
||||
Nav.q[0] = X[6];
|
||||
Nav.q[1] = X[7];
|
||||
Nav.q[2] = X[8];
|
||||
Nav.q[3] = X[9];
|
||||
}
|
||||
|
||||
// ************* CovariancePrediction *************
|
||||
// Does the prediction step of the Kalman filter for the covariance matrix
|
||||
// Output, Pnew, overwrites P, the input covariance
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
|
||||
// Q is the discrete time covariance of process noise
|
||||
// Q is vector of the diagonal for a square matrix with
|
||||
// dimensions equal to the number of disturbance noise variables
|
||||
// The General Method is very inefficient,not taking advantage of the sparse F and G
|
||||
// The first Method is very specific to this implementation
|
||||
// ************************************************
|
||||
|
||||
#ifdef COVARIANCE_PREDICTION_GENERAL
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float Dummy[NUMX][NUMX], dTsq;
|
||||
uint8_t i, j, k;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
|
||||
|
||||
dTsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) // Calculate Dummy = (P/T +F*P)
|
||||
for (j = 0; j < NUMX; j++) {
|
||||
Dummy[i][j] = P[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
Dummy[i][j] += F[i][k] * P[k][j];
|
||||
}
|
||||
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
|
||||
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
|
||||
P[i][j] = Dummy[i][j] / dT;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
|
||||
for (k = 0; k < NUMW; k++)
|
||||
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
|
||||
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
|
||||
float Q[NUMW], float dT, float P[NUMX][NUMX])
|
||||
{
|
||||
float D[NUMX][NUMX], T, Tsq;
|
||||
uint8_t i, j;
|
||||
|
||||
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
|
||||
|
||||
T = dT;
|
||||
Tsq = dT * dT;
|
||||
|
||||
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
|
||||
for (j = i; j < NUMX; j++)
|
||||
D[i][j] = P[i][j];
|
||||
|
||||
// Brute force calculation of the elements of P
|
||||
P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
|
||||
P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
|
||||
P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
|
||||
P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9] + F[3][13]*D[3][13] + F[3][14]*D[3][14] + F[3][15]*D[3][15])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9] + F[3][13]*D[0][13] + F[3][14]*D[0][14] + F[3][15]*D[0][15])*T + D[0][3];
|
||||
P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9] + F[4][13]*D[3][13] + F[4][14]*D[3][14] + F[4][15]*D[3][15])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9] + F[4][13]*D[0][13] + F[4][14]*D[0][14] + F[4][15]*D[0][15])*T + D[0][4];
|
||||
P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9] + F[5][13]*D[3][13] + F[5][14]*D[3][14] + F[5][15]*D[3][15])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9] + F[5][13]*D[0][13] + F[5][14]*D[0][14] + F[5][15]*D[0][15])*T + D[0][5];
|
||||
P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
|
||||
P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
|
||||
P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
|
||||
P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
|
||||
P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
|
||||
P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
|
||||
P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
|
||||
P[0][13] = P[13][0] = D[3][13]*T + D[0][13];
|
||||
P[0][14] = P[14][0] = D[3][14]*T + D[0][14];
|
||||
P[0][15] = P[15][0] = D[3][15]*T + D[0][15];
|
||||
P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
|
||||
P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
|
||||
P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9] + F[3][13]*D[4][13] + F[3][14]*D[4][14] + F[3][15]*D[4][15])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9] + F[3][13]*D[1][13] + F[3][14]*D[1][14] + F[3][15]*D[1][15])*T + D[1][3];
|
||||
P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9] + F[4][13]*D[4][13] + F[4][14]*D[4][14] + F[4][15]*D[4][15])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9] + F[4][13]*D[1][13] + F[4][14]*D[1][14] + F[4][15]*D[1][15])*T + D[1][4];
|
||||
P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9] + F[5][13]*D[4][13] + F[5][14]*D[4][14] + F[5][15]*D[4][15])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9] + F[5][13]*D[1][13] + F[5][14]*D[1][14] + F[5][15]*D[1][15])*T + D[1][5];
|
||||
P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
|
||||
P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
|
||||
P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
|
||||
P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
|
||||
P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
|
||||
P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
|
||||
P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
|
||||
P[1][13] = P[13][1] = D[4][13]*T + D[1][13];
|
||||
P[1][14] = P[14][1] = D[4][14]*T + D[1][14];
|
||||
P[1][15] = P[15][1] = D[4][15]*T + D[1][15];
|
||||
P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
|
||||
P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9] + F[3][13]*D[5][13] + F[3][14]*D[5][14] + F[3][15]*D[5][15])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9] + F[3][13]*D[2][13] + F[3][14]*D[2][14] + F[3][15]*D[2][15])*T + D[2][3];
|
||||
P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9] + F[4][13]*D[5][13] + F[4][14]*D[5][14] + F[4][15]*D[5][15])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9] + F[4][13]*D[2][13] + F[4][14]*D[2][14] + F[4][15]*D[2][15])*T + D[2][4];
|
||||
P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9] + F[5][13]*D[5][13] + F[5][14]*D[5][14] + F[5][15]*D[5][15])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9] + F[5][13]*D[2][13] + F[5][14]*D[2][14] + F[5][15]*D[2][15])*T + D[2][5];
|
||||
P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
|
||||
P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
|
||||
P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
|
||||
P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
|
||||
P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
|
||||
P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
|
||||
P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
|
||||
P[2][13] = P[13][2] = D[5][13]*T + D[2][13];
|
||||
P[2][14] = P[14][2] = D[5][14]*T + D[2][14];
|
||||
P[2][15] = P[15][2] = D[5][15]*T + D[2][15];
|
||||
P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[3][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[3][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[3][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9] + 2*F[3][13]*D[3][13] + 2*F[3][14]*D[3][14] + 2*F[3][15]*D[3][15])*T + D[3][3];
|
||||
P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[4][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[4][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[4][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9] + F[3][13]*D[4][13] + F[4][13]*D[3][13] + F[3][14]*D[4][14] + F[4][14]*D[3][14] + F[3][15]*D[4][15] + F[4][15]*D[3][15])*T + D[3][4];
|
||||
P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[5][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[5][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[5][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9] + F[3][13]*D[5][13] + F[5][13]*D[3][13] + F[3][14]*D[5][14] + F[5][14]*D[3][14] + F[3][15]*D[5][15] + F[5][15]*D[3][15])*T + D[3][5];
|
||||
P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15])*T + D[3][6];
|
||||
P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15])*T + D[3][7];
|
||||
P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15])*T + D[3][8];
|
||||
P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
|
||||
P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
|
||||
P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
|
||||
P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
|
||||
P[3][13] = P[13][3] = (F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13])*T + D[3][13];
|
||||
P[3][14] = P[14][3] = (F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14])*T + D[3][14];
|
||||
P[3][15] = P[15][3] = (F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15])*T + D[3][15];
|
||||
P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[4][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[4][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[4][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9] + 2*F[4][13]*D[4][13] + 2*F[4][14]*D[4][14] + 2*F[4][15]*D[4][15])*T + D[4][4];
|
||||
P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[5][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[5][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[5][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9] + F[4][13]*D[5][13] + F[5][13]*D[4][13] + F[4][14]*D[5][14] + F[5][14]*D[4][14] + F[4][15]*D[5][15] + F[5][15]*D[4][15])*T + D[4][5];
|
||||
P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15])*T + D[4][6];
|
||||
P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15])*T + D[4][7];
|
||||
P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15])*T + D[4][8];
|
||||
P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
|
||||
P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
|
||||
P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
|
||||
P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
|
||||
P[4][13] = P[13][4] = (F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13])*T + D[4][13];
|
||||
P[4][14] = P[14][4] = (F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14])*T + D[4][14];
|
||||
P[4][15] = P[15][4] = (F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15])*T + D[4][15];
|
||||
P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[5][13]*(F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13]) + F[5][14]*(F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14]) + F[5][15]*(F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15]) + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9] + 2*F[5][13]*D[5][13] + 2*F[5][14]*D[5][14] + 2*F[5][15]*D[5][15])*T + D[5][5];
|
||||
P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15])*T + D[5][6];
|
||||
P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15])*T + D[5][7];
|
||||
P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15])*T + D[5][8];
|
||||
P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
|
||||
P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
|
||||
P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
|
||||
P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
|
||||
P[5][13] = P[13][5] = (F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13])*T + D[5][13];
|
||||
P[5][14] = P[14][5] = (F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14])*T + D[5][14];
|
||||
P[5][15] = P[15][5] = (F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15])*T + D[5][15];
|
||||
P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
|
||||
P[6][7] = P[7][6] = (F[7][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
|
||||
P[6][8] = P[8][6] = (F[8][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[8][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[8][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[8][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
|
||||
P[6][9] = P[9][6] = (F[9][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
|
||||
P[6][10] = P[10][6] = (F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
|
||||
P[6][11] = P[11][6] = (F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
|
||||
P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
|
||||
P[6][13] = P[13][6] = (F[6][9]*D[9][13] + F[6][10]*D[10][13] + F[6][11]*D[11][13] + F[6][12]*D[12][13] + F[6][7]*D[7][13] + F[6][8]*D[8][13])*T + D[6][13];
|
||||
P[6][14] = P[14][6] = (F[6][9]*D[9][14] + F[6][10]*D[10][14] + F[6][11]*D[11][14] + F[6][12]*D[12][14] + F[6][7]*D[7][14] + F[6][8]*D[8][14])*T + D[6][14];
|
||||
P[6][15] = P[15][6] = (F[6][9]*D[9][15] + F[6][10]*D[10][15] + F[6][11]*D[11][15] + F[6][12]*D[12][15] + F[6][7]*D[7][15] + F[6][8]*D[8][15])*T + D[6][15];
|
||||
P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
|
||||
P[7][8] = P[8][7] = (F[8][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[8][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[8][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[8][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
|
||||
P[7][9] = P[9][7] = (F[9][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
|
||||
P[7][10] = P[10][7] = (F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
|
||||
P[7][11] = P[11][7] = (F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
|
||||
P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
|
||||
P[7][13] = P[13][7] = (F[7][9]*D[9][13] + F[7][10]*D[10][13] + F[7][11]*D[11][13] + F[7][12]*D[12][13] + F[7][6]*D[6][13] + F[7][8]*D[8][13])*T + D[7][13];
|
||||
P[7][14] = P[14][7] = (F[7][9]*D[9][14] + F[7][10]*D[10][14] + F[7][11]*D[11][14] + F[7][12]*D[12][14] + F[7][6]*D[6][14] + F[7][8]*D[8][14])*T + D[7][14];
|
||||
P[7][15] = P[15][7] = (F[7][9]*D[9][15] + F[7][10]*D[10][15] + F[7][11]*D[11][15] + F[7][12]*D[12][15] + F[7][6]*D[6][15] + F[7][8]*D[8][15])*T + D[7][15];
|
||||
P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][9]*(F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
|
||||
P[8][9] = P[9][8] = (F[9][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
|
||||
P[8][10] = P[10][8] = (F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
|
||||
P[8][11] = P[11][8] = (F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
|
||||
P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
|
||||
P[8][13] = P[13][8] = (F[8][9]*D[9][13] + F[8][10]*D[10][13] + F[8][11]*D[11][13] + F[8][12]*D[12][13] + F[8][6]*D[6][13] + F[8][7]*D[7][13])*T + D[8][13];
|
||||
P[8][14] = P[14][8] = (F[8][9]*D[9][14] + F[8][10]*D[10][14] + F[8][11]*D[11][14] + F[8][12]*D[12][14] + F[8][6]*D[6][14] + F[8][7]*D[7][14])*T + D[8][14];
|
||||
P[8][15] = P[15][8] = (F[8][9]*D[9][15] + F[8][10]*D[10][15] + F[8][11]*D[11][15] + F[8][12]*D[12][15] + F[8][6]*D[6][15] + F[8][7]*D[7][15])*T + D[8][15];
|
||||
P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
|
||||
P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
|
||||
P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
|
||||
P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
|
||||
P[9][13] = P[13][9] = (F[9][10]*D[10][13] + F[9][11]*D[11][13] + F[9][12]*D[12][13] + F[9][6]*D[6][13] + F[9][7]*D[7][13] + F[9][8]*D[8][13])*T + D[9][13];
|
||||
P[9][14] = P[14][9] = (F[9][10]*D[10][14] + F[9][11]*D[11][14] + F[9][12]*D[12][14] + F[9][6]*D[6][14] + F[9][7]*D[7][14] + F[9][8]*D[8][14])*T + D[9][14];
|
||||
P[9][15] = P[15][9] = (F[9][10]*D[10][15] + F[9][11]*D[11][15] + F[9][12]*D[12][15] + F[9][6]*D[6][15] + F[9][7]*D[7][15] + F[9][8]*D[8][15])*T + D[9][15];
|
||||
P[10][10] = Q[6]*Tsq + D[10][10];
|
||||
P[10][11] = P[11][10] = D[10][11];
|
||||
P[10][12] = P[12][10] = D[10][12];
|
||||
P[10][13] = P[13][10] = D[10][13];
|
||||
P[10][14] = P[14][10] = D[10][14];
|
||||
P[10][15] = P[15][10] = D[10][15];
|
||||
P[11][11] = Q[7]*Tsq + D[11][11];
|
||||
P[11][12] = P[12][11] = D[11][12];
|
||||
P[11][13] = P[13][11] = D[11][13];
|
||||
P[11][14] = P[14][11] = D[11][14];
|
||||
P[11][15] = P[15][11] = D[11][15];
|
||||
P[12][12] = Q[8]*Tsq + D[12][12];
|
||||
P[12][13] = P[13][12] = D[12][13];
|
||||
P[12][14] = P[14][12] = D[12][14];
|
||||
P[12][15] = P[15][12] = D[12][15];
|
||||
P[13][13] = Q[9]*Tsq + D[13][13];
|
||||
P[13][14] = P[14][13] = D[13][14];
|
||||
P[13][15] = P[15][13] = D[13][15];
|
||||
P[14][14] = Q[10]*Tsq + D[14][14];
|
||||
P[14][15] = P[15][14] = D[14][15];
|
||||
P[15][15] = Q[11]*Tsq + D[14][15];
|
||||
}
|
||||
#endif
|
||||
|
||||
// ************* SerialUpdate *******************
|
||||
// Does the update step of the Kalman filter for the covariance and estimate
|
||||
// Outputs are Xnew & Pnew, and are written over P and X
|
||||
// Z is actual measurement, Y is predicted measurement
|
||||
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
|
||||
// where K=P*H'*inv[H*P*H'+R]
|
||||
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
|
||||
// i.e. the measurment noises are uncorrelated.
|
||||
// It therefore uses a serial update that requires no matrix inversion by
|
||||
// processing the measurements one at a time.
|
||||
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
|
||||
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
|
||||
// The SensorsUsed variable is a bitwise mask indicating which sensors
|
||||
// should be used in the update.
|
||||
// ************************************************
|
||||
|
||||
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
|
||||
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
|
||||
uint16_t SensorsUsed)
|
||||
{
|
||||
float HP[NUMX], HPHR, Error;
|
||||
uint8_t i, j, k, m;
|
||||
|
||||
for (m = 0; m < NUMV; m++) {
|
||||
|
||||
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
|
||||
|
||||
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
|
||||
HP[j] = 0;
|
||||
for (k = 0; k < NUMX; k++)
|
||||
HP[j] += H[m][k] * P[k][j];
|
||||
}
|
||||
HPHR = R[m]; // Find HPHR = H*P*H' + R
|
||||
for (k = 0; k < NUMX; k++)
|
||||
HPHR += HP[k] * H[m][k];
|
||||
|
||||
for (k = 0; k < NUMX; k++)
|
||||
K[k][m] = HP[k] / HPHR; // find K = HP/HPHR
|
||||
|
||||
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
|
||||
for (j = i; j < NUMX; j++)
|
||||
P[i][j] = P[j][i] =
|
||||
P[i][j] - K[i][m] * HP[j];
|
||||
}
|
||||
|
||||
Error = Z[m] - Y[m];
|
||||
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
|
||||
X[i] = X[i] + K[i][m] * Error;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ************* RungeKutta **********************
|
||||
// Does a 4th order Runge Kutta numerical integration step
|
||||
// Output, Xnew, is written over X
|
||||
// NOTE the algorithm assumes time invariant state equations and
|
||||
// constant inputs over integration step
|
||||
// ************************************************
|
||||
|
||||
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
|
||||
{
|
||||
|
||||
float dT2 =
|
||||
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NUMX; i++)
|
||||
Xlast[i] = X[i]; // make a working copy
|
||||
|
||||
StateEq(X, U, K1); // k1 = f(x,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT2 * K1[i];
|
||||
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT2 * K2[i];
|
||||
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] = Xlast[i] + dT * K3[i];
|
||||
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
|
||||
|
||||
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
|
||||
for (i = 0; i < NUMX; i++)
|
||||
X[i] =
|
||||
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
|
||||
K4[i]) / 6;
|
||||
}
|
||||
|
||||
// ************* Model Specific Stuff ***************************
|
||||
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
|
||||
//
|
||||
// State Variables = [Pos Vel Quaternion GyroBias AccelBias]
|
||||
// Deterministic Inputs = [AngularVel Accel]
|
||||
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise]
|
||||
//
|
||||
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
|
||||
// Inputs to Measurement = [EarthFrameMagField]
|
||||
//
|
||||
// Notes: Pos and Vel in earth frame
|
||||
// AngularVel and Accel in body frame
|
||||
// MagFields are unit vectors
|
||||
// Xdot is output of StateEq()
|
||||
// F and G are outputs of LinearizeFG(), all elements not set should be zero
|
||||
// y is output of OutputEq()
|
||||
// H is output of LinearizeH(), all elements not set should be zero
|
||||
// ************************************************
|
||||
|
||||
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
ax=U[3]-X[13];
|
||||
ay=U[4]-X[14];
|
||||
az=U[5]-X[15]; // subtract the biases on accels
|
||||
wx = U[0] - X[10];
|
||||
wy = U[1] - X[11];
|
||||
wz = U[2] - X[12]; // subtract the biases on gyros
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// Pdot = V
|
||||
Xdot[0] = X[3];
|
||||
Xdot[1] = X[4];
|
||||
Xdot[2] = X[5];
|
||||
|
||||
// Vdot = Reb*a
|
||||
Xdot[3] =
|
||||
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
|
||||
q0 * q3) *
|
||||
ay + 2 * (q1 * q3 + q0 * q2) * az;
|
||||
Xdot[4] =
|
||||
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
|
||||
q3 * q3) * ay + 2 * (q2 * q3 -
|
||||
q0 * q1) *
|
||||
az;
|
||||
Xdot[5] =
|
||||
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
|
||||
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
|
||||
|
||||
// qdot = Q*w
|
||||
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
|
||||
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
|
||||
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
|
||||
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
|
||||
|
||||
// best guess is that bias stays constant
|
||||
Xdot[10] = Xdot[11] = Xdot[12] = 0;
|
||||
Xdot[13] = Xdot[14] = Xdot[15] = 0;
|
||||
}
|
||||
|
||||
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
|
||||
float G[NUMX][NUMW])
|
||||
{
|
||||
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
|
||||
|
||||
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
|
||||
ax = U[3];
|
||||
ay = U[4];
|
||||
az = U[5]; // NO BIAS STATES ON ACCELS
|
||||
wx = U[0] - X[10];
|
||||
wy = U[1] - X[11];
|
||||
wz = U[2] - X[12]; // subtract the biases on gyros
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// Pdot = V
|
||||
F[0][3] = F[1][4] = F[2][5] = 1;
|
||||
|
||||
// dVdot/dq
|
||||
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
|
||||
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
|
||||
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
|
||||
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
|
||||
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
|
||||
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
|
||||
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
|
||||
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
|
||||
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
|
||||
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
|
||||
|
||||
// dVdot/dabias & dVdot/dna
|
||||
F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
|
||||
F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
|
||||
F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
|
||||
|
||||
// dqdot/dq
|
||||
F[6][6] = 0;
|
||||
F[6][7] = -wx / 2;
|
||||
F[6][8] = -wy / 2;
|
||||
F[6][9] = -wz / 2;
|
||||
F[7][6] = wx / 2;
|
||||
F[7][7] = 0;
|
||||
F[7][8] = wz / 2;
|
||||
F[7][9] = -wy / 2;
|
||||
F[8][6] = wy / 2;
|
||||
F[8][7] = -wz / 2;
|
||||
F[8][8] = 0;
|
||||
F[8][9] = wx / 2;
|
||||
F[9][6] = wz / 2;
|
||||
F[9][7] = wy / 2;
|
||||
F[9][8] = -wx / 2;
|
||||
F[9][9] = 0;
|
||||
|
||||
// dqdot/dwbias
|
||||
F[6][10] = q1 / 2;
|
||||
F[6][11] = q2 / 2;
|
||||
F[6][12] = q3 / 2;
|
||||
F[7][10] = -q0 / 2;
|
||||
F[7][11] = q3 / 2;
|
||||
F[7][12] = -q2 / 2;
|
||||
F[8][10] = -q3 / 2;
|
||||
F[8][11] = -q0 / 2;
|
||||
F[8][12] = q1 / 2;
|
||||
F[9][10] = q2 / 2;
|
||||
F[9][11] = -q1 / 2;
|
||||
F[9][12] = -q0 / 2;
|
||||
|
||||
// dVdot/dna - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
|
||||
//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
|
||||
//G[4][3]=-2*(q1*q2+q0*q3); G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
|
||||
//G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
|
||||
|
||||
// dqdot/dnw
|
||||
G[6][0] = q1 / 2;
|
||||
G[6][1] = q2 / 2;
|
||||
G[6][2] = q3 / 2;
|
||||
G[7][0] = -q0 / 2;
|
||||
G[7][1] = q3 / 2;
|
||||
G[7][2] = -q2 / 2;
|
||||
G[8][0] = -q3 / 2;
|
||||
G[8][1] = -q0 / 2;
|
||||
G[8][2] = q1 / 2;
|
||||
G[9][0] = q2 / 2;
|
||||
G[9][1] = -q1 / 2;
|
||||
G[9][2] = -q0 / 2;
|
||||
|
||||
// dwbias = random walk noise
|
||||
G[10][6] = G[11][7] = G[12][8] = 1;
|
||||
// dabias = random walk noise
|
||||
G[13][9] = G[14][10] = G[15][11] = 1;
|
||||
}
|
||||
|
||||
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
|
||||
{
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// first six outputs are P and V
|
||||
Y[0] = X[0];
|
||||
Y[1] = X[1];
|
||||
Y[2] = X[2];
|
||||
Y[3] = X[3];
|
||||
Y[4] = X[4];
|
||||
Y[5] = X[5];
|
||||
|
||||
// Bb=Rbe*Be
|
||||
Y[6] =
|
||||
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
|
||||
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
|
||||
q0 * q2) * Be[2];
|
||||
Y[7] =
|
||||
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
|
||||
q2 * q2 - q3 * q3) * Be[1] +
|
||||
2 * (q2 * q3 + q0 * q1) * Be[2];
|
||||
Y[8] =
|
||||
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
|
||||
q0 * q1) * Be[1] +
|
||||
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
|
||||
|
||||
// Alt = -Pz
|
||||
Y[9] = -X[2];
|
||||
}
|
||||
|
||||
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
|
||||
{
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
q0 = X[6];
|
||||
q1 = X[7];
|
||||
q2 = X[8];
|
||||
q3 = X[9];
|
||||
|
||||
// dP/dP=I;
|
||||
H[0][0] = H[1][1] = H[2][2] = 1;
|
||||
// dV/dV=I;
|
||||
H[3][3] = H[4][4] = H[5][5] = 1;
|
||||
|
||||
// dBb/dq
|
||||
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
|
||||
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
|
||||
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
|
||||
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
|
||||
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
|
||||
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
|
||||
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
|
||||
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
|
||||
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
|
||||
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
|
||||
|
||||
// dAlt/dPz = -1
|
||||
H[9][2] = -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
Loading…
x
Reference in New Issue
Block a user