mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 06:43:35 +08:00
Merge branch 'master' into navigator_rewrite
Conflicts: src/drivers/gps/gps.cpp src/drivers/gps/mtk.cpp src/modules/commander/commander.cpp src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp src/modules/navigator/mission.cpp src/modules/navigator/mission.h src/modules/navigator/navigator_main.cpp src/modules/navigator/navigator_state.h src/modules/position_estimator_inav/position_estimator_inav_main.c
This commit is contained in:
Julian Oes
+34
-22
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file fw_att_pos_estimator_main.cpp
|
||||
* @file ekf_att_pos_estimator_main.cpp
|
||||
* Implementation of the attitude and position estimator.
|
||||
*
|
||||
* @author Paul Riseborough <p_riseborough@live.com.au>
|
||||
@@ -97,6 +97,7 @@ __EXPORT uint32_t millis();
|
||||
|
||||
static uint64_t last_run = 0;
|
||||
static uint64_t IMUmsec = 0;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
@@ -194,6 +195,7 @@ private:
|
||||
bool _baro_init;
|
||||
bool _gps_initialized;
|
||||
uint64_t _gps_start_time;
|
||||
uint64_t _filter_start_time;
|
||||
bool _gyro_valid;
|
||||
bool _accel_valid;
|
||||
bool _mag_valid;
|
||||
@@ -246,7 +248,7 @@ private:
|
||||
|
||||
float _velocity_xy_filtered;
|
||||
float _velocity_z_filtered;
|
||||
float _airspeed_filtered;
|
||||
float _airspeed_filtered;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
@@ -272,7 +274,7 @@ private:
|
||||
/**
|
||||
* Main sensor collection task.
|
||||
*/
|
||||
void task_main() __attribute__((noreturn));
|
||||
void task_main();
|
||||
};
|
||||
|
||||
namespace estimator
|
||||
@@ -306,6 +308,8 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
_vstatus_sub(-1),
|
||||
_params_sub(-1),
|
||||
_manual_control_sub(-1),
|
||||
_mission_sub(-1),
|
||||
_home_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_att_pub(-1),
|
||||
@@ -336,13 +340,13 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
_baro_gps_offset(0.0f),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")),
|
||||
_perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")),
|
||||
_perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")),
|
||||
_perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")),
|
||||
_perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")),
|
||||
_perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")),
|
||||
_loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")),
|
||||
_perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")),
|
||||
_perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")),
|
||||
_perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")),
|
||||
_perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")),
|
||||
_perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")),
|
||||
|
||||
/* states */
|
||||
_initialized(false),
|
||||
@@ -509,14 +513,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
|
||||
estimator::g_estimator->task_main();
|
||||
}
|
||||
|
||||
float dt = 0.0f; // time lapsed since last covariance prediction
|
||||
|
||||
void
|
||||
FixedwingEstimator::task_main()
|
||||
{
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
_ekf = new AttPosEKF();
|
||||
float dt = 0.0f; // time lapsed since last covariance prediction
|
||||
_filter_start_time = hrt_absolute_time();
|
||||
|
||||
if (!_ekf) {
|
||||
errx(1, "failed allocating EKF filter - out of RAM!");
|
||||
@@ -642,6 +646,7 @@ FixedwingEstimator::task_main()
|
||||
|
||||
_ekf->ZeroVariables();
|
||||
_ekf->dtIMU = 0.01f;
|
||||
_filter_start_time = last_sensor_timestamp;
|
||||
|
||||
/* now skip this loop and get data on the next one, which will also re-init the filter */
|
||||
continue;
|
||||
@@ -719,6 +724,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (last_accel != _sensor_combined.accelerometer_timestamp) {
|
||||
accel_updated = true;
|
||||
} else {
|
||||
accel_updated = false;
|
||||
}
|
||||
|
||||
last_accel = _sensor_combined.accelerometer_timestamp;
|
||||
@@ -809,7 +816,6 @@ FixedwingEstimator::task_main()
|
||||
perf_count(_perf_gps);
|
||||
|
||||
if (_gps.fix_type < 3) {
|
||||
gps_updated = false;
|
||||
newDataGps = false;
|
||||
|
||||
} else {
|
||||
@@ -1025,7 +1031,7 @@ FixedwingEstimator::task_main()
|
||||
* PART TWO: EXECUTE THE FILTER
|
||||
**/
|
||||
|
||||
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
|
||||
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
|
||||
|
||||
float initVelNED[3];
|
||||
|
||||
@@ -1042,9 +1048,9 @@ FixedwingEstimator::task_main()
|
||||
|
||||
// Set up height correctly
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
|
||||
_baro_gps_offset = gps_alt - _baro.altitude;
|
||||
_baro_gps_offset = _baro_ref - _baro.altitude;
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
|
||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
|
||||
|
||||
// Set up position variables correctly
|
||||
_ekf->GPSstatus = _gps.fix_type;
|
||||
@@ -1061,14 +1067,14 @@ FixedwingEstimator::task_main()
|
||||
// Initialize projection
|
||||
_local_pos.ref_lat = lat;
|
||||
_local_pos.ref_lon = lon;
|
||||
_local_pos.ref_alt = _baro_ref + _baro_gps_offset;
|
||||
_local_pos.ref_alt = gps_alt;
|
||||
_local_pos.ref_timestamp = _gps.timestamp_position;
|
||||
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
|
||||
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
|
||||
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
|
||||
warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea);
|
||||
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
|
||||
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
|
||||
|
||||
_gps_initialized = true;
|
||||
@@ -1084,6 +1090,10 @@ FixedwingEstimator::task_main()
|
||||
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
|
||||
_local_pos.ref_alt = _baro_ref;
|
||||
_baro_gps_offset = 0.0f;
|
||||
|
||||
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
|
||||
}
|
||||
}
|
||||
@@ -1263,7 +1273,8 @@ FixedwingEstimator::task_main()
|
||||
_local_pos.timestamp = last_sensor_timestamp;
|
||||
_local_pos.x = _ekf->states[7];
|
||||
_local_pos.y = _ekf->states[8];
|
||||
_local_pos.z = _ekf->states[9] + _baro_gps_offset;
|
||||
// XXX need to announce change of Z reference somehow elegantly
|
||||
_local_pos.z = _ekf->states[9] - _baro_gps_offset;
|
||||
|
||||
_local_pos.vx = _ekf->states[4];
|
||||
_local_pos.vy = _ekf->states[5];
|
||||
@@ -1324,8 +1335,8 @@ FixedwingEstimator::task_main()
|
||||
_global_pos.vel_e = 0.0f;
|
||||
}
|
||||
|
||||
/* local pos alt is negative, change sign and add alt offset */
|
||||
_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
|
||||
/* local pos alt is negative, change sign and add alt offsets */
|
||||
_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
|
||||
|
||||
if (_local_pos.v_z_valid) {
|
||||
_global_pos.vel_d = _local_pos.vz;
|
||||
@@ -1400,7 +1411,8 @@ FixedwingEstimator::print_status()
|
||||
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
|
||||
|
||||
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec);
|
||||
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
|
||||
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
|
||||
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
|
||||
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
|
||||
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
|
||||
+3
-3
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file fw_att_pos_estimator_params.c
|
||||
* @file ekf_att_pos_estimator_params.c
|
||||
*
|
||||
* Parameters defined by the attitude and position estimator task
|
||||
*
|
||||
@@ -226,11 +226,11 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
|
||||
* Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
|
||||
* Increasing this value makes the bias estimation faster and noisier.
|
||||
*
|
||||
* @min 0.0001
|
||||
* @min 0.00001
|
||||
* @max 0.001
|
||||
* @group Position Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
|
||||
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f);
|
||||
|
||||
/**
|
||||
* Magnetometer earth frame offsets process noise
|
||||
@@ -1,5 +1,6 @@
|
||||
#include "estimator.h"
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
// Define EKF_DEBUG here to enable the debug print calls
|
||||
// if the macro is not set, these will be completely
|
||||
@@ -1268,7 +1269,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
for (uint8_t i = 0; i < n_states; i++) {
|
||||
H_MAG[i] = 0.0f;
|
||||
}
|
||||
uint8_t indexLimit;
|
||||
unsigned indexLimit;
|
||||
|
||||
// Perform sequential fusion of Magnetometer measurements.
|
||||
// This assumes that the errors in the different components are
|
||||
@@ -1281,11 +1282,11 @@ void AttPosEKF::FuseMagnetometer()
|
||||
// Limit range of states modified when on ground
|
||||
if(!onGround)
|
||||
{
|
||||
indexLimit = 22;
|
||||
indexLimit = n_states;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexLimit = 13;
|
||||
indexLimit = 13 + 1;
|
||||
}
|
||||
|
||||
// Sequential fusion of XYZ components to spread processing load across
|
||||
@@ -1346,7 +1347,16 @@ void AttPosEKF::FuseMagnetometer()
|
||||
H_MAG[19] = 1.0f;
|
||||
|
||||
// Calculate Kalman gain
|
||||
SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MX[0] = 1.0f / temp;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[19][19] += 0.1f*R_MAG;
|
||||
obsIndex = 1;
|
||||
return;
|
||||
}
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
@@ -1398,7 +1408,16 @@ void AttPosEKF::FuseMagnetometer()
|
||||
H_MAG[20] = 1;
|
||||
|
||||
// Calculate Kalman gain
|
||||
SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MY[0] = 1.0f / temp;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[20][20] += 0.1f*R_MAG;
|
||||
obsIndex = 2;
|
||||
return;
|
||||
}
|
||||
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
|
||||
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
SK_MY[3] = 2*q0*q3 - 2*q1*q2;
|
||||
@@ -1444,7 +1463,16 @@ void AttPosEKF::FuseMagnetometer()
|
||||
H_MAG[21] = 1;
|
||||
|
||||
// Calculate Kalman gain
|
||||
SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
|
||||
if (temp >= R_MAG) {
|
||||
SK_MZ[0] = 1.0f / temp;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the state variances and try again next time
|
||||
P[21][21] += 0.1f*R_MAG;
|
||||
obsIndex = 3;
|
||||
return;
|
||||
}
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
|
||||
@@ -1483,7 +1511,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j= 0; j<=indexLimit; j++)
|
||||
for (uint8_t j= 0; j < indexLimit; j++)
|
||||
{
|
||||
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
|
||||
}
|
||||
@@ -1500,7 +1528,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (uint8_t i = 0; i<=indexLimit; i++)
|
||||
for (uint8_t i = 0; i < indexLimit; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= 3; j++)
|
||||
{
|
||||
@@ -1522,9 +1550,9 @@ void AttPosEKF::FuseMagnetometer()
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i<=indexLimit; i++)
|
||||
for (uint8_t i = 0; i < indexLimit; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j<=indexLimit; j++)
|
||||
for (uint8_t j = 0; j < indexLimit; j++)
|
||||
{
|
||||
KHP[i][j] = 0.0f;
|
||||
for (uint8_t k = 0; k <= 3; k++)
|
||||
@@ -1541,9 +1569,9 @@ void AttPosEKF::FuseMagnetometer()
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i <= indexLimit; i++)
|
||||
for (uint8_t i = 0; i < indexLimit; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= indexLimit; j++)
|
||||
for (uint8_t j = 0; j < indexLimit; j++)
|
||||
{
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
@@ -1564,6 +1592,7 @@ void AttPosEKF::FuseAirspeed()
|
||||
float vwe;
|
||||
float R_TAS = sq(airspeedMeasurementSigma);
|
||||
float SH_TAS[3];
|
||||
float SK_TAS;
|
||||
float VtasPred;
|
||||
|
||||
// Copy required states to local variable names
|
||||
@@ -1593,7 +1622,16 @@ void AttPosEKF::FuseAirspeed()
|
||||
H_TAS[15] = -SH_TAS[1];
|
||||
|
||||
// Calculate Kalman gains
|
||||
float SK_TAS = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
|
||||
float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
|
||||
if (temp >= R_TAS) {
|
||||
SK_TAS = 1.0f / temp;
|
||||
} else {
|
||||
// the calculation is badly conditioned, so we cannot perform fusion on this step
|
||||
// we increase the wind state variances and try again next time
|
||||
P[14][14] += 0.05f*R_TAS;
|
||||
P[15][15] += 0.05f*R_TAS;
|
||||
return;
|
||||
}
|
||||
Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
|
||||
Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
|
||||
Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
|
||||
@@ -2001,7 +2039,22 @@ void AttPosEKF::CovarianceInit()
|
||||
|
||||
float AttPosEKF::ConstrainFloat(float val, float min, float max)
|
||||
{
|
||||
return (val > max) ? max : ((val < min) ? min : val);
|
||||
float ret;
|
||||
if (val > max) {
|
||||
ret = max;
|
||||
ekf_debug("> max: %8.4f, val: %8.4f", max, val);
|
||||
} else if (val < min) {
|
||||
ret = min;
|
||||
ekf_debug("< min: %8.4f, val: %8.4f", min, val);
|
||||
} else {
|
||||
ret = val;
|
||||
}
|
||||
|
||||
if (!isfinite(val)) {
|
||||
ekf_debug("constrain: non-finite!");
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void AttPosEKF::ConstrainVariances()
|
||||
@@ -2466,12 +2519,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
|
||||
|
||||
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
|
||||
{
|
||||
//store initial lat,long and height
|
||||
// store initial lat,long and height
|
||||
latRef = referenceLat;
|
||||
lonRef = referenceLon;
|
||||
hgtRef = referenceHgt;
|
||||
refSet = true;
|
||||
|
||||
// we are at reference altitude, so measurement must be zero
|
||||
hgtMea = 0.0f;
|
||||
|
||||
// the baro offset must be this difference now
|
||||
baroHgtOffset = baroHgt - referenceHgt;
|
||||
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
|
||||
InitializeDynamic(initvelNED, declination);
|
||||
|
||||
@@ -198,6 +198,7 @@ public:
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
|
||||
float rngMea; // Ground distance
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
|
||||
@@ -32,11 +32,11 @@
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Main Attitude and Position Estimator for Fixed Wing Aircraft
|
||||
# Main EKF Attitude and Position Estimator
|
||||
#
|
||||
|
||||
MODULE_COMMAND = ekf_att_pos_estimator
|
||||
|
||||
SRCS = fw_att_pos_estimator_main.cpp \
|
||||
fw_att_pos_estimator_params.c \
|
||||
SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
ekf_att_pos_estimator_params.c \
|
||||
estimator.cpp
|
||||
|
||||
Reference in New Issue
Block a user