From 82cbfafb34fb36e31033f20b6a0c6d4a04f9ce87 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:10:46 +1100 Subject: [PATCH 01/23] EKF: Add source file for terrain vertical position estimator Implements a single state Kalman filter to estimate terrain vertical position relative to the NED origin. --- EKF/terrain_estimator.cpp | 148 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 EKF/terrain_estimator.cpp diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp new file mode 100644 index 0000000000..f9caa3e03d --- /dev/null +++ b/EKF/terrain_estimator.cpp @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file terrain_estimator.cpp + * Function for fusing rangefinder measurements to estimate terrain vertical position/ + * + * @author Paul Riseborough + * + */ + +#include "ekf.h" +#include "mathlib.h" + +bool Ekf::initHagl() +{ + // get most recent range measurement from buffer + rangeSample latest_measurement = _range_buffer.get_newest(); + + if ((_time_last_imu - latest_measurement.time_us) < 2e5) { + // if we have a fresh measurement, use it to initialise the terrain estimator + _terrain_vpos = _state.pos(2) + latest_measurement.rng; + // initialise state variance to variance of measurement + _terrain_var = sq(_params.range_noise); + // success + return true; + + } else if (!_in_air) { + // if on ground we assume a ground clearance + _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; + // Use the ground clearance value as our uncertainty + _terrain_var = sq(_params.rng_gnd_clearance); + // ths is a guess + return false; + + } else { + // no information - cannot initialise + return false; + } +} + +void Ekf::predictHagl() +{ + // predict the state variance growth + // the state is the vertical position of the terrain underneath the vehicle + _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); + + // limit the variance to prevent it becoming badly conditioned + _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); +} + +void Ekf::fuseHagl() +{ + // If the vehicle is excessively tilted, do not try to fuse range finder observations + if (_R_prev(2, 2) > 0.7071f) { + // get a height above ground measurement from the range finder assuming a flat earth + float meas_hagl = _range_sample_delayed.rng * _R_prev(2, 2); + + // predict the hagl from the vehicle position and terrain height + float pred_hagl = _terrain_vpos - _state.pos(2); + + // calculate the innovation + _hagl_innov = pred_hagl - meas_hagl; + + // calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error + float obs_variance = fmaxf(P[8][8], 0.0f) + sq(_params.range_noise / _R_prev(2, 2)); + + // calculate the innovation variance - limiting it to prevent a badly conditioned fusion + _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + + // perform an innovation consistency check and only fuse data if it passes + float gate_size = fmaxf(_params.range_innov_gate, 1.0f); + float test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + + if (test_ratio <= 1.0f) { + // calculate the Kalman gain + float gain = obs_variance / _hagl_innov_var; + // correct the state + _terrain_vpos -= gain * _hagl_innov; + // correct the variance + _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); + // record last successful fusion time + _time_last_hagl_fuse = _time_last_imu; + } + + + } else { + return; + } +} + +// return true if the estimate is fresh +// return the estimated vertical position of the terrain relative to the NED origin +bool Ekf::get_terrain_vert_pos(float *ret) +{ + memcpy(ret, &_terrain_vpos, sizeof(float)); + + // The height is useful if the uncertainty in terrain height is significantly smaller than than the estimated height above terrain + bool accuracy_useful = (sqrtf(_terrain_var) < 0.2f * fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance)); + + if (_time_last_imu - _time_last_hagl_fuse < 1e6 || accuracy_useful) { + return true; + + } else { + return false; + } +} + +void Ekf::get_hagl_innov(float *hagl_innov) +{ + memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov)); +} + + +void Ekf::get_hagl_innov_var(float *hagl_innov_var) +{ + memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var)); +} From 122dd9c53125242c84d1946c15726df481b8f626 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:12:06 +1100 Subject: [PATCH 02/23] EKF: Add source file for optical flow LOS rate fusion --- EKF/optflow_fusion.cpp | 541 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 541 insertions(+) create mode 100644 EKF/optflow_fusion.cpp diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp new file mode 100644 index 0000000000..40da07635d --- /dev/null +++ b/EKF/optflow_fusion.cpp @@ -0,0 +1,541 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vel_pos_fusion.cpp + * Function for fusing gps and baro measurements/ + * + * @author Paul Riseborough + * @author Siddharth Bharat Purohit + * + */ + +#include "ekf.h" +#include "mathlib.h" + +void Ekf::fuseOptFlow() +{ + float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f); + float optflow_test_ratio[2] = {0}; + + // get latest estimated orientation + float q0 = _state.quat_nominal(0); + float q1 = _state.quat_nominal(1); + float q2 = _state.quat_nominal(2); + float q3 = _state.quat_nominal(3); + + // get latest velocity in earth frame + float vn = _state.vel(0); + float ve = _state.vel(1); + float vd = _state.vel(2); + + // calculate the observation noise variance - scaling noise linearly across flow quality range + float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); + float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f); + + // calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst + float weighting = (255.0f - (float)_params.flow_qual_min); + + if (weighting >= 1.0f) { + weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f, + 1.0f); + + } else { + weighting = 0.0f; + } + + // take the weighted average of the observation noie for the best and wort flow quality + float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); + + float H_LOS[2][24] = {0}; // Optical flow observation Jacobians + float Kfusion[24][2] = {0}; // Optical flow Kalman gains + + // constrain height above ground to be above minimum height when sitting on ground + float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance); + + // get rotation nmatrix from earth to body + matrix::Dcm earth_to_body(_state.quat_nominal); + earth_to_body = earth_to_body.transpose(); + + // rotate earth velocities into body frame + Vector3f vel_body = earth_to_body * _state.vel; + + // calculate range from focal point to centre of image + float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view + + // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed + // correct for gyro bias errors in the data used to do the motion compensation + // Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis. + Vector2f opt_flow_rate; + opt_flow_rate(0) = _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); + opt_flow_rate(1) = _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); + + if (opt_flow_rate.norm() < _params.flow_rate_max) { + _flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis + _flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis + + } else { + return; + } + + + // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated + // Calculate Obser ation Jacobians and Kalman gans for each measurement axis + for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { + + if (obs_index == 0) { + + // calculate X axis observation Jacobian + float t2 = 1.0f / range; + float t3 = q0 * q0; + float t4 = q1 * q1; + float t5 = q2 * q2; + float t6 = q3 * q3; + float t7 = q0 * q2 * 2.0f; + float t8 = q1 * q3 * 2.0f; + float t9 = q0 * q3 * 2.0f; + float t10 = q1 * q2 * 2.0f; + float t11 = q0 * q1 * 2.0f; + H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f)); + H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6)); + H_LOS[0][3] = -t2 * (t9 - t10); + H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6); + H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f); + + // calculate intermediate variables for the X observaton innovatoin variance and klmna gains + t2 = 1.0f / range; + t3 = q0 * q1 * 2.0f; + t4 = q2 * q3 * 2.0f; + t5 = q0 * q0; + t6 = q1 * q1; + t7 = q2 * q2; + t8 = q3 * q3; + t9 = q0 * q2 * 2.0f; + t10 = q1 * q3 * 2.0f; + t11 = q0 * q3 * 2.0f; + float t12 = q1 * q2 * 2.0f; + float t13 = t11 - t12; + float t14 = t3 + t4; + float t15 = t5 - t6 - t7 + t8; + float t16 = t15 * vd; + float t17 = t3 - t4; + float t18 = t9 + t10; + float t19 = t18 * vn; + float t28 = t17 * ve; + float t20 = t16 + t19 - t28; + float t21 = t5 + t6 - t7 - t8; + float t22 = t21 * vn; + float t23 = t9 - t10; + float t24 = t11 + t12; + float t25 = t24 * ve; + float t29 = t23 * vd; + float t26 = t22 + t25 - t29; + float t27 = t5 - t6 + t7 - t8; + float t30 = P[0][0] * t2 * t20; + float t31 = P[5][3] * t2 * t14; + float t32 = P[0][3] * t2 * t20; + float t33 = P[4][3] * t2 * t27; + float t56 = P[3][3] * t2 * t13; + float t57 = P[2][3] * t2 * t26; + float t34 = t31 + t32 + t33 - t56 - t57; + float t35 = P[5][5] * t2 * t14; + float t36 = P[0][5] * t2 * t20; + float t37 = P[4][5] * t2 * t27; + float t59 = P[3][5] * t2 * t13; + float t60 = P[2][5] * t2 * t26; + float t38 = t35 + t36 + t37 - t59 - t60; + float t39 = t2 * t14 * t38; + float t40 = P[5][0] * t2 * t14; + float t41 = P[4][0] * t2 * t27; + float t61 = P[3][0] * t2 * t13; + float t62 = P[2][0] * t2 * t26; + float t42 = t30 + t40 + t41 - t61 - t62; + float t43 = t2 * t20 * t42; + float t44 = P[5][2] * t2 * t14; + float t45 = P[0][2] * t2 * t20; + float t46 = P[4][2] * t2 * t27; + float t55 = P[2][2] * t2 * t26; + float t63 = P[3][2] * t2 * t13; + float t47 = t44 + t45 + t46 - t55 - t63; + float t48 = P[5][4] * t2 * t14; + float t49 = P[0][4] * t2 * t20; + float t50 = P[4][4] * t2 * t27; + float t65 = P[3][4] * t2 * t13; + float t66 = P[2][4] * t2 * t26; + float t51 = t48 + t49 + t50 - t65 - t66; + float t52 = t2 * t27 * t51; + float t58 = t2 * t13 * t34; + float t64 = t2 * t26 * t47; + float t53 = R_LOS + t39 + t43 + t52 - t58 - t64; + float t54; + + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation + if (t53 >= R_LOS) { + t54 = 1.0f / t53; + _flow_innov_var[0] = t53; + + } else { + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } + + // calculate Kalman gains for X-axis observation + Kfusion[0][0] = t54 * (t30 - P[0][3] * t2 * (t11 - q1 * q2 * 2.0f) + P[0][5] * t2 * t14 - P[0][2] * t2 * t26 + P[0][4] * + t2 * t27); + Kfusion[1][0] = t54 * (-P[1][3] * t2 * t13 + P[1][5] * t2 * t14 + P[1][0] * t2 * t20 - P[1][2] * t2 * t26 + P[1][4] * t2 + * t27); + Kfusion[2][0] = t54 * (-t55 - P[2][3] * t2 * t13 + P[2][5] * t2 * t14 + P[2][0] * t2 * t20 + P[2][4] * t2 * t27); + Kfusion[3][0] = t54 * (-t56 + P[3][5] * t2 * t14 + P[3][0] * t2 * t20 - P[3][2] * t2 * t26 + P[3][4] * t2 * t27); + Kfusion[4][0] = t54 * (t50 - P[4][3] * t2 * t13 + P[4][5] * t2 * t14 + P[4][0] * t2 * t20 - P[4][2] * t2 * t26); + Kfusion[5][0] = t54 * (t35 - P[5][3] * t2 * t13 + P[5][0] * t2 * t20 - P[5][2] * t2 * t26 + P[5][4] * t2 * t27); + Kfusion[6][0] = t54 * (-P[6][3] * t2 * t13 + P[6][5] * t2 * t14 + P[6][0] * t2 * t20 - P[6][2] * t2 * t26 + P[6][4] * t2 + * t27); + Kfusion[7][0] = t54 * (-P[7][3] * t2 * t13 + P[7][5] * t2 * t14 + P[7][0] * t2 * t20 - P[7][2] * t2 * t26 + P[7][4] * t2 + * t27); + Kfusion[8][0] = t54 * (-P[8][3] * t2 * t13 + P[8][5] * t2 * t14 + P[8][0] * t2 * t20 - P[8][2] * t2 * t26 + P[8][4] * t2 + * t27); + Kfusion[9][0] = t54 * (-P[9][3] * t2 * t13 + P[9][5] * t2 * t14 + P[9][0] * t2 * t20 - P[9][2] * t2 * t26 + P[9][4] * t2 + * t27); + Kfusion[10][0] = t54 * (-P[10][3] * t2 * t13 + P[10][5] * t2 * t14 + P[10][0] * t2 * t20 - P[10][2] * t2 * t26 + + P[10][4] * t2 * t27); + Kfusion[11][0] = t54 * (-P[11][3] * t2 * t13 + P[11][5] * t2 * t14 + P[11][0] * t2 * t20 - P[11][2] * t2 * t26 + + P[11][4] * t2 * t27); + Kfusion[12][0] = t54 * (-P[12][3] * t2 * t13 + P[12][5] * t2 * t14 + P[12][0] * t2 * t20 - P[12][2] * t2 * t26 + + P[12][4] * t2 * t27); + Kfusion[13][0] = t54 * (-P[13][3] * t2 * t13 + P[13][5] * t2 * t14 + P[13][0] * t2 * t20 - P[13][2] * t2 * t26 + + P[13][4] * t2 * t27); + Kfusion[14][0] = t54 * (-P[14][3] * t2 * t13 + P[14][5] * t2 * t14 + P[14][0] * t2 * t20 - P[14][2] * t2 * t26 + + P[14][4] * t2 * t27); + Kfusion[15][0] = t54 * (-P[15][3] * t2 * t13 + P[15][5] * t2 * t14 + P[15][0] * t2 * t20 - P[15][2] * t2 * t26 + + P[15][4] * t2 * t27); + + if (_control_status.flags.mag_3D) { + Kfusion[16][0] = t54 * (-P[16][3] * t2 * t13 + P[16][5] * t2 * t14 + P[16][0] * t2 * t20 - P[16][2] * t2 * t26 + + P[16][4] * t2 * t27); + Kfusion[17][0] = t54 * (-P[17][3] * t2 * t13 + P[17][5] * t2 * t14 + P[17][0] * t2 * t20 - P[17][2] * t2 * t26 + + P[17][4] * t2 * t27); + Kfusion[18][0] = t54 * (-P[18][3] * t2 * t13 + P[18][5] * t2 * t14 + P[18][0] * t2 * t20 - P[18][2] * t2 * t26 + + P[18][4] * t2 * t27); + Kfusion[19][0] = t54 * (-P[19][3] * t2 * t13 + P[19][5] * t2 * t14 + P[19][0] * t2 * t20 - P[19][2] * t2 * t26 + + P[19][4] * t2 * t27); + Kfusion[20][0] = t54 * (-P[20][3] * t2 * t13 + P[20][5] * t2 * t14 + P[20][0] * t2 * t20 - P[20][2] * t2 * t26 + + P[20][4] * t2 * t27); + Kfusion[21][0] = t54 * (-P[21][3] * t2 * t13 + P[21][5] * t2 * t14 + P[21][0] * t2 * t20 - P[21][2] * t2 * t26 + + P[21][4] * t2 * t27); + } + + if (_control_status.flags.wind) { + Kfusion[22][0] = t54 * (-P[22][3] * t2 * t13 + P[22][5] * t2 * t14 + P[22][0] * t2 * t20 - P[22][2] * t2 * t26 + + P[22][4] * t2 * t27); + Kfusion[23][0] = t54 * (-P[23][3] * t2 * t13 + P[23][5] * t2 * t14 + P[23][0] * t2 * t20 - P[23][2] * t2 * t26 + + P[23][4] * t2 * t27); + } + + // run innovation consistency checks + optflow_test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]); + + } else if (obs_index == 1) { + + // calculate Y axis observation Jacobian + float t2 = 1.0f / range; + float t3 = q0 * q0; + float t4 = q1 * q1; + float t5 = q2 * q2; + float t6 = q3 * q3; + float t7 = q0 * q1 * 2.0f; + float t8 = q0 * q3 * 2.0f; + float t9 = q0 * q2 * 2.0f; + float t10 = q1 * q3 * 2.0f; + H_LOS[1][1] = t2 * (vn * (t9 + t10) + vd * (t3 - t4 - t5 + t6) - ve * (t7 - q2 * q3 * 2.0f)); + H_LOS[1][2] = -t2 * (ve * (t3 - t4 + t5 - t6) + vd * (t7 + q2 * q3 * 2.0f) - vn * (t8 - q1 * q2 * 2.0f)); + H_LOS[1][3] = -t2 * (t3 + t4 - t5 - t6); + H_LOS[1][4] = -t2 * (t8 + q1 * q2 * 2.0f); + H_LOS[1][5] = t2 * (t9 - t10); + + // calculate intermediate variables for the X observaton innovatoin variance and klmna gains + t2 = 1.0f / range; + t3 = q0 * q2 * 2.0f; + t4 = q0 * q0; + t5 = q1 * q1; + t6 = q2 * q2; + t7 = q3 * q3; + t8 = q0 * q1 * 2.0f; + t9 = q0 * q3 * 2.0f; + t10 = q1 * q2 * 2.0f; + float t11 = t9 + t10; + float t12 = q1 * q3 * 2.0f; + float t13 = t4 - t5 - t6 + t7; + float t14 = t13 * vd; + float t15 = q2 * q3 * 2.0f; + float t16 = t3 + t12; + float t17 = t16 * vn; + float t18 = t4 - t5 + t6 - t7; + float t19 = t18 * ve; + float t20 = t8 + t15; + float t21 = t20 * vd; + float t22 = t9 - t10; + float t28 = t22 * vn; + float t23 = t19 + t21 - t28; + float t24 = t4 + t5 - t6 - t7; + float t25 = t3 - t12; + float t26 = t8 - t15; + float t29 = t26 * ve; + float t27 = t14 + t17 - t29; + float t30 = P[4][4] * t2 * t11; + float t31 = P[2][4] * t2 * t23; + float t32 = P[3][4] * t2 * t24; + float t56 = P[5][4] * t2 * t25; + float t57 = P[1][4] * t2 * t27; + float t33 = t30 + t31 + t32 - t56 - t57; + float t34 = t2 * t11 * t33; + float t35 = P[4][5] * t2 * t11; + float t36 = P[2][5] * t2 * t23; + float t37 = P[3][5] * t2 * t24; + float t58 = P[5][5] * t2 * t25; + float t59 = P[1][5] * t2 * t27; + float t38 = t35 + t36 + t37 - t58 - t59; + float t39 = P[4][1] * t2 * t11; + float t40 = P[2][1] * t2 * t23; + float t41 = P[3][1] * t2 * t24; + float t55 = P[1][1] * t2 * t27; + float t61 = P[5][1] * t2 * t25; + float t42 = t39 + t40 + t41 - t55 - t61; + float t43 = P[4][2] * t2 * t11; + float t44 = P[2][2] * t2 * t23; + float t45 = P[3][2] * t2 * t24; + float t63 = P[5][2] * t2 * t25; + float t64 = P[1][2] * t2 * t27; + float t46 = t43 + t44 + t45 - t63 - t64; + float t47 = t2 * t23 * t46; + float t48 = P[4][3] * t2 * t11; + float t49 = P[2][3] * t2 * t23; + float t50 = P[3][3] * t2 * t24; + float t65 = P[5][3] * t2 * t25; + float t66 = P[1][3] * t2 * t27; + float t51 = t48 + t49 + t50 - t65 - t66; + float t52 = t2 * t24 * t51; + float t60 = t2 * t25 * t38; + float t62 = t2 * t27 * t42; + float t53 = R_LOS + t34 + t47 + t52 - t60 - t62; + float t54; + + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation + if (t53 >= R_LOS) { + t54 = 1.0f / t53; + _flow_innov_var[1] = t53; + + } else { + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } + + // calculate Kalman gains for X-axis observation + Kfusion[0][1] = -t54 * (P[0][4] * t2 * t11 + P[0][2] * t2 * t23 + P[0][3] * t2 * t24 - P[0][1] * t2 * t27 - P[0][5] * t2 + * t25); + Kfusion[1][1] = -t54 * (-t55 + P[1][4] * t2 * t11 + P[1][2] * t2 * t23 + P[1][3] * t2 * t24 - P[1][5] * t2 * t25); + Kfusion[2][1] = -t54 * (t44 + P[2][4] * t2 * t11 + P[2][3] * t2 * t24 - P[2][1] * t2 * t27 - P[2][5] * t2 * t25); + Kfusion[3][1] = -t54 * (t50 + P[3][4] * t2 * t11 + P[3][2] * t2 * t23 - P[3][1] * t2 * t27 - P[3][5] * t2 * t25); + Kfusion[4][1] = -t54 * (t30 + P[4][2] * t2 * t23 + P[4][3] * t2 * t24 - P[4][1] * t2 * t27 - P[4][5] * t2 * t25); + Kfusion[5][1] = -t54 * (-t58 + P[5][4] * t2 * t11 + P[5][2] * t2 * t23 + P[5][3] * t2 * t24 - P[5][1] * t2 * t27); + Kfusion[6][1] = -t54 * (P[6][4] * t2 * t11 + P[6][2] * t2 * t23 + P[6][3] * t2 * t24 - P[6][1] * t2 * t27 - P[6][5] * t2 + * t25); + Kfusion[7][1] = -t54 * (P[7][4] * t2 * t11 + P[7][2] * t2 * t23 + P[7][3] * t2 * t24 - P[7][1] * t2 * t27 - P[7][5] * t2 + * t25); + Kfusion[8][1] = -t54 * (P[8][4] * t2 * t11 + P[8][2] * t2 * t23 + P[8][3] * t2 * t24 - P[8][1] * t2 * t27 - P[8][5] * t2 + * t25); + Kfusion[9][1] = -t54 * (P[9][4] * t2 * t11 + P[9][2] * t2 * t23 + P[9][3] * t2 * t24 - P[9][1] * t2 * t27 - P[9][5] * t2 + * t25); + Kfusion[10][1] = -t54 * (P[10][4] * t2 * t11 + P[10][2] * t2 * t23 + P[10][3] * t2 * t24 - P[10][1] * t2 * t27 - + P[10][5] * t2 * t25); + Kfusion[11][1] = -t54 * (P[11][4] * t2 * t11 + P[11][2] * t2 * t23 + P[11][3] * t2 * t24 - P[11][1] * t2 * t27 - + P[11][5] * t2 * t25); + Kfusion[12][1] = -t54 * (P[12][4] * t2 * t11 + P[12][2] * t2 * t23 + P[12][3] * t2 * t24 - P[12][1] * t2 * t27 - + P[12][5] * t2 * t25); + Kfusion[13][1] = -t54 * (P[13][4] * t2 * t11 + P[13][2] * t2 * t23 + P[13][3] * t2 * t24 - P[13][1] * t2 * t27 - + P[13][5] * t2 * t25); + Kfusion[14][1] = -t54 * (P[14][4] * t2 * t11 + P[14][2] * t2 * t23 + P[14][3] * t2 * t24 - P[14][1] * t2 * t27 - + P[14][5] * t2 * t25); + Kfusion[15][1] = -t54 * (P[15][4] * t2 * t11 + P[15][2] * t2 * t23 + P[15][3] * t2 * t24 - P[15][1] * t2 * t27 - + P[15][5] * t2 * t25); + + if (_control_status.flags.mag_3D) { + Kfusion[16][1] = -t54 * (P[16][4] * t2 * t11 + P[16][2] * t2 * t23 + P[16][3] * t2 * t24 - P[16][1] * t2 * t27 - + P[16][5] * t2 * t25); + Kfusion[17][1] = -t54 * (P[17][4] * t2 * t11 + P[17][2] * t2 * t23 + P[17][3] * t2 * t24 - P[17][1] * t2 * t27 - + P[17][5] * t2 * t25); + Kfusion[18][1] = -t54 * (P[18][4] * t2 * t11 + P[18][2] * t2 * t23 + P[18][3] * t2 * t24 - P[18][1] * t2 * t27 - + P[18][5] * t2 * t25); + Kfusion[19][1] = -t54 * (P[19][4] * t2 * t11 + P[19][2] * t2 * t23 + P[19][3] * t2 * t24 - P[19][1] * t2 * t27 - + P[19][5] * t2 * t25); + Kfusion[20][1] = -t54 * (P[20][4] * t2 * t11 + P[20][2] * t2 * t23 + P[20][3] * t2 * t24 - P[20][1] * t2 * t27 - + P[20][5] * t2 * t25); + Kfusion[21][1] = -t54 * (P[21][4] * t2 * t11 + P[21][2] * t2 * t23 + P[21][3] * t2 * t24 - P[21][1] * t2 * t27 - + P[21][5] * t2 * t25); + } + + if (_control_status.flags.wind) { + Kfusion[22][1] = -t54 * (P[22][4] * t2 * t11 + P[22][2] * t2 * t23 + P[22][3] * t2 * t24 - P[22][1] * t2 * t27 - + P[22][5] * t2 * t25); + Kfusion[23][1] = -t54 * (P[23][4] * t2 * t11 + P[23][2] * t2 * t23 + P[23][3] * t2 * t24 - P[23][1] * t2 * t27 - + P[23][5] * t2 * t25); + } + + // run innovation consistency check + optflow_test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]); + + } else { + return; + } + } + + // if either axis fails, we fail the sensor + if (optflow_test_ratio[0] > 1.0f || optflow_test_ratio[1] > 1.0f) { + return; + } + + for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { + + // by definition our error state is zero at the time of fusion + _state.ang_error.setZero(); + + // copy the Kalman gain vector for the axis we are fusing + float gain[24]; + + for (unsigned row = 0; row <= 23; row++) { + gain[row] = Kfusion[row][obs_index]; + } + + // Update the state vector + fuse(gain, _flow_innov[obs_index]); + + // correct the quaternion using the attitude error state + Quaternion q_correction; + q_correction.from_axis_angle(_state.ang_error); + _state.quat_nominal = q_correction * _state.quat_nominal; + _state.quat_nominal.normalize(); + + // reset attitude error to zero after the correction has been applied + _state.ang_error.setZero(); + + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column <= 5; column++) { + KH[row][column] = gain[row] * H_LOS[obs_index][column]; + } + + KH[row][8] = gain[row] * H_LOS[obs_index][8]; + } + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + float tmp = KH[row][0] * P[0][column]; + tmp += KH[row][1] * P[1][column]; + tmp += KH[row][2] * P[2][column]; + tmp += KH[row][3] * P[3][column]; + tmp += KH[row][4] * P[4][column]; + tmp += KH[row][5] * P[5][column]; + tmp += KH[row][8] * P[8][column]; + KHP[row][column] = tmp; + } + } + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] -= KHP[row][column]; + } + } + + _time_last_of_fuse = _time_last_imu; + _gps_check_fail_status.value = 0; + makeSymmetrical(); + limitCov(); + } +} + +void Ekf::get_flow_innov(float flow_innov[2]) +{ + memcpy(flow_innov, _flow_innov, sizeof(_flow_innov)); +} + + +void Ekf::get_flow_innov_var(float flow_innov_var[2]) +{ + memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var)); +} + +// calculate optical flow gyro bias errors +void Ekf::calcOptFlowBias() +{ + // accumulate the bias corrected delta angles from the navigation sensor and lapsed time + _imu_del_ang_of(0) += _imu_sample_delayed.delta_ang(0); + _imu_del_ang_of(1) += _imu_sample_delayed.delta_ang(1); + _delta_time_of += _imu_sample_delayed.delta_ang_dt; + + // reset the accumulators if the time interval is too large + if (_delta_time_of > 1.0f) { + _imu_del_ang_of.setZero(); + _delta_time_of = 0.0f; + } + + // if accumulation time differences are not excessive and accumulation time is adequate + // compare the optical flow and and navigation rate data and calculate a bias error + if (_fuse_flow) { + if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f) + && (_flow_sample_delayed.dt > 0.01f)) { + // calculate a reference angular rate + Vector2f reference_body_rate; + reference_body_rate(0) = _imu_del_ang_of(0) / _delta_time_of; + reference_body_rate(1) = _imu_del_ang_of(1) / _delta_time_of; + + // calculate the optical flow sensor measured body rate + Vector2f of_body_rate; + of_body_rate(0) = _flow_sample_delayed.gyroXY(0) / _flow_sample_delayed.dt; + of_body_rate(1) = _flow_sample_delayed.gyroXY(1) / _flow_sample_delayed.dt; + + // calculate the bias estimate using a combined LPF and spike filter + _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), + -0.1f, 0.1f); + _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), + -0.1f, 0.1f); + + } + + // reset the accumulators + _imu_del_ang_of.setZero(); + _delta_time_of = 0.0f; + } +} From 75b22c95acbec9ea96046b9b6fa0e546794efb90 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:12:46 +1100 Subject: [PATCH 03/23] EKF: Add new source files to cmake --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a2de84701c..62f2a90f43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,8 +54,10 @@ px4_add_module( EKF/covariance.cpp EKF/vel_pos_fusion.cpp EKF/mag_fusion.cpp - EKF/gps_checks.cpp + EKF/gps_checks.cpp + EKF/optflow_fusion.cpp EKF/control.cpp + EKF/terrain_estimator.cpp DEPENDS platforms__common ) From dca186c6e874c3f1a989023695ddcb540126420a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:14:15 +1100 Subject: [PATCH 04/23] EKF: Add required declarations for optical flow --- EKF/common.h | 174 ++++++++++++++++++++++++++------------ EKF/ekf.h | 113 +++++++++++++++++++------ EKF/estimator_interface.h | 45 +++++++--- 3 files changed, 241 insertions(+), 91 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index c0c0597d5c..97db40d1ab 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -64,6 +64,13 @@ typedef matrix::Vector Vector3f; typedef matrix::Quaternion Quaternion; typedef matrix::Matrix Matrix3f; +struct flow_message { + uint8_t quality; // Quality of Flow data + Vector2f flowdata; // Flow data received + Vector2f gyrodata; // Gyro data from flow sensor + uint32_t dt; // integration time of flow samples +}; + struct outputSample { Quaternion quat_nominal; // nominal quaternion describing vehicle attitude Vector3f vel; // NED velocity estimate in earth frame in m/s @@ -107,43 +114,90 @@ struct airspeedSample { }; struct flowSample { - Vector2f flowRadXY; - Vector2f flowRadXYcomp; - uint64_t time_us; + uint8_t quality; // quality indicator between 0 and 255 + Vector2f flowRadXY; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive + Vector2f flowRadXYcomp; // measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive + Vector2f gyroXY; // measured delta angle of the inertial frame about the X and Y body axes obtained from rate gyro measurements (rad), RH rotation is positive + float dt; // amount of integration time (sec) + uint64_t time_us; // timestamp in microseconds of the integration period mid-point }; +enum vdist_sensor_type_t { + VDIST_SENSOR_RANGE, + VDIST_SENSOR_BARO, + VDIST_SENSOR_GPS, + VDIST_SENSOR_NONE +}; + +// Bit locations for mag_declination_source +#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value +#define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library +#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed + +// Bit locations for fusion_mode +#define MASK_USE_GPS (1<<0) // set to true to use GPS data +#define MASK_USE_OF (1<<1) // set to true to use optical flow data + +// Integer definitions for mag_fusion_type +#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic +#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg +#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions +#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions. + struct parameters { - float mag_delay_ms; // magnetometer measurement delay relative to the IMU - float baro_delay_ms; // barometer height measurement delay relative to the IMU - float gps_delay_ms; // GPS measurement delay relative to the IMU - float airspeed_delay_ms; // airspeed measurement delay relative to the IMU + // measurement source control + int fusion_mode; + int vdist_sensor_type; + + // measurement time delays + float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec) + float baro_delay_ms; // barometer height measurement delay relative to the IMU (msec) + float gps_delay_ms; // GPS measurement delay relative to the IMU (msec) + float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec) + float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval + float range_delay_ms; // range finder measurement delay relative to the IMU (msec) // input noise - float gyro_noise; // IMU angular rate noise used for covariance prediction - float accel_noise; // IMU acceleration noise use for covariance prediction + float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec) + float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec) // process noise - float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction - float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction - float gyro_scale_p_noise; // process noise for gyro scale factor prediction - float mag_p_noise; // process noise for magnetic field prediction - float wind_vel_p_noise; // process noise for wind velocity prediction + float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec) + float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec) + float gyro_scale_p_noise; // process noise for gyro scale factor prediction (N/A) + float mag_p_noise; // process noise for magnetic field prediction (Guass/sec) + float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec) + float terrain_p_noise; // process noise for terrain offset (m/sec) - float gps_vel_noise; // observation noise for gps velocity fusion - float gps_pos_noise; // observation noise for gps position fusion - float pos_noaid_noise; // observation noise for non-aiding position fusion - float baro_noise; // observation noise for barometric height fusion - float baro_innov_gate; // barometric height innovation consistency gate size in standard deviations - float posNE_innov_gate; // GPS horizontal position innovation consistency gate size in standard deviations - float vel_innov_gate; // GPS velocity innovation consistency gate size in standard deviations + // position and velocity fusion + float gps_vel_noise; // observation noise for gps velocity fusion (m/sec) + float gps_pos_noise; // observation noise for gps position fusion (m) + float pos_noaid_noise; // observation noise for non-aiding position fusion (m) + float baro_noise; // observation noise for barometric height fusion (m) + float baro_innov_gate; // barometric height innovation consistency gate size (STD) + float posNE_innov_gate; // GPS horizontal position innovation consistency gate size (STD) + float vel_innov_gate; // GPS velocity innovation consistency gate size (STD) - float mag_heading_noise; // measurement noise used for simple heading fusion - float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion - float mag_declination_deg; // magnetic declination in degrees - float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations - float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations - int mag_declination_source; // bitmask used to control the handling of declination data - int mag_fusion_type; // integer used to specify the type of magnetometer fusion used + // magnetometer fusion + float mag_heading_noise; // measurement noise used for simple heading fusion (rad) + float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion (Gauss) + float mag_declination_deg; // magnetic declination (degrees) + float heading_innov_gate; // heading fusion innovation consistency gate size (STD) + float mag_innov_gate; // magnetometer fusion innovation consistency gate size (STD) + int mag_declination_source; // bitmask used to control the handling of declination data + int mag_fusion_type; // integer used to specify the type of magnetometer fusion used + + // range finder fusion + float range_noise; // observation noise for range finder measurements (m) + float range_innov_gate; // range finder fusion innovation consistency gate size (STD) + float rng_gnd_clearance; // minimum valid value for range when on ground (m) + + // optical flow fusion + float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec) + float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) + int flow_qual_min; // minimum acceptable quality integer from the flow sensor + float flow_innov_gate; // optical flow fusion innovation consistency gate size (STD) + float flow_rate_max; // maximum valid optical flow rate (rad/sec) // these parameters control the strictness of GPS quality checks used to determine uf the GPS is // good enough to set a local origin and commence aiding @@ -159,10 +213,17 @@ struct parameters { // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. parameters() { + // measurement source control + fusion_mode = MASK_USE_OF; + vdist_sensor_type = VDIST_SENSOR_BARO; + + // measurement time delays mag_delay_ms = 0.0f; baro_delay_ms = 0.0f; gps_delay_ms = 200.0f; airspeed_delay_ms = 200.0f; + flow_delay_ms = 60.0f; + range_delay_ms = 200.0f; // input noise gyro_noise = 1.0e-3f; @@ -174,7 +235,9 @@ struct parameters { gyro_scale_p_noise = 3.0e-3f; mag_p_noise = 2.5e-2f; wind_vel_p_noise = 1.0e-1f; + terrain_p_noise = 0.5f; + // position and velocity fusion gps_vel_noise = 5.0e-1f; gps_pos_noise = 1.0f; pos_noaid_noise = 10.0f; @@ -183,15 +246,28 @@ struct parameters { posNE_innov_gate = 3.0f; vel_innov_gate = 3.0f; - mag_heading_noise = 5.0e-1f; + // magnetometer fusion + mag_heading_noise = 1.7e-1f; mag_noise = 5.0e-2f; mag_declination_deg = 0.0f; heading_innov_gate = 3.0f; mag_innov_gate = 3.0f; - - mag_declination_source = 7; + mag_declination_source = 3; mag_fusion_type = 0; + // range finder fusion + range_noise = 0.1f; + range_innov_gate = 5.0f; + rng_gnd_clearance = 0.1f; + + // optical flow fusion + flow_noise = 0.15f; + flow_noise_qual_min = 0.5f; + flow_qual_min = 1; + flow_innov_gate = 3.0f; + flow_rate_max = 2.5f; + + // GPS quality checks gps_check_mask = 21; req_hacc = 5.0f; req_vacc = 8.0f; @@ -203,17 +279,6 @@ struct parameters { } }; -// Bit locations for mag_declination_source -#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value -#define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library -#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed - -// Integer definitions for mag_fusion_type -#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic -#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg -#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions -#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions. - struct stateSample { Vector3f ang_error; // attitude axis angle error (error state formulation) Vector3f vel; // NED velocity in earth frame in m/s @@ -259,17 +324,20 @@ union gps_check_fail_status_u { // bitmask containing filter control status union filter_control_status_u { struct { - uint8_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete - uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete - uint8_t gps : 1; // 2 - true if GPS measurements are being fused - uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused - uint8_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused - uint8_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused - uint8_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused - uint8_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused - uint8_t in_air : 1; // 8 - true when the vehicle is airborne - uint8_t armed : 1; // 9 - true when the vehicle motors are armed - uint8_t wind : 1; // 10 - true when wind velocity is being estimated + uint16_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete + uint16_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete + uint16_t gps : 1; // 2 - true if GPS measurements are being fused + uint16_t opt_flow : 1; // 3 - true if optical flow measurements are being fused + uint16_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused + uint16_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused + uint16_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused + uint16_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused + uint16_t in_air : 1; // 8 - true when the vehicle is airborne + uint16_t armed : 1; // 9 - true when the vehicle motors are armed + uint16_t wind : 1; // 10 - true when wind velocity is being estimated + uint16_t baro_hgt : 1; // 11 - true when baro height is being fused as a primary height reference + uint16_t rng_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference + uint16_t gps_hgt : 1; // 15 - true when range finder height is being fused as a primary height reference } flags; uint16_t value; }; diff --git a/EKF/ekf.h b/EKF/ekf.h index a44ac191c3..71c2e09af9 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -76,6 +76,18 @@ public: // gets the innovation variance of the heading measurement void get_heading_innov_var(float *heading_innov_var); + // gets the innovation variance of the flow measurement + void get_flow_innov_var(float flow_innov_var[2]); + + // gets the innovation of the flow measurement + void get_flow_innov(float flow_innov[2]); + + // gets the innovation variance of the HAGL measurement + void get_hagl_innov_var(float *hagl_innov_var); + + // gets the innovation of the HAGL measurement + void get_hagl_innov(float *hagl_innov); + // get the state vector at the delayed time horizon void get_state_delayed(float *state); @@ -98,6 +110,17 @@ public: // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning); + void get_vel_var(Vector3f &vel_var); + + void get_pos_var(Vector3f &pos_var); + + // return true if the global position estimate is valid + bool global_position_is_valid(); + + // return true if the etimate is valid + // return the estimated terrain vertical position relative to the NED origin + bool get_terrain_vert_pos(float *ret); + private: static const uint8_t _k_num_states = 24; @@ -105,13 +128,15 @@ private: stateSample _state; // state struct of the ekf running at the delayed time horizon - bool _filter_initialised; - bool _earth_rate_initialised; + bool _filter_initialised; // true when the EKF sttes and covariances been initialised + bool _earth_rate_initialised; // true when we know the earth rotatin rate (requires GPS) - bool _fuse_height; // baro height data should be fused - bool _fuse_pos; // gps position data should be fused + bool _fuse_height; // baro height data should be fused + bool _fuse_pos; // gps position data should be fused bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused - bool _fuse_vert_vel; // gps vertical velocity measurement should be fused + bool _fuse_vert_vel; // gps vertical velocity measurement should be fused + bool _fuse_flow; // flow measurement should be fused + bool _fuse_hagl_data; // if true then range data will be fused to estimate terrain height uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode @@ -131,45 +156,64 @@ private: float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos + float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos + float _mag_innov[3]; // earth magnetic field innovations + float _mag_innov_var[3]; // earth magnetic field innovation variance + float _heading_innov; // heading measurement innovation + float _heading_innov_var; // heading measurement innovation variance - float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos - float _mag_innov_var[3]; // earth magnetic field innovation variance - float _heading_innov_var; // heading measurement innovation variance + Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion + float _tilt_err_length_filt; // filtered length of _tilt_err_vec - float _mag_declination; // magnetic declination used by reset and fusion functions (rad) + // optical flow processing + float _flow_innov[2]; // flow measurement innovation + float _flow_innov_var[2]; // flow innovation variance + Vector2f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs + Vector2f _imu_del_ang_of; // bias corrected XY delta angle measurements accumulated across the same time frame as the optical flow rates + float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over + + float _mag_declination; // magnetic declination used by reset and fusion functions (rad) // complementary filter states Vector3f _delta_angle_corr; // delta angle correction vector Vector3f _delta_vel_corr; // delta velocity correction vector - Vector3f _vel_corr; // velocity correction vector + Vector3f _vel_corr; // velocity correction vector imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate) - Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) + Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps) // variables used for the GPS quality checks - float _gpsDriftVelN; // GPS north position derivative (m/s) - float _gpsDriftVelE; // GPS east position derivative (m/s) - float _gps_drift_velD; // GPS down position derivative (m/s) - float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s) - float _gps_velN_filt; // GPS filtered North velocity (m/s) - float _gps_velE_filt; // GPS filtered East velocity (m/s) - uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks + float _gpsDriftVelN; // GPS north position derivative (m/s) + float _gpsDriftVelE; // GPS east position derivative (m/s) + float _gps_drift_velD; // GPS down position derivative (m/s) + float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s) + float _gps_velN_filt; // GPS filtered North velocity (m/s) + float _gps_velE_filt; // GPS filtered East velocity (m/s) + uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec) - float _gps_alt_ref; // WGS-84 height (m) + float _gps_alt_ref; // WGS-84 height (m) // Variables used to initialise the filter states - uint8_t _baro_counter; // number of baro samples averaged - float _baro_sum; // summed baro measurement - uint8_t _mag_counter; // number of magnetometer samples averaged - Vector3f _mag_sum; // summed magnetometer measurement - Vector3f _delVel_sum; // summed delta velocity - float _baro_at_alignment; // baro offset relative to alignment position + uint8_t _hgt_counter; // number of baro samples averaged + float _hgt_sum; // summed baro measurement + uint8_t _mag_counter; // number of magnetometer samples averaged + Vector3f _mag_sum; // summed magnetometer measurement + Vector3f _delVel_sum; // summed delta velocity + float _hgt_at_alignment; // baro offset relative to alignment position gps_check_fail_status_u _gps_check_fail_status; + // Terrain height state estimation + float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m) + float _terrain_var; // variance of terrain position estimate (m^2) + float _hagl_innov; // innovation of the last height above terrain measurement (m) + float _hagl_innov_var; // innovation variance for the last height above terrain measurement (m^2) + uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks + bool _terrain_initialised; // true when the terrain estimator has been intialised + // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates(); @@ -201,15 +245,28 @@ private: // fuse airspeed measurement void fuseAirspeed(); - // fuse range measurements - void fuseRange(); - // fuse velocity and position measurements (also barometer height) void fuseVelPosHeight(); // reset velocity states of the ekf void resetVelocity(); + // fuse optical flow line of sight rate measurements + void fuseOptFlow(); + + // calculate optical flow bias errors + void calcOptFlowBias(); + + // initialise the terrain vertical position estimator + // return true if the initialisation is successful + bool initHagl(); + + // predict the terrain vertical position state and variance + void predictHagl(); + + // update the terrain vertical position estimate using a height above ground measurement from the range finder + void fuseHagl(); + // reset the heading and magnetic field states using the declination and magnetometer measurements // return true if successful bool resetMagHeading(Vector3f &mag_init); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index ae6a4b4702..400445dcc6 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -81,6 +81,22 @@ public: virtual void get_covariances(float *covariances) = 0; // get the ekf WGS-84 origin position and height and the system time it was last set + virtual void get_vel_var(Vector3f &vel_var) = 0; + virtual void get_pos_var(Vector3f &pos_var) = 0; + + // gets the innovation variance of the flow measurement + virtual void get_flow_innov_var(float flow_innov_var[2]) = 0; + + // gets the innovation of the flow measurement + virtual void get_flow_innov(float flow_innov[2]) = 0; + + // gets the innovation variance of the HAGL measurement + virtual void get_hagl_innov_var(float *flow_innov_var) = 0; + + // gets the innovation of the HAGL measurement + virtual void get_hagl_innov(float *flow_innov_var) = 0; + + // get the ekf WGS-84 origin positoin and height and the system time it was last set virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position @@ -99,7 +115,7 @@ public: virtual bool collect_range(uint64_t time_usec, float *data) { return true; } - virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; } + virtual bool collect_opticalflow(uint64_t time_usec, flow_message *flow) { return true; } // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); @@ -121,7 +137,7 @@ public: void setRangeData(uint64_t time_usec, float *data); // set optical flow data - void setOpticalFlowData(uint64_t time_usec, float *data); + void setOpticalFlowData(uint64_t time_usec, flow_message *flow); // return a address to the parameters struct // in order to give access to the application @@ -133,7 +149,15 @@ public: // set vehicle landed status data void set_in_air_status(bool in_air) {_in_air = in_air;} - bool position_is_valid(); + // return true if the global position estimate is valid + virtual bool global_position_is_valid() = 0; + + // return true if the estimate is valid + // return the estimated terrain vertical position relative to the NED origin + virtual bool get_terrain_vert_pos(float *ret) = 0; + + // return true if the local position estimate is valid + bool local_position_is_valid(); void copy_quaternion(float *quat) @@ -196,13 +220,13 @@ protected: bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground bool _in_air; // we assume vehicle is in the air, set by the given landing detector - bool _NED_origin_initialised; - bool _gps_speed_valid; - float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s) - struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) - float _gps_hpos_accuracy; // GPS receiver reported 1-sigma horizontal accuracy (m) - float _gps_origin_eph; // horizontal position uncertainty of the GPS origin - float _gps_origin_epv; // vertical position uncertainty of the GPS origin + bool _NED_origin_initialised = false; + bool _gps_speed_valid = false; + float _gps_speed_accuracy = 0.0f; // GPS receiver reported 1-sigma speed accuracy (m/s) + float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m) + float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin + float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin + struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) bool _mag_healthy; // computed by mag innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio @@ -226,6 +250,7 @@ protected: uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds uint64_t _time_last_range; // timestamp of last range measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds + uint64_t _time_last_optflow; fault_status_t _fault_status; From 270451e17b0299250277cfec58fca0bec25f79ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:20:24 +1100 Subject: [PATCH 05/23] EKF: Update height reset to support range finder height use --- EKF/ekf_helper.cpp | 34 +++++++++++++++------------------- 1 file changed, 15 insertions(+), 19 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f126098563..2a7a5e85b3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -80,32 +80,28 @@ void Ekf::resetPosition() } } -// Reset height state using the last baro measurement +// Reset height state using the last height measurement void Ekf::resetHeight() { - // if we have a valid GPS measurement use it to initialise the vertical velocity state - gpsSample gps_newest = _gps_buffer.get_newest(); + if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + rangeSample range_newest = _range_buffer.get_newest(); - if (_time_last_imu - gps_newest.time_us < 400000) { - _state.vel(2) = gps_newest.vel(2); + if (_time_last_imu - range_newest.time_us < 200000) { + _state.pos(2) = _hgt_at_alignment - range_newest.rng; + } else { + // TODO: reset to last known range based estimate + } } else { - _state.vel(2) = 0.0f; - } + // initialize vertical position with newest baro measurement + baroSample baro_newest = _baro_buffer.get_newest(); - // if we have a valid height measurement, use it to initialise the vertical position state - baroSample baro_newest = _baro_buffer.get_newest(); + if (_time_last_imu - baro_newest.time_us < 200000) { + _state.pos(2) = _hgt_at_alignment - baro_newest.hgt; - if (_time_last_imu - baro_newest.time_us < 200000) { - // use baro as the default - _state.pos(2) = _baro_at_alignment - baro_newest.hgt; - - } else if (_time_last_imu - gps_newest.time_us < 400000) { - // use GPS as a backup - _state.pos(2) = _gps_alt_ref - gps_newest.hgt; - - } else { - // Do not modify the state as there are no measurements to use + } else { + // TODO: reset to last known baro based estimate + } } } From 32b03819ef0c004d1624071620622449915d086e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:21:04 +1100 Subject: [PATCH 06/23] EKF: Add function to calculate global position validity --- EKF/ekf_helper.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 2a7a5e85b3..8b4ac5484f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -393,3 +393,10 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first)); } } + +bool Ekf::global_position_is_valid() +{ + // return true if the position estimate is valid + // TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status + return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps); +} From 2c2850c0ce084f951fbededcfe711924c0da83cf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:22:06 +1100 Subject: [PATCH 07/23] EKF: Add functions to get position and velocity state variance --- EKF/covariance.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 59a87db520..ececbb4cb3 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -103,6 +103,20 @@ void Ekf::initialiseCovariance() } +void Ekf::get_pos_var(Vector3f &pos_var) +{ + pos_var(0) = P[6][6]; + pos_var(1) = P[7][7]; + pos_var(2) = P[8][8]; +} + +void Ekf::get_vel_var(Vector3f &vel_var) +{ + vel_var(0) = P[3][3]; + vel_var(1) = P[4][4]; + vel_var(2) = P[5][5]; +} + void Ekf::predictCovariance() { // assign intermediate state variables From 2ff338048d51bbbba1285fbe50a709b1ea51d9f6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:27:40 +1100 Subject: [PATCH 08/23] EKF: Add support for range-finder fusion as primary height reference --- EKF/vel_pos_fusion.cpp | 43 ++++++++++++++++++++++++++++++++---------- 1 file changed, 33 insertions(+), 10 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 54d59187af..ade2dcdb0c 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -36,6 +36,8 @@ * Function for fusing gps and baro measurements/ * * @author Roman Bast + * @author Siddharth Bharat Purohit + * @author Paul Riseborough * */ @@ -105,14 +107,27 @@ void Ekf::fuseVelPosHeight() } if (_fuse_height) { - fuse_map[5] = true; - // vertical position innovation - baro measurement has opposite sign to earth z axis - _vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment - _baro_sample_delayed.hgt); - // observation variance - user parameter defined - R[5] = fmaxf(_params.baro_noise, 0.01f); - R[5] = R[5] * R[5]; - // innovation gate size - gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); + if (_control_status.flags.baro_hgt) { + fuse_map[5] = true; + // vertical position innovation - baro measurement has opposite sign to earth z axis + _vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt); + // observation variance - user parameter defined + R[5] = fmaxf(_params.baro_noise, 0.01f); + R[5] = R[5] * R[5]; + // innovation gate size + gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); + + } else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) { + fuse_map[5] = true; + // use range finder with tilt correction + _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng *_R_prev(2, 2), + _params.rng_gnd_clearance)); + // observation variance - user parameter defined + R[5] = fmaxf(_params.range_noise, 0.01f); + R[5] = R[5] * R[5]; + // innovation gate size + gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); + } } // calculate innovation test ratios @@ -122,7 +137,8 @@ void Ekf::fuseVelPosHeight() unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index]; // Compute the ratio of innovation to gate size - _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov_var[obs_index]); + _vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * + _vel_pos_innov_var[obs_index]); } } @@ -141,11 +157,13 @@ void Ekf::fuseVelPosHeight() // record the successful velocity fusion time if (vel_check_pass && _fuse_hor_vel) { _time_last_vel_fuse = _time_last_imu; + _tilt_err_vec.setZero(); } // record the successful position fusion time if (pos_check_pass && _fuse_pos) { _time_last_pos_fuse = _time_last_imu; + _tilt_err_vec.setZero(); } // record the successful height fusion time @@ -190,6 +208,12 @@ void Ekf::fuseVelPosHeight() } } + // sum the attitude error from velocity and position fusion only + // used as a metric for convergence monitoring + if (obs_index != 5) { + _tilt_err_vec += _state.ang_error; + } + // by definition the angle error state is zero at the fusion time _state.ang_error.setZero(); @@ -222,4 +246,3 @@ void Ekf::fuseVelPosHeight() } } - From 836fe39070070bae3b931152f8ae97758b53ae65 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:28:45 +1100 Subject: [PATCH 09/23] EKF: Update external interface functions to support optical flow --- EKF/estimator_interface.cpp | 62 +++++++++++++++++++++++++++++++++---- 1 file changed, 56 insertions(+), 6 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 8658551b3c..766164082c 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -213,21 +213,71 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data) _airspeed_buffer.push(airspeed_sample_new); } } - +static float rng; // set range data void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) { if (!collect_range(time_usec, data) || !_initialised) { return; } + + if (time_usec > _time_last_range) { + rangeSample range_sample_new; + range_sample_new.rng = *data; + rng = *data; + range_sample_new.time_us -= _params.range_delay_ms * 1000; + + range_sample_new.time_us = time_usec; + _time_last_range = time_usec; + + _range_buffer.push(range_sample_new); + } } // set optical flow data -void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data) +void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow) { - if (!collect_opticalflow(time_usec, data) || !_initialised) { + if (!collect_opticalflow(time_usec, flow) || !_initialised) { return; } + + // if data passes checks, push to buffer + if (time_usec > _time_last_optflow) { + // check if enough integration time + float delta_time = 1e-6f * (float)flow->dt; + bool delta_time_good = (delta_time >= 0.05f); + + // check magnitude is within sensor limits + float flow_rate_magnitude; + bool flow_magnitude_good = false; + + if (delta_time_good) { + flow_rate_magnitude = flow->flowdata.norm() / delta_time; + flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max); + } + + // check quality metric + bool flow_quality_good = (flow->quality >= _params.flow_qual_min); + + if (delta_time_good && flow_magnitude_good && flow_quality_good) { + flowSample optflow_sample_new; + // calculate the system time-stamp for the mid point of the integration period + optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2; + // copy the quality metric returned by the PX4Flow sensor + optflow_sample_new.quality = flow->quality; + // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis. + // copy the optical and gyro measured delta angles + optflow_sample_new.flowRadXY = - flow->flowdata; + optflow_sample_new.gyroXY = - flow->gyrodata; + // compensate for body motion to give a LOS rate + optflow_sample_new.flowRadXYcomp = optflow_sample_new.flowRadXY - optflow_sample_new.gyroXY; + // convert integraton interval to seconds + optflow_sample_new.dt = 1e-6f * (float)flow->dt; + _time_last_optflow = time_usec; + // push to buffer + _flow_buffer.push(optflow_sample_new); + } + } } bool EstimatorInterface::initialise_interface(uint64_t timestamp) @@ -265,6 +315,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _time_last_baro = 0; _time_last_range = 0; _time_last_airspeed = 0; + _time_last_optflow = 0; memset(&_fault_status, 0, sizeof(_fault_status)); return true; @@ -283,9 +334,8 @@ void EstimatorInterface::unallocate_buffers() } -bool EstimatorInterface::position_is_valid() +bool EstimatorInterface::local_position_is_valid() { // return true if the position estimate is valid - // TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status - return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6; + return ((_time_last_imu - _time_last_optflow) < 5e6) || global_position_is_valid(); } From d97d308ca729f310d2da3fe67fd29675f1a2c0e4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:31:19 +1100 Subject: [PATCH 10/23] EKF: Add control of optical flow and range finder fusion --- EKF/control.cpp | 89 ++++++++++++++++-- EKF/ekf.cpp | 244 ++++++++++++++++++++++++++++++------------------ 2 files changed, 234 insertions(+), 99 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7a0d753393..dc180d1aca 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -49,12 +49,71 @@ void Ekf::controlFusionModes() // Get the magnetic declination calcMagDeclination(); + // Check for tilt convergence during initial alignment + // filter the tilt error vector using a 1 sec time constant LPF + float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt; + _tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt; + + // Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states + if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) { + _control_status.flags.tilt_align = true; + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + // optical flow fusion mode selection logic - _control_status.flags.opt_flow = false; + // to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data + if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align + && (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) { + // If the heading is not aligned, reset the yaw and magnetic field states + if (!_control_status.flags.yaw_align) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + + // If the heading is valid, start using optical flow aiding + if (_control_status.flags.yaw_align) { + // set the flag and reset the fusion timeout + _control_status.flags.opt_flow = true; + _time_last_of_fuse = _time_last_imu; + + // if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading + if (!_control_status.flags.gps) { + // calculate the rotation matrix from body to earth frame + matrix::Dcm body_to_earth(_state.quat_nominal); + + // constrain height above ground to be above minimum possible + float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); + + // calculate absolute distance from focal point to centre of frame assuming a flat earth + float range = heightAboveGndEst / body_to_earth(2, 2); + + if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + // calculate X and Y body relative velocities from OF measurements + Vector3f vel_optflow_body; + vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; + vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; + vel_optflow_body(2) = 0.0f; + + // rotate from body to earth frame + Vector3f vel_optflow_earth; + vel_optflow_earth = body_to_earth * vel_optflow_body; + + // take x and Y components + _state.vel(0) = vel_optflow_earth(0); + _state.vel(1) = vel_optflow_earth(1); + + } else { + _state.vel.setZero(); + } + } + } + + } else if (!(_params.fusion_mode & MASK_USE_OF)) { + _control_status.flags.opt_flow = false; + } // GPS fusion mode selection logic - // To start using GPS we need tilt and yaw alignment completed, the local NED origin set and fresh GPS data - if (!_control_status.flags.gps) { + // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data + if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { // If the heading is not aligned, reset the yaw and magnetic field states @@ -67,13 +126,12 @@ void Ekf::controlFusionModes() resetPosition(); resetVelocity(); _control_status.flags.gps = true; + _time_last_gps = _time_last_imu; } } - } - // decide when to start using optical flow data - if (!_control_status.flags.opt_flow) { - // TODO optical flow start logic + } else if (!(_params.fusion_mode & MASK_USE_GPS)) { + _control_status.flags.gps = false; } // handle the case when we are relying on GPS fusion and lose it @@ -106,7 +164,15 @@ void Ekf::controlFusionModes() // handle the case when we are relying on optical flow fusion and lose it if (_control_status.flags.opt_flow && !_control_status.flags.gps) { - // TODO + // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something + if ((_time_last_imu - _time_last_of_fuse > 5e6)) { + // Switch to the non-aiding mode, zero the veloity states + // and set the synthetic position to the current estimate + _control_status.flags.opt_flow = false; + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + } } // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances @@ -177,6 +243,12 @@ void Ekf::controlFusionModes() _control_status.flags.mag_dec = false; } + // Control the soure of height measurements for the main filter + _control_status.flags.baro_hgt = true; + _control_status.flags.rng_hgt = false; + _control_status.flags.gps_hgt = false; + + // Placeholder for control of wind velocity states estimation // TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement if (false) { @@ -199,6 +271,7 @@ void Ekf::calculateVehicleStatus() // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming bool in_air = _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f; + if (!_control_status.flags.in_air && in_air) { _control_status.flags.in_air = true; } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5441c8a082..9565388ca4 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -36,7 +36,7 @@ * Core functions for ekf attitude and position estimator. * * @author Roman Bast - * + * @author Paul Riseborough */ #include "ekf.h" @@ -48,6 +48,8 @@ Ekf::Ekf(): _fuse_pos(false), _fuse_hor_vel(false), _fuse_vert_vel(false), + _fuse_flow(false), + _fuse_hagl_data(false), _time_last_fake_gps(0), _time_last_pos_fuse(0), _time_last_vel_fuse(0), @@ -56,6 +58,7 @@ Ekf::Ekf(): _last_disarmed_posD(0.0f), _heading_innov(0.0f), _heading_innov_var(0.0f), + _delta_time_of(0.0f), _mag_declination(0.0f), _gpsDriftVelN(0.0f), _gpsDriftVelE(0.0f), @@ -66,10 +69,14 @@ Ekf::Ekf(): _last_gps_fail_us(0), _last_gps_origin_time_us(0), _gps_alt_ref(0.0f), - _baro_counter(0), - _baro_sum(0.0f), + _hgt_counter(0), + _hgt_sum(0.0f), _mag_counter(0), - _baro_at_alignment(0.0f) + _hgt_at_alignment(0.0f), + _terrain_vpos(0.0f), + _hagl_innov(0.0f), + _hagl_innov_var(0.0f), + _time_last_hagl_fuse(0) { _control_status = {}; _control_status_prev = {}; @@ -89,6 +96,8 @@ Ekf::Ekf(): _q_down_sampled.setZero(); _mag_sum = {}; _delVel_sum = {}; + _flow_gyro_bias = {}; + _imu_del_ang_of = {}; } Ekf::~Ekf() @@ -137,93 +146,137 @@ bool Ekf::init(uint64_t timestamp) _mag_healthy = false; _filter_initialised = false; + _terrain_initialised = false; return ret; } bool Ekf::update() { - bool ret = false; // indicates if there has been an update - if (!_filter_initialised) { - _filter_initialised = initialiseFilter(); - - if (!_filter_initialised) { - return false; - } - } - - //printStates(); - //printStatesFast(); - // prediction + // Only run the filter if IMU data in the buffer has been updated if (_imu_updated) { - ret = true; + if (!_filter_initialised) { + _filter_initialised = initialiseFilter(); + + if (!_filter_initialised) { + return false; + } + } + + // perform state and covariance prediction for the main filter predictState(); predictCovariance(); - } - // control logic - controlFusionModes(); - - // measurement updates - - // Fuse magnetometer data using the selected fusion method and only if angular alignment is complete - if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { - fuseMag(); - - if (_control_status.flags.mag_dec) { - fuseDeclination(); - } - - } else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) { - fuseMag2D(); - - } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { - // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence - fuseHeading(); + // perform state and variance prediction for the terrain estimator + if (!_terrain_initialised) { + _terrain_initialised = initHagl(); } else { - // do no fusion at all + predictHagl(); + } + + // control logic + controlFusionModes(); + + // measurement updates + + // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete + if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { + if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { + fuseMag(); + + if (_control_status.flags.mag_dec) { + fuseDeclination(); + } + + } else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) { + fuseMag2D(); + + } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { + // fusion of a Euler yaw angle from either a 321 or 312 rotation sequence + fuseHeading(); + + } else { + // do no fusion at all + } + } + + // determine if we have height data that can be fused + if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) + && (_R_prev(2, 2) > 0.7071f)) { + // if we have range data we always try to estimate terrain nheight + _fuse_hagl_data = true; + + // only use if for height in the main filter if specifically enabled + if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + _fuse_height = true; + } + + } else if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed) + && _params.vdist_sensor_type == VDIST_SENSOR_BARO) { + _fuse_height = true; + } + + // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it + // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift + // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz + if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { + _fuse_pos = true; + _fuse_vert_vel = true; + _fuse_hor_vel = true; + _fuse_flow = false; + + } else if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) + && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 + && (_R_prev(2, 2) > 0.7071f)) { + _fuse_pos = false; + _fuse_vert_vel = false; + _fuse_hor_vel = false; + _fuse_flow = true; + + } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow + && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + _fuse_pos = true; + _gps_sample_delayed.pos(0) = _last_known_posNE(0); + _gps_sample_delayed.pos(1) = _last_known_posNE(1); + _time_last_fake_gps = _time_last_imu; + } + + // fuse available range finder data into a terrain height estimator if it has been initialised + if (_fuse_hagl_data && _terrain_initialised) { + fuseHagl(); + _fuse_hagl_data = false; + } + + // Fuse available NED velocity and position data into the main filter + if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { + fuseVelPosHeight(); + _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; + } + + // Update optical flow bias estimates + calcOptFlowBias(); + + // Fuse optical flow LOS rate observations into the main filter + if (_fuse_flow) { + fuseOptFlow(); + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _fuse_flow = false; } } - if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { - _fuse_height = true; - } - - // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it - // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift - // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { - _fuse_pos = true; - _fuse_vert_vel = true; - _fuse_hor_vel = true; - - } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { - _fuse_pos = true; - _gps_sample_delayed.pos(0) = _last_known_posNE(0); - _gps_sample_delayed.pos(1) = _last_known_posNE(1); - _time_last_fake_gps = _time_last_imu; - } - - if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { - fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; - } - - if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { - fuseRange(); - } - - if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) { - fuseAirspeed(); - } - + // the output observer always runs calculateOutputStates(); - return ret; + // We don't have valid data to output until tilt and yaw alignment is complete + if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { + return true; + + } else { + return false; + } } bool Ekf::initialiseFilter(void) @@ -231,7 +284,8 @@ bool Ekf::initialiseFilter(void) // Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter // Sum the IMU delta angle measurements - _delVel_sum += _imu_down_sampled.delta_vel; + imuSample imu_init = _imu_buffer.get_newest(); + _delVel_sum += imu_init.delta_vel; // Sum the magnetometer measurements magSample mag_init = _mag_buffer.get_newest(); @@ -241,17 +295,26 @@ bool Ekf::initialiseFilter(void) _mag_sum += mag_init.mag; } - // Sum the barometer measurements - // initialize vertical position with newest baro measurement - baroSample baro_init = _baro_buffer.get_newest(); + if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + rangeSample range_init = _range_buffer.get_newest(); - if (baro_init.time_us != 0) { - _baro_counter ++; - _baro_sum += baro_init.hgt; + if (range_init.time_us != 0) { + _hgt_counter ++; + _hgt_sum += range_init.rng; + } + + } else { + // initialize vertical position with newest baro measurement + baroSample baro_init = _baro_buffer.get_newest(); + + if (baro_init.time_us != 0) { + _hgt_counter ++; + _hgt_sum += baro_init.hgt; + } } - // check to see if we have enough measurements and return false if not - if (_baro_counter < 10 || _mag_counter < 10) { + // check to see if we have enough measruements and return false if not + if (_hgt_counter < 10 || _mag_counter < 10) { return false; } else { @@ -283,16 +346,18 @@ bool Ekf::initialiseFilter(void) matrix::Euler euler_init(roll, pitch, 0.0f); _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; - _control_status.flags.tilt_align = true; + + // initialise the filtered alignment error estimate to a larger starting value + _tilt_err_length_filt = 1.0f; // calculate the averaged magnetometer reading Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter))); // calculate the initial magnetic field and yaw alignment - _control_status.flags.yaw_align = resetMagHeading(mag_init); + resetMagHeading(mag_init); // calculate the averaged barometer reading - _baro_at_alignment = _baro_sum / (float)_baro_counter; + _hgt_at_alignment = _hgt_sum / (float)_hgt_counter; // set the velocity to the GPS measurement (by definition, the initial position and height is at the origin) resetVelocity(); @@ -300,6 +365,9 @@ bool Ekf::initialiseFilter(void) // initialise the state covariance matrix initialiseCovariance(); + // initialise the terrain estimator + initHagl(); + return true; } } @@ -354,7 +422,6 @@ bool Ekf::collect_imu(imuSample &imu) _imu_down_sampled.delta_ang_dt += imu.delta_ang_dt; _imu_down_sampled.delta_vel_dt += imu.delta_vel_dt; - Quaternion delta_q; delta_q.rotate(imu.delta_ang); _q_down_sampled = _q_down_sampled * delta_q; @@ -453,8 +520,3 @@ void Ekf::fuseAirspeed() { } - -void Ekf::fuseRange() -{ - -} From b3b0f1347a06097c9f7cc0bd346f844a405e77ff Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:56:48 +1100 Subject: [PATCH 11/23] EKF: Make normal GPS mode the default --- EKF/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 97db40d1ab..869e431c45 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -214,7 +214,7 @@ struct parameters { parameters() { // measurement source control - fusion_mode = MASK_USE_OF; + fusion_mode = MASK_USE_GPS; vdist_sensor_type = VDIST_SENSOR_BARO; // measurement time delays From dd1d58bab56de2b93aa5c461e82f82001e421ed6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 22:41:38 +1100 Subject: [PATCH 12/23] EKF: Remove unnecessary matrix operations from optical flow fusion The updated formulation means that H_LOS[][8] is always zero, so these operations are no longer required. --- EKF/optflow_fusion.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 40da07635d..f4aff85dbc 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -456,8 +456,6 @@ void Ekf::fuseOptFlow() for (unsigned column = 0; column <= 5; column++) { KH[row][column] = gain[row] * H_LOS[obs_index][column]; } - - KH[row][8] = gain[row] * H_LOS[obs_index][8]; } for (unsigned row = 0; row < _k_num_states; row++) { @@ -468,7 +466,6 @@ void Ekf::fuseOptFlow() tmp += KH[row][3] * P[3][column]; tmp += KH[row][4] * P[4][column]; tmp += KH[row][5] * P[5][column]; - tmp += KH[row][8] * P[8][column]; KHP[row][column] = tmp; } } From 5acd1cbac4423b4a14bdefe7c8652cca382c69ed Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 8 Mar 2016 09:16:50 +1100 Subject: [PATCH 13/23] EKF: Make definitions of parameters clearer for external use --- EKF/common.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 869e431c45..83c709cc67 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -122,12 +122,10 @@ struct flowSample { uint64_t time_us; // timestamp in microseconds of the integration period mid-point }; -enum vdist_sensor_type_t { - VDIST_SENSOR_RANGE, - VDIST_SENSOR_BARO, - VDIST_SENSOR_GPS, - VDIST_SENSOR_NONE -}; +// Integer definitions for vdist_sensor_type +#define VDIST_SENSOR_BARO 0 // Use baro height +#define VDIST_SENSOR_GPS 1 // Use GPS height +#define VDIST_SENSOR_RANGE 2 // Use range finder height // Bit locations for mag_declination_source #define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value @@ -146,8 +144,8 @@ enum vdist_sensor_type_t { struct parameters { // measurement source control - int fusion_mode; - int vdist_sensor_type; + int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used + int vdist_sensor_type; // selects the primary source for height data // measurement time delays float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec) From ffebaf384fd3ab75f5f34ffb6d087e30b4613f5d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 8 Mar 2016 13:28:45 +1100 Subject: [PATCH 14/23] EKF: Set initial optical flow fusion monitor outputs to zero --- EKF/ekf.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 9565388ca4..b696472266 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -86,8 +86,10 @@ Ekf::Ekf(): _R_prev = matrix::Dcm(); memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov)); memset(_mag_innov, 0, sizeof(_mag_innov)); + memset(_flow_innov, 0, sizeof(_flow_innov)); memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var)); memset(_mag_innov_var, 0, sizeof(_mag_innov_var)); + memset(_flow_innov_var, 0, sizeof(_flow_innov_var)); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); @@ -97,7 +99,7 @@ Ekf::Ekf(): _mag_sum = {}; _delVel_sum = {}; _flow_gyro_bias = {}; - _imu_del_ang_of = {}; + _imu_del_ang_of = {}; } Ekf::~Ekf() From e9ccfdd4849801f85d7568c95959885cc47208a0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 8 Mar 2016 16:39:47 +1100 Subject: [PATCH 15/23] EKF: Update derivation scripts and outputs Work around limitation in Symbolic toolbox environment space by incrementally saving and clearing workspace Remove vehicle vertical position from optical flow prediction equations (use range measurement instead) and regenerate auto-code Remove legacy optical flow auto-code conversion method as it is not required now a work around for symbolic toolbox limitations has been developed Fix out of date syntax --- .../Optical Flow Fusion Alternative.txt | 281 --------------- .../Inertial Nav EKF/Optical Flow Fusion.txt | 319 ++++++++++++------ matlab/scripts/Inertial Nav EKF/ConvertToC.m | 2 +- matlab/scripts/Inertial Nav EKF/ConvertToM.m | 2 +- .../GenerateNavFilterEquations.m | 260 ++++++++------ .../scripts/Inertial Nav EKF/SaveScriptCode.m | 23 +- matlab/scripts/Inertial Nav EKF/fix_c_code.m | 92 +++++ 7 files changed, 489 insertions(+), 490 deletions(-) delete mode 100644 matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt create mode 100644 matlab/scripts/Inertial Nav EKF/fix_c_code.m diff --git a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt deleted file mode 100644 index fbe105bf32..0000000000 --- a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt +++ /dev/null @@ -1,281 +0,0 @@ -// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis -// Generated using MAtlab in-built symbolic toolbox to c-code function -// Observations are body modtion compensated optica flow rates about the X and Y body axis -// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated) - -// intermediate variable from algebraic optimisation -float SH_LOS[14]; -SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); -SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2); -SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2); -SH_LOS[3] = 1/(pd - ptd); -SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3); -SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3; -SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3; -SH_LOS[7] = q0*q0; -SH_LOS[8] = q1*q1; -SH_LOS[9] = q2*q2; -SH_LOS[10] = q3*q3; -SH_LOS[11] = q0*q3*2.0f; -SH_LOS[12] = pd-ptd; -SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]); - -// Calculate the observation jacobians for the LOS rate about the X body axis -float H_LOS[24]; -H_LOS[0] = SH_LOS[3]*SH_LOS[2]*SH_LOS[6]-SH_LOS[3]*SH_LOS[0]*SH_LOS[4]; -H_LOS[1] = SH_LOS[3]*SH_LOS[2]*SH_LOS[5]; -H_LOS[2] = SH_LOS[3]*SH_LOS[0]*SH_LOS[1]; -H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]-q1*q2*2.0f); -H_LOS[4] = -SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]-SH_LOS[8]+SH_LOS[9]-SH_LOS[10]); -H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6]; -H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13]; - -// Calculate the observation jacobians for the LOS rate about the Y body axis -float H_LOS[24]; -H_LOS[0] = -SH_LOS[3]*SH_LOS[6]*SH_LOS[1]; -H_LOS[1] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[4]-SH_LOS[3]*SH_LOS[1]*SH_LOS[5]; -H_LOS[2] = SH_LOS[3]*SH_LOS[2]*SH_LOS[0]; -H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]+SH_LOS[8]-SH_LOS[9]-SH_LOS[10]); -H_LOS[4] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]+q1*q2*2.0f); -H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5]; -H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13]; - -// Intermediate variables used to calculate the Kalman gain matrices for the LOS rate about the X body axis -float t2 = SH_LOS[3]; -float t3 = SH_LOS[0]; -float t4 = SH_LOS[2]; -float t5 = SH_LOS[6]; -float t100 = t2 * t3 * t5; -float t6 = SH_LOS[4]; -float t7 = t2*t3*t6; -float t9 = t2*t4*t5; -float t8 = t7-t9; -float t10 = q0*q3*2.0f; -float t21 = q1*q2*2.0f; -float t11 = t10-t21; -float t101 = t2 * t3 * t11; -float t12 = pd-ptd; -float t13 = 1.0f/(t12*t12); -float t104 = t3 * t4 * t13; -float t14 = SH_LOS[5]; -float t102 = t2 * t4 * t14; -float t15 = SH_LOS[1]; -float t103 = t2 * t3 * t15; -float t16 = q0*q0; -float t17 = q1*q1; -float t18 = q2*q2; -float t19 = q3*q3; -float t20 = t16-t17+t18-t19; -float t105 = t2 * t3 * t20; -float t22 = P[1][1]*t102; -float t23 = P[3][0]*t101; -float t24 = P[8][0]*t104; -float t25 = P[1][0]*t102; -float t26 = P[2][0]*t103; -float t63 = P[0][0]*t8; -float t64 = P[5][0]*t100; -float t65 = P[4][0]*t105; -float t27 = t23+t24+t25+t26-t63-t64-t65; -float t28 = P[3][3]*t101; -float t29 = P[8][3]*t104; -float t30 = P[1][3]*t102; -float t31 = P[2][3]*t103; -float t67 = P[0][3]*t8; -float t68 = P[5][3]*t100; -float t69 = P[4][3]*t105; -float t32 = t28+t29+t30+t31-t67-t68-t69; -float t33 = t101*t32; -float t34 = P[3][8]*t101; -float t35 = P[8][8]*t104; -float t36 = P[1][8]*t102; -float t37 = P[2][8]*t103; -float t70 = P[0][8]*t8; -float t71 = P[5][8]*t100; -float t72 = P[4][8]*t105; -float t38 = t34+t35+t36+t37-t70-t71-t72; -float t39 = t104*t38; -float t40 = P[3][1]*t101; -float t41 = P[8][1]*t104; -float t42 = P[2][1]*t103; -float t73 = P[0][1]*t8; -float t74 = P[5][1]*t100; -float t75 = P[4][1]*t105; -float t43 = t22+t40+t41+t42-t73-t74-t75; -float t44 = t102*t43; -float t45 = P[3][2]*t101; -float t46 = P[8][2]*t104; -float t47 = P[1][2]*t102; -float t48 = P[2][2]*t103; -float t76 = P[0][2]*t8; -float t77 = P[5][2]*t100; -float t78 = P[4][2]*t105; -float t49 = t45+t46+t47+t48-t76-t77-t78; -float t50 = t103*t49; -float t51 = P[3][5]*t101; -float t52 = P[8][5]*t104; -float t53 = P[1][5]*t102; -float t54 = P[2][5]*t103; -float t79 = P[0][5]*t8; -float t80 = P[5][5]*t100; -float t81 = P[4][5]*t105; -float t55 = t51+t52+t53+t54-t79-t80-t81; -float t56 = P[3][4]*t101; -float t57 = P[8][4]*t104; -float t58 = P[1][4]*t102; -float t59 = P[2][4]*t103; -float t83 = P[0][4]*t8; -float t84 = P[5][4]*t100; -float t85 = P[4][4]*t105; -float t60 = t56+t57+t58+t59-t83-t84-t85; -float t66 = t8*t27; -float t82 = t100*t55; -float t86 = t105*t60; -float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86; // innovation variance - should always be >= R_LOS -float t62 = 1.0f/t61; - -// Calculate the Kalman gain matrix for the LOS rate about the X body axis -float Kfusion[24]; -Kfusion[0] = t62*(-P[0][0]*t8-P[0][5]*t100+P[0][3]*t101+P[0][1]*t102+P[0][2]*t103+P[0][8]*t104-P[0][4]*t105); -Kfusion[1] = t62*(t22-P[1][0]*t8-P[1][5]*t100+P[1][3]*t101+P[1][2]*t103+P[1][8]*t104-P[1][4]*t105); -Kfusion[2] = t62*(t48-P[2][0]*t8-P[2][5]*t100+P[2][3]*t101+P[2][1]*t102+P[2][8]*t104-P[2][4]*t105); -Kfusion[3] = t62*(t28-P[3][0]*t8-P[3][5]*t100+P[3][1]*t102+P[3][2]*t103+P[3][8]*t104-P[3][4]*t105); -Kfusion[4] = t62*(-t85-P[4][0]*t8-P[4][5]*t100+P[4][3]*t101+P[4][1]*t102+P[4][2]*t103+P[4][8]*t104); -Kfusion[5] = t62*(-t80-P[5][0]*t8+P[5][3]*t101+P[5][1]*t102+P[5][2]*t103+P[5][8]*t104-P[5][4]*t105); -Kfusion[6] = t62*(-P[6][0]*t8-P[6][5]*t100+P[6][3]*t101+P[6][1]*t102+P[6][2]*t103+P[6][8]*t104-P[6][4]*t105); -Kfusion[7] = t62*(-P[7][0]*t8-P[7][5]*t100+P[7][3]*t101+P[7][1]*t102+P[7][2]*t103+P[7][8]*t104-P[7][4]*t105); -Kfusion[8] = t62*(t35-P[8][0]*t8-P[8][5]*t100+P[8][3]*t101+P[8][1]*t102+P[8][2]*t103-P[8][4]*t105); -Kfusion[9] = t62*(-P[9][0]*t8-P[9][5]*t100+P[9][3]*t101+P[9][1]*t102+P[9][2]*t103+P[9][8]*t104-P[9][4]*t105); -Kfusion[10] = t62*(-P[10][0]*t8-P[10][5]*t100+P[10][3]*t101+P[10][1]*t102+P[10][2]*t103+P[10][8]*t104-P[10][4]*t105); -Kfusion[11] = t62*(-P[11][0]*t8-P[11][5]*t100+P[11][3]*t101+P[11][1]*t102+P[11][2]*t103+P[11][8]*t104-P[11][4]*t105); -Kfusion[12] = t62*(-P[12][0]*t8-P[12][5]*t100+P[12][3]*t101+P[12][1]*t102+P[12][2]*t103+P[12][8]*t104-P[12][4]*t105); -Kfusion[13] = t62*(-P[13][0]*t8-P[13][5]*t100+P[13][3]*t101+P[13][1]*t102+P[13][2]*t103+P[13][8]*t104-P[13][4]*t105); -Kfusion[14] = t62*(-P[14][0]*t8-P[14][5]*t100+P[14][3]*t101+P[14][1]*t102+P[14][2]*t103+P[14][8]*t104-P[14][4]*t105); -Kfusion[15] = t62*(-P[15][0]*t8-P[15][5]*t100+P[15][3]*t101+P[15][1]*t102+P[15][2]*t103+P[15][8]*t104-P[15][4]*t105); -Kfusion[16] = t62*(-P[16][0]*t8-P[16][5]*t100+P[16][3]*t101+P[16][1]*t102+P[16][2]*t103+P[16][8]*t104-P[16][4]*t105); -Kfusion[17] = t62*(-P[17][0]*t8-P[17][5]*t100+P[17][3]*t101+P[17][1]*t102+P[17][2]*t103+P[17][8]*t104-P[17][4]*t105); -Kfusion[18] = t62*(-P[18][0]*t8-P[18][5]*t100+P[18][3]*t101+P[18][1]*t102+P[18][2]*t103+P[18][8]*t104-P[18][4]*t105); -Kfusion[19] = t62*(-P[19][0]*t8-P[19][5]*t100+P[19][3]*t101+P[19][1]*t102+P[19][2]*t103+P[19][8]*t104-P[19][4]*t105); -Kfusion[20] = t62*(-P[20][0]*t8-P[20][5]*t100+P[20][3]*t101+P[20][1]*t102+P[20][2]*t103+P[20][8]*t104-P[20][4]*t105); -Kfusion[21] = t62*(-P[21][0]*t8-P[21][5]*t100+P[21][3]*t101+P[21][1]*t102+P[21][2]*t103+P[21][8]*t104-P[21][4]*t105); -Kfusion[22] = t62*(-P[22][0]*t8-P[22][5]*t100+P[22][3]*t101+P[22][1]*t102+P[22][2]*t103+P[22][8]*t104-P[22][4]*t105); -Kfusion[23] = t62*(-P[23][0]*t8-P[23][5]*t100+P[23][3]*t101+P[23][1]*t102+P[23][2]*t103+P[23][8]*t104-P[23][4]*t105); - -// Intermediate variables used to calculate the Kalman gain matrices for the LOS rate about the Y body axis -float t2 = SH_LOS[3]; -float t3 = SH_LOS[0]; -float t4 = SH_LOS[1]; -float t5 = SH_LOS[5]; -float t100 = t2 * t3 * t5; -float t6 = SH_LOS[4]; -float t7 = t2*t3*t6; -float t8 = t2*t4*t5; -float t9 = t7+t8; -float t10 = q0*q3*2.0f; -float t11 = q1*q2*2.0f; -float t12 = t10+t11; -float t101 = t2 * t3 * t12; -float t13 = pd-ptd; -float t14 = 1.0f/(t13*t13); -float t104 = t3 * t4 * t14; -float t15 = SH_LOS[6]; -float t105 = t2 * t4 * t15; -float t16 = SH_LOS[2]; -float t102 = t2 * t3 * t16; -float t17 = q0*q0; -float t18 = q1*q1; -float t19 = q2*q2; -float t20 = q3*q3; -float t21 = t17+t18-t19-t20; -float t103 = t2 * t3 * t21; -float t22 = P[0][0]*t105; -float t23 = P[1][1]*t9; -float t24 = P[8][1]*t104; -float t25 = P[0][1]*t105; -float t26 = P[5][1]*t100; -float t64 = P[4][1]*t101; -float t65 = P[2][1]*t102; -float t66 = P[3][1]*t103; -float t27 = t23+t24+t25+t26-t64-t65-t66; -float t28 = t9*t27; -float t29 = P[1][4]*t9; -float t30 = P[8][4]*t104; -float t31 = P[0][4]*t105; -float t32 = P[5][4]*t100; -float t67 = P[4][4]*t101; -float t68 = P[2][4]*t102; -float t69 = P[3][4]*t103; -float t33 = t29+t30+t31+t32-t67-t68-t69; -float t34 = P[1][8]*t9; -float t35 = P[8][8]*t104; -float t36 = P[0][8]*t105; -float t37 = P[5][8]*t100; -float t71 = P[4][8]*t101; -float t72 = P[2][8]*t102; -float t73 = P[3][8]*t103; -float t38 = t34+t35+t36+t37-t71-t72-t73; -float t39 = t104*t38; -float t40 = P[1][0]*t9; -float t41 = P[8][0]*t104; -float t42 = P[5][0]*t100; -float t74 = P[4][0]*t101; -float t75 = P[2][0]*t102; -float t76 = P[3][0]*t103; -float t43 = t22+t40+t41+t42-t74-t75-t76; -float t44 = t105*t43; -float t45 = P[1][2]*t9; -float t46 = P[8][2]*t104; -float t47 = P[0][2]*t105; -float t48 = P[5][2]*t100; -float t63 = P[2][2]*t102; -float t77 = P[4][2]*t101; -float t78 = P[3][2]*t103; -float t49 = t45+t46+t47+t48-t63-t77-t78; -float t50 = P[1][5]*t9; -float t51 = P[8][5]*t104; -float t52 = P[0][5]*t105; -float t53 = P[5][5]*t100; -float t80 = P[4][5]*t101; -float t81 = P[2][5]*t102; -float t82 = P[3][5]*t103; -float t54 = t50+t51+t52+t53-t80-t81-t82; -float t55 = t100*t54; -float t56 = P[1][3]*t9; -float t57 = P[8][3]*t104; -float t58 = P[0][3]*t105; -float t59 = P[5][3]*t100; -float t83 = P[4][3]*t101; -float t84 = P[2][3]*t102; -float t85 = P[3][3]*t103; -float t60 = t56+t57+t58+t59-t83-t84-t85; -float t70 = t101*t33; -float t79 = t102*t49; -float t86 = t103*t60; -float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86; // innovation variance - should always be >= R_LOS -float t62 = 1.0f/t61; - -// Calculate the Kalman gain matrix for the LOS rate about the Y body axis -float Kfusion[24]; -Kfusion[0] = -t62*(t22+P[0][1]*t9+P[0][5]*t100-P[0][4]*t101-P[0][2]*t102-P[0][3]*t103+P[0][8]*t104); -Kfusion[1] = -t62*(t23+P[1][5]*t100+P[1][0]*t105-P[1][4]*t101-P[1][2]*t102-P[1][3]*t103+P[1][8]*t104); -Kfusion[2] = -t62*(-t63+P[2][1]*t9+P[2][5]*t100+P[2][0]*t105-P[2][4]*t101-P[2][3]*t103+P[2][8]*t104); -Kfusion[3] = -t62*(-t85+P[3][1]*t9+P[3][5]*t100+P[3][0]*t105-P[3][4]*t101-P[3][2]*t102+P[3][8]*t104); -Kfusion[4] = -t62*(-t67+P[4][1]*t9+P[4][5]*t100+P[4][0]*t105-P[4][2]*t102-P[4][3]*t103+P[4][8]*t104); -Kfusion[5] = -t62*(t53+P[5][1]*t9+P[5][0]*t105-P[5][4]*t101-P[5][2]*t102-P[5][3]*t103+P[5][8]*t104); -Kfusion[6] = -t62*(P[6][1]*t9+P[6][5]*t100+P[6][0]*t105-P[6][4]*t101-P[6][2]*t102-P[6][3]*t103+P[6][8]*t104); -Kfusion[7] = -t62*(P[7][1]*t9+P[7][5]*t100+P[7][0]*t105-P[7][4]*t101-P[7][2]*t102-P[7][3]*t103+P[7][8]*t104); -Kfusion[8] = -t62*(t35+P[8][1]*t9+P[8][5]*t100+P[8][0]*t105-P[8][4]*t101-P[8][2]*t102-P[8][3]*t103); -Kfusion[9] = -t62*(P[9][1]*t9+P[9][5]*t100+P[9][0]*t105-P[9][4]*t101-P[9][2]*t102-P[9][3]*t103+P[9][8]*t104); -Kfusion[10] = -t62*(P[10][1]*t9+P[10][5]*t100+P[10][0]*t105-P[10][4]*t101-P[10][2]*t102-P[10][3]*t103+P[10][8]*t104); -Kfusion[11] = -t62*(P[11][1]*t9+P[11][5]*t100+P[11][0]*t105-P[11][4]*t101-P[11][2]*t102-P[11][3]*t103+P[11][8]*t104); -Kfusion[12] = -t62*(P[12][1]*t9+P[12][5]*t100+P[12][0]*t105-P[12][4]*t101-P[12][2]*t102-P[12][3]*t103+P[12][8]*t104); -Kfusion[13] = -t62*(P[13][1]*t9+P[13][5]*t100+P[13][0]*t105-P[13][4]*t101-P[13][2]*t102-P[13][3]*t103+P[13][8]*t104); -Kfusion[14] = -t62*(P[14][1]*t9+P[14][5]*t100+P[14][0]*t105-P[14][4]*t101-P[14][2]*t102-P[14][3]*t103+P[14][8]*t104); -Kfusion[15] = -t62*(P[15][1]*t9+P[15][5]*t100+P[15][0]*t105-P[15][4]*t101-P[15][2]*t102-P[15][3]*t103+P[15][8]*t104); -Kfusion[16] = -t62*(P[16][1]*t9+P[16][5]*t100+P[16][0]*t105-P[16][4]*t101-P[16][2]*t102-P[16][3]*t103+P[16][8]*t104); -Kfusion[17] = -t62*(P[17][1]*t9+P[17][5]*t100+P[17][0]*t105-P[17][4]*t101-P[17][2]*t102-P[17][3]*t103+P[17][8]*t104); -Kfusion[18] = -t62*(P[18][1]*t9+P[18][5]*t100+P[18][0]*t105-P[18][4]*t101-P[18][2]*t102-P[18][3]*t103+P[18][8]*t104); -Kfusion[19] = -t62*(P[19][1]*t9+P[19][5]*t100+P[19][0]*t105-P[19][4]*t101-P[19][2]*t102-P[19][3]*t103+P[19][8]*t104); -Kfusion[20] = -t62*(P[20][1]*t9+P[20][5]*t100+P[20][0]*t105-P[20][4]*t101-P[20][2]*t102-P[20][3]*t103+P[20][8]*t104); -Kfusion[21] = -t62*(P[21][1]*t9+P[21][5]*t100+P[21][0]*t105-P[21][4]*t101-P[21][2]*t102-P[21][3]*t103+P[21][8]*t104); -Kfusion[22] = -t62*(P[22][1]*t9+P[22][5]*t100+P[22][0]*t105-P[22][4]*t101-P[22][2]*t102-P[22][3]*t103+P[22][8]*t104); -Kfusion[23] = -t62*(P[23][1]*t9+P[23][5]*t100+P[23][0]*t105-P[23][4]*t101-P[23][2]*t102-P[23][3]*t103+P[23][8]*t104); diff --git a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt index 5320af7b77..c1f76bf491 100644 --- a/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt @@ -3,114 +3,219 @@ // Observations are body modtion compensated optica flow rates about the X and Y body axis // Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated) -// intermediate variable from algebraic optimisation -float SH_LOS[7]; -SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); -SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + ve*(2.0f*q0*q3 + 2.0f*q1*q2); -SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - vn*(2.0f*q0*q3 - 2.0f*q1*q2); -SH_LOS[3] = 1.0f/(pd - ptd); -SH_LOS[4] = vd*SH_LOS[0] - ve*(2.0f*q0*q1 - 2.0f*q2*q3) + vn*(2.0f*q0*q2 + 2.0f*q1*q3); -SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3; -SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3; +float H_LOS[2][24]; -// Calculate the observation jacobians for the LOS rate about the X body axis -float H_LOS[24]; -H_LOS[0] = SH_LOS[2]*SH_LOS[3]*SH_LOS[6] - SH_LOS[0]*SH_LOS[3]*SH_LOS[4]; -H_LOS[1] = SH_LOS[2]*SH_LOS[3]*SH_LOS[5]; -H_LOS[2] = SH_LOS[0]*SH_LOS[1]*SH_LOS[3]; -H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2); -H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); -H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[6]; -H_LOS[8] = SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]); +// calculate X axis observation Jacobian +float t2 = 1.0f / range; +float t3 = q0 * q0; +float t4 = q1 * q1; +float t5 = q2 * q2; +float t6 = q3 * q3; +float t7 = q0 * q2 * 2.0f; +float t8 = q1 * q3 * 2.0f; +float t9 = q0 * q3 * 2.0f; +float t10 = q1 * q2 * 2.0f; +float t11 = q0 * q1 * 2.0f; +H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f)); +H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6)); +H_LOS[0][3] = -t2 * (t9 - t10); +H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6); +H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f); -// Calculate the observation jacobians for the LOS rate about the Y body axis -float H_LOS[24]; -H_LOS[0] = -SH_LOS[1]*SH_LOS[3]*SH_LOS[6]; -H_LOS[1] = - SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[1]*SH_LOS[3]*SH_LOS[5]; -H_LOS[2] = SH_LOS[0]*SH_LOS[2]*SH_LOS[3]; -H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)); -H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2); -H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[5]; -H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]); +// calculate X axis Kalman gains +t2 = 1.0/range; +t3 = q0*q1*2.0; +t4 = q2*q3*2.0; +t5 = q0*q0; +t6 = q1*q1; +t7 = q2*q2; +t8 = q3*q3; +t9 = q0*q2*2.0; +t10 = q1*q3*2.0; +t11 = q0*q3*2.0; +float t12 = q1*q2*2.0; +float t13 = t11-t12; +float t14 = t3+t4; +float t15 = t5-t6-t7+t8; +float t16 = t15*vd; +float t17 = t3-t4; +float t18 = t9+t10; +float t19 = t18*vn; +float t28 = t17*ve; +float t20 = t16+t19-t28; +float t21 = t5+t6-t7-t8; +float t22 = t21*vn; +float t23 = t9-t10; +float t24 = t11+t12; +float t25 = t24*ve; +float t29 = t23*vd; +float t26 = t22+t25-t29; +float t27 = t5-t6+t7-t8; +float t30 = P[0][0]*t2*t20; +float t31 = P[5][3]*t2*t14; +float t32 = P[0][3]*t2*t20; +float t33 = P[4][3]*t2*t27; +float t56 = P[3][3]*t2*t13; +float t57 = P[2][3]*t2*t26; +float t34 = t31+t32+t33-t56-t57; +float t35 = P[5][5]*t2*t14; +float t36 = P[0][5]*t2*t20; +float t37 = P[4][5]*t2*t27; +float t59 = P[3][5]*t2*t13; +float t60 = P[2][5]*t2*t26; +float t38 = t35+t36+t37-t59-t60; +float t39 = t2*t14*t38; +float t40 = P[5][0]*t2*t14; +float t41 = P[4][0]*t2*t27; +float t61 = P[3][0]*t2*t13; +float t62 = P[2][0]*t2*t26; +float t42 = t30+t40+t41-t61-t62; +float t43 = t2*t20*t42; +float t44 = P[5][2]*t2*t14; +float t45 = P[0][2]*t2*t20; +float t46 = P[4][2]*t2*t27; +float t55 = P[2][2]*t2*t26; +float t63 = P[3][2]*t2*t13; +float t47 = t44+t45+t46-t55-t63; +float t48 = P[5][4]*t2*t14; +float t49 = P[0][4]*t2*t20; +float t50 = P[4][4]*t2*t27; +float t65 = P[3][4]*t2*t13; +float t66 = P[2][4]*t2*t26; +float t51 = t48+t49+t50-t65-t66; +float t52 = t2*t27*t51; +float t58 = t2*t13*t34; +float t64 = t2*t26*t47; +float t53 = R_LOS+t39+t43+t52-t58-t64; +float t54 = 1.0/t53; +Kfusion[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27); +Kfusion[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27); +Kfusion[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27); +Kfusion[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27); +Kfusion[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26); +Kfusion[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27); +Kfusion[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27); +Kfusion[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27); +Kfusion[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27); +Kfusion[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27); +Kfusion[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27); +Kfusion[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27); +Kfusion[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27); +Kfusion[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27); +Kfusion[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27); +Kfusion[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27); +Kfusion[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27); +Kfusion[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27); +Kfusion[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27); +Kfusion[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27); +Kfusion[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27); +Kfusion[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27); +Kfusion[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27); +Kfusion[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27); +// calculate Y axis observation jacobian +float t2 = 1.0f/range; +float t3 = q0*q0; +float t4 = q1*q1; +float t5 = q2*q2; +float t6 = q3*q3; +float t7 = q0*q1*2.0f; +float t8 = q0*q3*2.0f; +float t9 = q0*q2*2.0f; +float t10 = q1*q3*2.0f; +H_LOS[1][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0f)); +H_LOS[1][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0f)-vn*(t8-q1*q2*2.0f)); +H_LOS[1][3] = -t2*(t3+t4-t5-t6); +H_LOS[1][4] = -t2*(t8+q1*q2*2.0f); +H_LOS[1][5] = t2*(t9-t10); -// Intermediate variables used to calculate the Kalman gain matrices -float SK_LOS[22]; -// this is 1/(innovation variance) for the X axis measurement -SK_LOS[0] = 1.0f/(R_LOS - (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6])*(P[8][0]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][0]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][0]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[2]*SH_LOS[3]*SH_LOS[5]*(P[8][1]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][1]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][1]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[3]*(P[8][2]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][2]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][2]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*SH_LOS[6]*(P[8][5]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][5]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][5]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[8][4]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][4]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][4]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3])*(P[8][8]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][8]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][8]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][8]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2)*(P[8][3]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][3]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][3]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)))); -// this is 1/(innovation variance) for the Y axis measurement -SK_LOS[1] = 1.0f/(R_LOS + (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5])*(P[1][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][1]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][1]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[1]*SH_LOS[3]*SH_LOS[6]*(P[1][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][0]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][0]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[3]*(P[1][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][2]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][2]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*SH_LOS[5]*(P[1][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][5]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][5]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[1][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][3]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][3]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3])*(P[1][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][8]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][8]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][8]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][8]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2)*(P[1][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][4]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][4]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)))); -SK_LOS[2] = sq(q0) + sq(q1) - sq(q2) - sq(q3); -SK_LOS[3] = sq(q0) - sq(q1) + sq(q2) - sq(q3); -SK_LOS[4] = SH_LOS[3]; -SK_LOS[5] = SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]); -SK_LOS[6] = SH_LOS[0]*SH_LOS[4]*SK_LOS[4]; -SK_LOS[7] = SH_LOS[2]*SH_LOS[6]*SK_LOS[4]; -SK_LOS[8] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 - 2.0f*q1*q2); -SK_LOS[9] = SH_LOS[0]*SH_LOS[1]*SK_LOS[4]; -SK_LOS[10] = SH_LOS[2]*SH_LOS[5]*SK_LOS[4]; -SK_LOS[11] = SH_LOS[0]*SH_LOS[6]*SK_LOS[4]; -SK_LOS[12] = SH_LOS[0]*SK_LOS[3]*SK_LOS[4]; -SK_LOS[13] = SH_LOS[1]*SH_LOS[5]*SK_LOS[4]; -SK_LOS[14] = SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]); -SK_LOS[15] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 + 2.0f*q1*q2); -SK_LOS[16] = SH_LOS[0]*SH_LOS[2]*SK_LOS[4]; -SK_LOS[17] = SH_LOS[1]*SH_LOS[6]*SK_LOS[4]; -SK_LOS[18] = SH_LOS[0]*SH_LOS[5]*SK_LOS[4]; -SK_LOS[19] = SH_LOS[0]*SK_LOS[2]*SK_LOS[4]; -SK_LOS[20] = SK_LOS[6] - SK_LOS[7]; -SK_LOS[21] = SK_LOS[6] + SK_LOS[13]; - -// Calculate the Kalman gain matrix for the X axis measurement -float Kfusion[24]; -Kfusion[0] = SK_LOS[0]*(P[0][8]*SK_LOS[5] - P[0][0]*SK_LOS[20] + P[0][3]*SK_LOS[8] + P[0][2]*SK_LOS[9] + P[0][1]*SK_LOS[10] - P[0][5]*SK_LOS[11] - P[0][4]*SK_LOS[12]); -Kfusion[1] = SK_LOS[0]*(P[1][8]*SK_LOS[5] - P[1][0]*SK_LOS[20] + P[1][3]*SK_LOS[8] + P[1][2]*SK_LOS[9] + P[1][1]*SK_LOS[10] - P[1][5]*SK_LOS[11] - P[1][4]*SK_LOS[12]); -Kfusion[2] = SK_LOS[0]*(P[2][8]*SK_LOS[5] - P[2][0]*SK_LOS[20] + P[2][3]*SK_LOS[8] + P[2][2]*SK_LOS[9] + P[2][1]*SK_LOS[10] - P[2][5]*SK_LOS[11] - P[2][4]*SK_LOS[12]); -Kfusion[3] = SK_LOS[0]*(P[3][8]*SK_LOS[5] - P[3][0]*SK_LOS[20] + P[3][3]*SK_LOS[8] + P[3][2]*SK_LOS[9] + P[3][1]*SK_LOS[10] - P[3][5]*SK_LOS[11] - P[3][4]*SK_LOS[12]); -Kfusion[4] = SK_LOS[0]*(P[4][8]*SK_LOS[5] - P[4][0]*SK_LOS[20] + P[4][3]*SK_LOS[8] + P[4][2]*SK_LOS[9] + P[4][1]*SK_LOS[10] - P[4][5]*SK_LOS[11] - P[4][4]*SK_LOS[12]); -Kfusion[5] = SK_LOS[0]*(P[5][8]*SK_LOS[5] - P[5][0]*SK_LOS[20] + P[5][3]*SK_LOS[8] + P[5][2]*SK_LOS[9] + P[5][1]*SK_LOS[10] - P[5][5]*SK_LOS[11] - P[5][4]*SK_LOS[12]); -Kfusion[6] = SK_LOS[0]*(P[6][8]*SK_LOS[5] - P[6][0]*SK_LOS[20] + P[6][3]*SK_LOS[8] + P[6][2]*SK_LOS[9] + P[6][1]*SK_LOS[10] - P[6][5]*SK_LOS[11] - P[6][4]*SK_LOS[12]); -Kfusion[7] = SK_LOS[0]*(P[7][8]*SK_LOS[5] - P[7][0]*SK_LOS[20] + P[7][3]*SK_LOS[8] + P[7][2]*SK_LOS[9] + P[7][1]*SK_LOS[10] - P[7][5]*SK_LOS[11] - P[7][4]*SK_LOS[12]); -Kfusion[8] = SK_LOS[0]*(P[8][8]*SK_LOS[5] - P[8][0]*SK_LOS[20] + P[8][3]*SK_LOS[8] + P[8][2]*SK_LOS[9] + P[8][1]*SK_LOS[10] - P[8][5]*SK_LOS[11] - P[8][4]*SK_LOS[12]); -Kfusion[9] = SK_LOS[0]*(P[9][8]*SK_LOS[5] - P[9][0]*SK_LOS[20] + P[9][3]*SK_LOS[8] + P[9][2]*SK_LOS[9] + P[9][1]*SK_LOS[10] - P[9][5]*SK_LOS[11] - P[9][4]*SK_LOS[12]); -Kfusion[10] = SK_LOS[0]*(P[10][8]*SK_LOS[5] - P[10][0]*SK_LOS[20] + P[10][3]*SK_LOS[8] + P[10][2]*SK_LOS[9] + P[10][1]*SK_LOS[10] - P[10][5]*SK_LOS[11] - P[10][4]*SK_LOS[12]); -Kfusion[11] = SK_LOS[0]*(P[11][8]*SK_LOS[5] - P[11][0]*SK_LOS[20] + P[11][3]*SK_LOS[8] + P[11][2]*SK_LOS[9] + P[11][1]*SK_LOS[10] - P[11][5]*SK_LOS[11] - P[11][4]*SK_LOS[12]); -Kfusion[12] = SK_LOS[0]*(P[12][8]*SK_LOS[5] - P[12][0]*SK_LOS[20] + P[12][3]*SK_LOS[8] + P[12][2]*SK_LOS[9] + P[12][1]*SK_LOS[10] - P[12][5]*SK_LOS[11] - P[12][4]*SK_LOS[12]); -Kfusion[13] = SK_LOS[0]*(P[13][8]*SK_LOS[5] - P[13][0]*SK_LOS[20] + P[13][3]*SK_LOS[8] + P[13][2]*SK_LOS[9] + P[13][1]*SK_LOS[10] - P[13][5]*SK_LOS[11] - P[13][4]*SK_LOS[12]); -Kfusion[14] = SK_LOS[0]*(P[14][8]*SK_LOS[5] - P[14][0]*SK_LOS[20] + P[14][3]*SK_LOS[8] + P[14][2]*SK_LOS[9] + P[14][1]*SK_LOS[10] - P[14][5]*SK_LOS[11] - P[14][4]*SK_LOS[12]); -Kfusion[15] = SK_LOS[0]*(P[15][8]*SK_LOS[5] - P[15][0]*SK_LOS[20] + P[15][3]*SK_LOS[8] + P[15][2]*SK_LOS[9] + P[15][1]*SK_LOS[10] - P[15][5]*SK_LOS[11] - P[15][4]*SK_LOS[12]); -Kfusion[16] = SK_LOS[0]*(P[16][8]*SK_LOS[5] - P[16][0]*SK_LOS[20] + P[16][3]*SK_LOS[8] + P[16][2]*SK_LOS[9] + P[16][1]*SK_LOS[10] - P[16][5]*SK_LOS[11] - P[16][4]*SK_LOS[12]); -Kfusion[17] = SK_LOS[0]*(P[17][8]*SK_LOS[5] - P[17][0]*SK_LOS[20] + P[17][3]*SK_LOS[8] + P[17][2]*SK_LOS[9] + P[17][1]*SK_LOS[10] - P[17][5]*SK_LOS[11] - P[17][4]*SK_LOS[12]); -Kfusion[18] = SK_LOS[0]*(P[18][8]*SK_LOS[5] - P[18][0]*SK_LOS[20] + P[18][3]*SK_LOS[8] + P[18][2]*SK_LOS[9] + P[18][1]*SK_LOS[10] - P[18][5]*SK_LOS[11] - P[18][4]*SK_LOS[12]); -Kfusion[19] = SK_LOS[0]*(P[19][8]*SK_LOS[5] - P[19][0]*SK_LOS[20] + P[19][3]*SK_LOS[8] + P[19][2]*SK_LOS[9] + P[19][1]*SK_LOS[10] - P[19][5]*SK_LOS[11] - P[19][4]*SK_LOS[12]); -Kfusion[20] = SK_LOS[0]*(P[20][8]*SK_LOS[5] - P[20][0]*SK_LOS[20] + P[20][3]*SK_LOS[8] + P[20][2]*SK_LOS[9] + P[20][1]*SK_LOS[10] - P[20][5]*SK_LOS[11] - P[20][4]*SK_LOS[12]); -Kfusion[21] = SK_LOS[0]*(P[21][8]*SK_LOS[5] - P[21][0]*SK_LOS[20] + P[21][3]*SK_LOS[8] + P[21][2]*SK_LOS[9] + P[21][1]*SK_LOS[10] - P[21][5]*SK_LOS[11] - P[21][4]*SK_LOS[12]); -Kfusion[22] = SK_LOS[0]*(P[22][8]*SK_LOS[5] - P[22][0]*SK_LOS[20] + P[22][3]*SK_LOS[8] + P[22][2]*SK_LOS[9] + P[22][1]*SK_LOS[10] - P[22][5]*SK_LOS[11] - P[22][4]*SK_LOS[12]); -Kfusion[23] = SK_LOS[0]*(P[23][8]*SK_LOS[5] - P[23][0]*SK_LOS[20] + P[23][3]*SK_LOS[8] + P[23][2]*SK_LOS[9] + P[23][1]*SK_LOS[10] - P[23][5]*SK_LOS[11] - P[23][4]*SK_LOS[12]); - -// Calculate the Kalman gain matrix for the Y axis measurement -float Kfusion[24]; -Kfusion[0] = -SK_LOS[1]*(P[0][1]*SK_LOS[21] + P[0][8]*SK_LOS[14] - P[0][4]*SK_LOS[15] - P[0][2]*SK_LOS[16] + P[0][0]*SK_LOS[17] + P[0][5]*SK_LOS[18] - P[0][3]*SK_LOS[19]); -Kfusion[1] = -SK_LOS[1]*(P[1][1]*SK_LOS[21] + P[1][8]*SK_LOS[14] - P[1][4]*SK_LOS[15] - P[1][2]*SK_LOS[16] + P[1][0]*SK_LOS[17] + P[1][5]*SK_LOS[18] - P[1][3]*SK_LOS[19]); -Kfusion[2] = -SK_LOS[1]*(P[2][1]*SK_LOS[21] + P[2][8]*SK_LOS[14] - P[2][4]*SK_LOS[15] - P[2][2]*SK_LOS[16] + P[2][0]*SK_LOS[17] + P[2][5]*SK_LOS[18] - P[2][3]*SK_LOS[19]); -Kfusion[3] = -SK_LOS[1]*(P[3][1]*SK_LOS[21] + P[3][8]*SK_LOS[14] - P[3][4]*SK_LOS[15] - P[3][2]*SK_LOS[16] + P[3][0]*SK_LOS[17] + P[3][5]*SK_LOS[18] - P[3][3]*SK_LOS[19]); -Kfusion[4] = -SK_LOS[1]*(P[4][1]*SK_LOS[21] + P[4][8]*SK_LOS[14] - P[4][4]*SK_LOS[15] - P[4][2]*SK_LOS[16] + P[4][0]*SK_LOS[17] + P[4][5]*SK_LOS[18] - P[4][3]*SK_LOS[19]); -Kfusion[5] = -SK_LOS[1]*(P[5][1]*SK_LOS[21] + P[5][8]*SK_LOS[14] - P[5][4]*SK_LOS[15] - P[5][2]*SK_LOS[16] + P[5][0]*SK_LOS[17] + P[5][5]*SK_LOS[18] - P[5][3]*SK_LOS[19]); -Kfusion[6] = -SK_LOS[1]*(P[6][1]*SK_LOS[21] + P[6][8]*SK_LOS[14] - P[6][4]*SK_LOS[15] - P[6][2]*SK_LOS[16] + P[6][0]*SK_LOS[17] + P[6][5]*SK_LOS[18] - P[6][3]*SK_LOS[19]); -Kfusion[7] = -SK_LOS[1]*(P[7][1]*SK_LOS[21] + P[7][8]*SK_LOS[14] - P[7][4]*SK_LOS[15] - P[7][2]*SK_LOS[16] + P[7][0]*SK_LOS[17] + P[7][5]*SK_LOS[18] - P[7][3]*SK_LOS[19]); -Kfusion[8] = -SK_LOS[1]*(P[8][1]*SK_LOS[21] + P[8][8]*SK_LOS[14] - P[8][4]*SK_LOS[15] - P[8][2]*SK_LOS[16] + P[8][0]*SK_LOS[17] + P[8][5]*SK_LOS[18] - P[8][3]*SK_LOS[19]); -Kfusion[9] = -SK_LOS[1]*(P[9][1]*SK_LOS[21] + P[9][8]*SK_LOS[14] - P[9][4]*SK_LOS[15] - P[9][2]*SK_LOS[16] + P[9][0]*SK_LOS[17] + P[9][5]*SK_LOS[18] - P[9][3]*SK_LOS[19]); -Kfusion[10] = -SK_LOS[1]*(P[10][1]*SK_LOS[21] + P[10][8]*SK_LOS[14] - P[10][4]*SK_LOS[15] - P[10][2]*SK_LOS[16] + P[10][0]*SK_LOS[17] + P[10][5]*SK_LOS[18] - P[10][3]*SK_LOS[19]); -Kfusion[11] = -SK_LOS[1]*(P[11][1]*SK_LOS[21] + P[11][8]*SK_LOS[14] - P[11][4]*SK_LOS[15] - P[11][2]*SK_LOS[16] + P[11][0]*SK_LOS[17] + P[11][5]*SK_LOS[18] - P[11][3]*SK_LOS[19]); -Kfusion[12] = -SK_LOS[1]*(P[12][1]*SK_LOS[21] + P[12][8]*SK_LOS[14] - P[12][4]*SK_LOS[15] - P[12][2]*SK_LOS[16] + P[12][0]*SK_LOS[17] + P[12][5]*SK_LOS[18] - P[12][3]*SK_LOS[19]); -Kfusion[13] = -SK_LOS[1]*(P[13][1]*SK_LOS[21] + P[13][8]*SK_LOS[14] - P[13][4]*SK_LOS[15] - P[13][2]*SK_LOS[16] + P[13][0]*SK_LOS[17] + P[13][5]*SK_LOS[18] - P[13][3]*SK_LOS[19]); -Kfusion[14] = -SK_LOS[1]*(P[14][1]*SK_LOS[21] + P[14][8]*SK_LOS[14] - P[14][4]*SK_LOS[15] - P[14][2]*SK_LOS[16] + P[14][0]*SK_LOS[17] + P[14][5]*SK_LOS[18] - P[14][3]*SK_LOS[19]); -Kfusion[15] = -SK_LOS[1]*(P[15][1]*SK_LOS[21] + P[15][8]*SK_LOS[14] - P[15][4]*SK_LOS[15] - P[15][2]*SK_LOS[16] + P[15][0]*SK_LOS[17] + P[15][5]*SK_LOS[18] - P[15][3]*SK_LOS[19]); -Kfusion[16] = -SK_LOS[1]*(P[16][1]*SK_LOS[21] + P[16][8]*SK_LOS[14] - P[16][4]*SK_LOS[15] - P[16][2]*SK_LOS[16] + P[16][0]*SK_LOS[17] + P[16][5]*SK_LOS[18] - P[16][3]*SK_LOS[19]); -Kfusion[17] = -SK_LOS[1]*(P[17][1]*SK_LOS[21] + P[17][8]*SK_LOS[14] - P[17][4]*SK_LOS[15] - P[17][2]*SK_LOS[16] + P[17][0]*SK_LOS[17] + P[17][5]*SK_LOS[18] - P[17][3]*SK_LOS[19]); -Kfusion[18] = -SK_LOS[1]*(P[18][1]*SK_LOS[21] + P[18][8]*SK_LOS[14] - P[18][4]*SK_LOS[15] - P[18][2]*SK_LOS[16] + P[18][0]*SK_LOS[17] + P[18][5]*SK_LOS[18] - P[18][3]*SK_LOS[19]); -Kfusion[19] = -SK_LOS[1]*(P[19][1]*SK_LOS[21] + P[19][8]*SK_LOS[14] - P[19][4]*SK_LOS[15] - P[19][2]*SK_LOS[16] + P[19][0]*SK_LOS[17] + P[19][5]*SK_LOS[18] - P[19][3]*SK_LOS[19]); -Kfusion[20] = -SK_LOS[1]*(P[20][1]*SK_LOS[21] + P[20][8]*SK_LOS[14] - P[20][4]*SK_LOS[15] - P[20][2]*SK_LOS[16] + P[20][0]*SK_LOS[17] + P[20][5]*SK_LOS[18] - P[20][3]*SK_LOS[19]); -Kfusion[21] = -SK_LOS[1]*(P[21][1]*SK_LOS[21] + P[21][8]*SK_LOS[14] - P[21][4]*SK_LOS[15] - P[21][2]*SK_LOS[16] + P[21][0]*SK_LOS[17] + P[21][5]*SK_LOS[18] - P[21][3]*SK_LOS[19]); -Kfusion[22] = -SK_LOS[1]*(P[22][1]*SK_LOS[21] + P[22][8]*SK_LOS[14] - P[22][4]*SK_LOS[15] - P[22][2]*SK_LOS[16] + P[22][0]*SK_LOS[17] + P[22][5]*SK_LOS[18] - P[22][3]*SK_LOS[19]); -Kfusion[23] = -SK_LOS[1]*(P[23][1]*SK_LOS[21] + P[23][8]*SK_LOS[14] - P[23][4]*SK_LOS[15] - P[23][2]*SK_LOS[16] + P[23][0]*SK_LOS[17] + P[23][5]*SK_LOS[18] - P[23][3]*SK_LOS[19]); +// calculate Y axis Kalman gains +t2 = 1.0f/range; +t3 = q0*q2*2.0f; +t4 = q0*q0; +t5 = q1*q1; +t6 = q2*q2; +t7 = q3*q3; +t8 = q0*q1*2.0f; +t9 = q0*q3*2.0f; +t10 = q1*q2*2.0f; +float t11 = t9+t10; +float t12 = q1*q3*2.0f; +float t13 = t4-t5-t6+t7; +float t14 = t13*vd; +float t15 = q2*q3*2.0f; +float t16 = t3+t12; +float t17 = t16*vn; +float t18 = t4-t5+t6-t7; +float t19 = t18*ve; +float t20 = t8+t15; +float t21 = t20*vd; +float t22 = t9-t10; +float t28 = t22*vn; +float t23 = t19+t21-t28; +float t24 = t4+t5-t6-t7; +float t25 = t3-t12; +float t26 = t8-t15; +float t29 = t26*ve; +float t27 = t14+t17-t29; +float t30 = P[4][4]*t2*t11; +float t31 = P[2][4]*t2*t23; +float t32 = P[3][4]*t2*t24; +float t56 = P[5][4]*t2*t25; +float t57 = P[1][4]*t2*t27; +float t33 = t30+t31+t32-t56-t57; +float t34 = t2*t11*t33; +float t35 = P[4][5]*t2*t11; +float t36 = P[2][5]*t2*t23; +float t37 = P[3][5]*t2*t24; +float t58 = P[5][5]*t2*t25; +float t59 = P[1][5]*t2*t27; +float t38 = t35+t36+t37-t58-t59; +float t39 = P[4][1]*t2*t11; +float t40 = P[2][1]*t2*t23; +float t41 = P[3][1]*t2*t24; +float t55 = P[1][1]*t2*t27; +float t61 = P[5][1]*t2*t25; +float t42 = t39+t40+t41-t55-t61; +float t43 = P[4][2]*t2*t11; +float t44 = P[2][2]*t2*t23; +float t45 = P[3][2]*t2*t24; +float t63 = P[5][2]*t2*t25; +float t64 = P[1][2]*t2*t27; +float t46 = t43+t44+t45-t63-t64; +float t47 = t2*t23*t46; +float t48 = P[4][3]*t2*t11; +float t49 = P[2][3]*t2*t23; +float t50 = P[3][3]*t2*t24; +float t65 = P[5][3]*t2*t25; +float t66 = P[1][3]*t2*t27; +float t51 = t48+t49+t50-t65-t66; +float t52 = t2*t24*t51; +float t60 = t2*t25*t38; +float t62 = t2*t27*t42; +float t53 = R_LOS+t34+t47+t52-t60-t62; +float t54 = 1.0f/t53; +Kfusion[0][1] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25); +Kfusion[1][1] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25); +Kfusion[2][1] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25); +Kfusion[3][1] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25); +Kfusion[4][1] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25); +Kfusion[5][1] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27); +Kfusion[6][1] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25); +Kfusion[7][1] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25); +Kfusion[8][1] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25); +Kfusion[9][1] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25); +Kfusion[10][1] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25); +Kfusion[11][1] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25); +Kfusion[12][1] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25); +Kfusion[13][1] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25); +Kfusion[14][1] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25); +Kfusion[15][1] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25); +Kfusion[16][1] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25); +Kfusion[17][1] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25); +Kfusion[18][1] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25); +Kfusion[19][1] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25); +Kfusion[20][1] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25); +Kfusion[21][1] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25); +Kfusion[22][1] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25); +Kfusion[23][1] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25); diff --git a/matlab/scripts/Inertial Nav EKF/ConvertToC.m b/matlab/scripts/Inertial Nav EKF/ConvertToC.m index 7c18441859..23176f36ca 100644 --- a/matlab/scripts/Inertial Nav EKF/ConvertToC.m +++ b/matlab/scripts/Inertial Nav EKF/ConvertToC.m @@ -15,7 +15,7 @@ fileID = fopen(fileName,'r'); % This call is based on the structure of the file used to generate this % code. If an error occurs for a different file, try regenerating the code % from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false, 'Bufsize', 65535); +dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); %% Close the text file. fclose(fileID); diff --git a/matlab/scripts/Inertial Nav EKF/ConvertToM.m b/matlab/scripts/Inertial Nav EKF/ConvertToM.m index 1e5f7f4388..a9b0489de7 100644 --- a/matlab/scripts/Inertial Nav EKF/ConvertToM.m +++ b/matlab/scripts/Inertial Nav EKF/ConvertToM.m @@ -15,7 +15,7 @@ fileID = fopen(fileName,'r'); % This call is based on the structure of the file used to generate this % code. If an error occurs for a different file, try regenerating the code % from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false,'Bufsize',65535); +dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); %% Close the text file. fclose(fileID); diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m index 44d0eeeb40..99741cb303 100644 --- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m +++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m @@ -1,13 +1,13 @@ % IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run -% Derivation of Navigation EKF using a local NED earth Tangent Frame and +% Derivation of Navigation EKF using a local NED earth Tangent Frame and % XYZ body fixed frame % Sequential fusion of velocity and position measurements % Fusion of true airspeed % Sequential fusion of magnetic flux measurements % 24 state architecture. % IMU data is assumed to arrive at a constant rate with a time step of dt -% IMU delta angle and velocity data are used as time varying parameters, +% IMU delta angle and velocity data are used as control inputs, % not observations % Author: Paul Riseborough @@ -15,8 +15,8 @@ % Based on use of a rotation vector for attitude estimation as described % here: -% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", -% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003), +% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation", +% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003), % pp. 855-860. % State vector: @@ -52,7 +52,7 @@ syms vn ve vd real % NED velocity - m/sec syms pn pe pd real % NED position - m syms dax_b day_b daz_b real % delta angle bias - rad syms dax_s day_s daz_s real % delta angle scale factor -syms dvz_b real % delta velocity bias - m/sec +syms dvz_b dvy_b dvz_b real % delta velocity bias - m/sec syms dt real % IMU time step - sec syms gravity real % gravity - m/sec^2 syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise @@ -72,9 +72,9 @@ syms R_DECL R_YAW real; % variance of declination or yaw angle observation syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes syms rho real % air density (kg/m^3) syms R_ACC real % variance of accelerometer measurements (m/s^2)^2 -syms Kacc real % ratio of horizontal acceleration to top speed for a multirotor +syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s) -%% define the process equations +%% define the state prediction equations % define the measured Delta angle and delta velocity vectors dAngMeas = [dax; day; daz]; @@ -101,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat); Tbn = Quat2Tbn(truthQuat); % define the truth delta angle -% ignore coning compensation as these effects are negligible in terms of +% ignore coning compensation as these effects are negligible in terms of % covariance growth for our application and grade of sensor dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise]; @@ -157,7 +157,27 @@ nStates=numel(stateVector); % Define vector of process equations newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew]; -%% derive the covariance prediction equation +% derive the state transition matrix +F = jacobian(newStateVector, stateVector); +% set the rotation error states to zero +F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +[F,SF]=OptimiseAlgebra(F,'SF'); + +% define a symbolic covariance matrix using strings to represent +% '_l_' to represent '( ' +% '_c_' to represent , +% '_r_' to represent ')' +% these can be substituted later to create executable code +for rowIndex = 1:nStates + for colIndex = 1:nStates + eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']); + eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']); + end +end + +save 'StatePrediction.mat'; + +%% derive the covariance prediction equations % This reduces the number of floating point operations by a factor of 6 or % more compared to using the standard matrix operations in code @@ -179,26 +199,8 @@ Q = G*distMatrix*transpose(G); % remove the disturbance noise from the process equations as it is only % needed when calculating the disturbance influence matrix -vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0); -errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0); - -% derive the state transition matrix -F = jacobian(newStateVector, stateVector); -% set the rotation error states to zero -F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -[F,SF]=OptimiseAlgebra(F,'SF'); - -% define a symbolic covariance matrix using strings to represent -% '_l_' to represent '( ' -% '_c_' to represent , -% '_r_' to represent ')' -% these can be substituted later to create executable code -for rowIndex = 1:nStates - for colIndex = 1:nStates - eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']); - eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']); - end -end +vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0}); +errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0}); % Derive the predicted covariance matrix using the standard equation PP = F*P*transpose(F) + Q; @@ -206,14 +208,27 @@ PP = F*P*transpose(F) + Q; % Collect common expressions to optimise processing [PP,SPP]=OptimiseAlgebra(PP,'SPP'); +save('StateAndCovariancePrediction.mat'); +clear all; +reset(symengine); + %% derive equations for fusion of true airspeed measurements +load('StatePrediction.mat'); VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing -K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector +K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS); +[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector + +% save equations and reset workspace +save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS'); +clear all; +reset(symengine); %% derive equations for fusion of angle of sideslip measurements +load('StatePrediction.mat'); + % calculate wind relative velocities in nav frame and rotate into body frame Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd]; % calculate predicted angle of sideslip using small angle assumption @@ -223,7 +238,14 @@ H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); [H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector +% save equations and reset workspace +save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA'); +clear all; +reset(symengine); + %% derive equations for fusion of magnetic field measurement +load('StatePrediction.mat'); + magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); @@ -236,99 +258,128 @@ K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector [K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ'); -%% derive equations for sequential fusion of optical flow measurements +% save equations and reset workspace +save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ'); +clear all; +reset(symengine); + +%% derive equations for sequential fusion of optical flow measurements +load('StatePrediction.mat'); + +% range is defined as distance from camera focal point to centre of sensor fov +syms range real; -% calculate range from plane to centre of sensor fov assuming flat earth -% and camera axes aligned with body axes -range = ((ptd - pd)/Tbn(3,3)); % calculate relative velocity in body frame relVelBody = transpose(Tbn)*[vn;ve;vd]; + % divide by range to get predicted angular LOS rates relative to X and Y % axes. Note these are body angular rate motion compensated optical flow rates losRateX = +relVelBody(2)/range; losRateY = -relVelBody(1)/range; -H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian -H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -H_LOS = simplify(H_LOS); -[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS'); +save('temp1.mat','losRateX','losRateY'); -% combine into a single K matrix to enable common expressions to be found -% note this matrix cannot be used in a single step fusion -K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector +% calculate the observation Jacobian for the X axis +H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian +H_LOSX = subs(H_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_LOSX = simplify(H_LOSX); +save('temp2.mat','H_LOSX'); +ccode(H_LOSX,'file','H_LOSX.c'); +fix_c_code('H_LOSX.c'); + +clear all; +reset(symengine); +load('StatePrediction.mat'); +load('temp1.mat'); + +% calculate the observation Jacobian for the Y axis +H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian +H_LOSY = subs(H_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_LOSY = simplify(H_LOSY); +save('temp3.mat','H_LOSY'); +ccode(H_LOSY,'file','H_LOSY.c'); +fix_c_code('H_LOSY.c'); + +clear all; +reset(symengine); +load('StatePrediction.mat'); +load('temp1.mat'); +load('temp2.mat'); + +% calculate Kalman gain vector for the X axis +K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -K_LOSY = (P*transpose(H_LOS(2,:)))/(H_LOS(2,:)*P*transpose(H_LOS(2,:)) + R_LOS); % Kalman gain vector +K_LOSX = simplify(K_LOSX); +ccode(K_LOSX,'file','K_LOSX.c'); +fix_c_code('K_LOSX.c'); + +clear all; +reset(symengine); +load('StatePrediction.mat'); +load('temp1.mat'); +load('temp3.mat'); + +% calculate Kalman gain vector for the Y axis +K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -K_LOS = [K_LOSX,K_LOSY]; -simplify(K_LOS); -[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS'); +K_LOSY = simplify(K_LOSY); +ccode(K_LOSY,'file','K_LOSY.c'); +fix_c_code('K_LOSY.c'); -% Use matlab c code converter for an alternate method of -ccode(H_LOS,'file','H_LOS.txt'); -ccode(K_LOSX,'file','K_LOSX.txt'); -ccode(K_LOSY,'file','K_LOSY.txt'); - -%% derive equations for simple fusion of 2-D magnetic heading measurements - -% rotate magnetic field into earth axes -magMeasNED = Tbn*[magX;magY;magZ]; -% the predicted measurement is the angle wrt true north of the horizontal -% component of the measured field -angMeas = atan(magMeasNED(2)/magMeasNED(1)); -simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b]; -Psimple = P(1:16,1:16); -H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian -%H_MAGS = H_MAGS(1:3); -H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -%H_MAGS = simplify(H_MAGS); -%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS'); -ccode(H_MAGS,'file','calcH_MAGS.c'); -% Calculate Kalman gain vector -K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL); -%K_MAGS = simplify(K_MAGS); -%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS'); -ccode(K_MAGS,'file','calcK_MAGS.c'); +% reset workspace +clear all; +reset(symengine); %% derive equations for fusion of 321 sequence yaw measurement +load('StatePrediction.mat'); % Calculate the yaw (first rotation) angle from the 321 rotation sequence angMeas = atan(Tbn(2,1)/Tbn(1,1)); -H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian -H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -ccode(H_YAW,'file','calcH_YAW321.c'); -% Calculate Kalman gain vector -K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW); -ccode([K_YAW;H_YAW'],'file','calcYAW321.c'); +H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian +H_YAW321 = subs(H_YAW321, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_YAW321 = simplify(H_YAW321); +ccode(H_YAW321,'file','calcH_YAW321.c'); +fix_c_code('calcH_YAW321.c'); + +% reset workspace +clear all; +reset(symengine); %% derive equations for fusion of 312 sequence yaw measurement +load('StatePrediction.mat'); % Calculate the yaw (first rotation) angle from an Euler 312 sequence angMeas = atan(-Tbn(1,2)/Tbn(2,2)); -H_YAW2 = jacobian(angMeas,stateVector); % measurement Jacobianclea -H_YAW2 = subs(H_YAW2, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); -ccode(H_YAW2,'file','calcH_YAW312.c'); -% Calculate Kalman gain vector -K_YAW2 = (P*transpose(H_YAW2))/(H_YAW2*P*transpose(H_YAW2) + R_YAW); -ccode([K_YAW2;H_YAW2'],'file','calcYAW312.c'); +H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea +H_YAW312 = subs(H_YAW312, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_YAW312 = simplify(H_YAW312); +ccode(H_YAW312,'file','calcH_YAW312.c'); +fix_c_code('calcH_YAW312.c'); + +% reset workspace +clear all; +reset(symengine); + +%% derive equations for fusion of declination +load('StatePrediction.mat'); -%% derive equations for fusion of synthetic deviation measurement -% used to keep correct heading when operating without absolute position or -% velocity measurements - eg when using optical flow -% rotate magnetic field into earth axes -magMeasNED = [magN;magE;magD]; % the predicted measurement is the angle wrt magnetic north of the horizontal % component of the measured field -angMeas = atan(magMeasNED(2)/magMeasNED(1)); +angMeas = atan(magE/magN); H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); H_MAGD = simplify(H_MAGD); -%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD'); -ccode(H_MAGD,'file','calcH_MAGD.c'); -% Calculate Kalman gain vector K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL); -ccode([H_MAGD',K_MAGD],'file','calcMAGD.c'); +K_MAGD = simplify(K_MAGD); +ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c'); +fix_c_code('calcMAGD.c'); + +% reset workspace +clear all; +reset(symengine); %% derive equations for fusion of lateral body acceleration (multirotors only) +load('StatePrediction.mat'); % use relationship between airspeed along the X and Y body axis and the % drag to predict the lateral acceleration for a multirotor vehicle type @@ -344,32 +395,45 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity % accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis % Use a simple viscous drag model for the linear estimator equations -% Use the the derivative from speed to acceleration averaged across the +% Use the the derivative from speed to acceleration averaged across the % speed range % The nonlinear equation will be used to calculate the predicted % measurement in implementation -accXpred = -Kacc*vrel(1); % predicted acceleration measured along X body axis -accYpred = -Kacc*vrel(2); % predicted acceleration measured along Y body axis +accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis +accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis % Derive observation Jacobian and Kalman gain matrix for X accel fusion H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_ACCX = simplify(H_ACCX); [H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC); -ccode([H_ACCX',K_ACCX],'file','calcACCX.c'); [K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector % Derive observation Jacobian and Kalman gain matrix for Y accel fusion H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0}); +H_ACCY = simplify(H_ACCY); [H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC); -ccode([H_ACCY',K_ACCY],'file','calcACCY.c'); [K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector +% save equations and reset workspace +save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY'); +clear all; +reset(symengine); + %% Save output and convert to m and c code fragments + +% load equations for predictions and updates +load('StateAndCovariancePrediction.mat'); +load('Airspeed.mat'); +load('Sideslip.mat'); +load('Magnetometer.mat'); +load('Drag.mat'); + fileName = strcat('SymbolicOutput',int2str(nStates),'.mat'); save(fileName); SaveScriptCode(nStates); ConvertToM(nStates); -ConvertToC(nStates); +ConvertToC(nStates); \ No newline at end of file diff --git a/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m b/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m index a1f755136c..b86f59eb04 100644 --- a/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m +++ b/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m @@ -409,7 +409,7 @@ if exist('SH_LOS','var') fprintf(fid,'\n'); fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); for rowIndex = 1:nRow - string = char(K_LOS(rowIndex,1)); + string = char(K_LOSX(rowIndex)); % don't write out a zero-assignment if ~strcmpi(string,'0') fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); @@ -421,7 +421,7 @@ if exist('SH_LOS','var') fprintf(fid,'\n'); fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); for rowIndex = 1:nRow - string = char(K_LOS(rowIndex,2)); + string = char(K_LOSY(rowIndex)); % don't write out a zero-assignment if ~strcmpi(string,'0') fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); @@ -547,6 +547,25 @@ if exist('SH_MAGS','var') end fprintf(fid,'\n'); + fprintf(fid,'\n'); + fprintf(fid,'SK_MAGS = zeros(%d,1);\n',numel(SK_MAGS)); + for rowIndex = 1:numel(SK_MAGS) + string = char(SK_MAGS(rowIndex,1)); + fprintf(fid,'SK_MAGS(%d) = %s;\n',rowIndex,string); + end + fprintf(fid,'\n'); + + [nRow,nCol] = size(K_MAGS); + fprintf(fid,'\n'); + fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol); + for rowIndex = 1:nRow + string = char(K_MAGS(rowIndex,1)); + % don't write out a zero-assignment + if ~strcmpi(string,'0') + fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string); + end + end + fprintf(fid,'\n'); end %% Write equations for X accel fusion diff --git a/matlab/scripts/Inertial Nav EKF/fix_c_code.m b/matlab/scripts/Inertial Nav EKF/fix_c_code.m new file mode 100644 index 0000000000..0eb2874843 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/fix_c_code.m @@ -0,0 +1,92 @@ +function fix_c_code(fileName) +%% Initialize variables +delimiter = ''; + +%% Format string for each line of text: +% column1: text (%s) +% For more information, see the TEXTSCAN documentation. +formatSpec = '%s%[^\n\r]'; + +%% Open the text file. +fileID = fopen(fileName,'r'); + +%% Read columns of data according to format string. +% This call is based on the structure of the file used to generate this +% code. If an error occurs for a different file, try regenerating the code +% from the Import Tool. +dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false); + +%% Close the text file. +fclose(fileID); + +%% Create output variable +SymbolicOutput = [dataArray{1:end-1}]; + +%% Clear temporary variables +clearvars filename delimiter formatSpec fileID dataArray ans; + +%% replace brackets and commas +for lineIndex = 1:length(SymbolicOutput) + SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '('); + SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ','); + SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')'); +end + +%% Convert indexing and replace brackets + +% replace 1-D indexes +for arrayIndex = 1:99 + strIndex = int2str(arrayIndex); + strRep = sprintf('[%d]',(arrayIndex-1)); + strPat = strcat('\(',strIndex,'\)'); + for lineIndex = 1:length(SymbolicOutput) + str = char(SymbolicOutput(lineIndex)); + SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; + end +end + +% replace 2-D left indexes +for arrayIndex = 1:99 + strIndex = int2str(arrayIndex); + strRep = sprintf('[%d,',(arrayIndex-1)); + strPat = strcat('\(',strIndex,'\,'); + for lineIndex = 1:length(SymbolicOutput) + str = char(SymbolicOutput(lineIndex)); + SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; + end +end + +% replace 2-D right indexes +for arrayIndex = 1:99 + strIndex = int2str(arrayIndex); + strRep = sprintf(',%d]',(arrayIndex-1)); + strPat = strcat('\,',strIndex,'\)'); + for lineIndex = 1:length(SymbolicOutput) + str = char(SymbolicOutput(lineIndex)); + SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; + end +end + +% replace commas +for lineIndex = 1:length(SymbolicOutput) + str = char(SymbolicOutput(lineIndex)); + SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')}; +end + +%% Change covariance matrix variable name to P +for lineIndex = 1:length(SymbolicOutput) + strIn = char(SymbolicOutput(lineIndex)); + strIn = regexprep(strIn,'OP\[','P['); + SymbolicOutput(lineIndex) = cellstr(strIn); +end + +%% Write to file +fid = fopen(fileName,'wt'); +for lineIndex = 1:length(SymbolicOutput) + fprintf(fid,char(SymbolicOutput(lineIndex))); + fprintf(fid,'\n'); +end +fclose(fid); +clear all; + +end \ No newline at end of file From cc5512905a56ee5f3d77883832bd6fba6453b534 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 8 Mar 2016 20:33:21 +1100 Subject: [PATCH 16/23] EKF: prevent optical flow, GPS and baro fusion from blocking each other --- EKF/ekf.cpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b696472266..8fe2e016ff 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -204,40 +204,43 @@ bool Ekf::update() } } - // determine if we have height data that can be fused + // determine if range finder data has fallen behind the fusin time horizon fuse it if we are + // not tilted too much to use it if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) - && (_R_prev(2, 2) > 0.7071f)) { - // if we have range data we always try to estimate terrain nheight + && (_R_prev(2, 2) > 0.7071f)) { + // if we have range data we always try to estimate terrain height _fuse_hagl_data = true; - // only use if for height in the main filter if specifically enabled + // only use range finder as a height observation in the main filter if specifically enabled if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { _fuse_height = true; } - } else if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed) + } + + // determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled + if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed) && _params.vdist_sensor_type == VDIST_SENSOR_BARO) { _fuse_height = true; } // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it - // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift - // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; - _fuse_flow = false; + } - } else if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) + // If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it + if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 && (_R_prev(2, 2) > 0.7071f)) { - _fuse_pos = false; - _fuse_vert_vel = false; - _fuse_hor_vel = false; _fuse_flow = true; + } - } else if (!_control_status.flags.gps && !_control_status.flags.opt_flow + // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift + // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz + if (!_control_status.flags.gps && !_control_status.flags.opt_flow && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { _fuse_pos = true; _gps_sample_delayed.pos(0) = _last_known_posNE(0); From 1a2da887abd27e3309d4917f37f98d0813c65169 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 Mar 2016 09:29:15 +1100 Subject: [PATCH 17/23] EKF: Fix bug in calculation of terrain estimator Kalman gain --- EKF/terrain_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index f9caa3e03d..dd6fd9a206 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -104,7 +104,7 @@ void Ekf::fuseHagl() if (test_ratio <= 1.0f) { // calculate the Kalman gain - float gain = obs_variance / _hagl_innov_var; + float gain = _terrain_var / _hagl_innov_var; // correct the state _terrain_vpos -= gain * _hagl_innov; // correct the variance From 962fd0aaf27b7194fc06c29e3ae70ae969c6789a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 Mar 2016 10:06:05 +1100 Subject: [PATCH 18/23] EKF: Adjust terrain process noise for gradient effect --- EKF/common.h | 2 ++ EKF/terrain_estimator.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 83c709cc67..44fbba8dd5 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -166,6 +166,7 @@ struct parameters { float mag_p_noise; // process noise for magnetic field prediction (Guass/sec) float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec) float terrain_p_noise; // process noise for terrain offset (m/sec) + float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m) // position and velocity fusion float gps_vel_noise; // observation noise for gps velocity fusion (m/sec) @@ -234,6 +235,7 @@ struct parameters { mag_p_noise = 2.5e-2f; wind_vel_p_noise = 1.0e-1f; terrain_p_noise = 0.5f; + terrain_gradient = 0.5f; // position and velocity fusion gps_vel_noise = 5.0e-1f; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index dd6fd9a206..c4f3655d71 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -73,8 +73,13 @@ void Ekf::predictHagl() { // predict the state variance growth // the state is the vertical position of the terrain underneath the vehicle + + // process noise due to errors in vehicle height estimate _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); + // process noise due to terrain gradient + _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1))); + // limit the variance to prevent it becoming badly conditioned _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); } From eab0ef4266053a09468c53f251cf69f1f03b1d8a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 Mar 2016 10:36:41 +1100 Subject: [PATCH 19/23] EKF: Update default process noise for terrain estimator --- EKF/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 44fbba8dd5..291e3df73a 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -234,7 +234,7 @@ struct parameters { gyro_scale_p_noise = 3.0e-3f; mag_p_noise = 2.5e-2f; wind_vel_p_noise = 1.0e-1f; - terrain_p_noise = 0.5f; + terrain_p_noise = 5.0f; terrain_gradient = 0.5f; // position and velocity fusion From 48f980b05473a828559323a5004b8704d5cde63b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 Mar 2016 09:39:45 +1100 Subject: [PATCH 20/23] EKF: Fix syntax causing posix build to fail --- EKF/optflow_fusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index f4aff85dbc..4d14fa7fbc 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -77,8 +77,8 @@ void Ekf::fuseOptFlow() // take the weighted average of the observation noie for the best and wort flow quality float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); - float H_LOS[2][24] = {0}; // Optical flow observation Jacobians - float Kfusion[24][2] = {0}; // Optical flow Kalman gains + float H_LOS[2][24] = {}; // Optical flow observation Jacobians + float Kfusion[24][2] = {}; // Optical flow Kalman gains // constrain height above ground to be above minimum height when sitting on ground float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance); From e0fcce14636717c8f74703f01929eaba14fdcb8b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 Mar 2016 15:09:23 +1100 Subject: [PATCH 21/23] EKF: Make position and velocity reset publish success Some users of the position and velocity reset functions will need to know if the reset has been successful. --- EKF/ekf.h | 4 ++-- EKF/ekf_helper.cpp | 10 +++++++--- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 71c2e09af9..89a590d732 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -249,7 +249,7 @@ private: void fuseVelPosHeight(); // reset velocity states of the ekf - void resetVelocity(); + bool resetVelocity(); // fuse optical flow line of sight rate measurements void fuseOptFlow(); @@ -275,7 +275,7 @@ private: void calcMagDeclination(); // reset position states of the ekf (only vertical position) - void resetPosition(); + bool resetPosition(); // reset height state of the ekf void resetHeight(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8b4ac5484f..43713992e4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -49,22 +49,24 @@ // Reset the velocity states. If we have a recent and valid // gps measurement then use for velocity initialisation -void Ekf::resetVelocity() +bool Ekf::resetVelocity() { // if we have a valid GPS measurement use it to initialise velocity states gpsSample gps_newest = _gps_buffer.get_newest(); if (_time_last_imu - gps_newest.time_us < 400000) { _state.vel = gps_newest.vel; + return true; } else { - _state.vel.setZero(); + // XXX use the value of the last known velocity + return false; } } // Reset position states. If we have a recent and valid // gps measurement then use for position initialisation -void Ekf::resetPosition() +bool Ekf::resetPosition() { // if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay gpsSample gps_newest = _gps_buffer.get_newest(); @@ -74,9 +76,11 @@ void Ekf::resetPosition() if (time_delay < 0.4f) { _state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay; _state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay; + return true; } else { // XXX use the value of the last known position + return false; } } From 26238bc2f585c2e3c65f3fdff64cf2497c584b77 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 Mar 2016 15:13:47 +1100 Subject: [PATCH 22/23] EKF: Allow for change in position when defining the WGS-84 origin position This allows GPS aiding to commence later in flight without step changes in local position output --- EKF/gps_checks.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 77f94a1ee8..3a2a5a220c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -55,15 +55,21 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { - // run gps checks if we have not yet set the NED origin or have not started using GPS - if (!_NED_origin_initialised || !_control_status.flags.gps) { - // if we have good GPS data and the NED origin is not set, set to the last GPS fix + // If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix + if (!_NED_origin_initialised ) { + // we have good GPS data so can now set the origin's WGS-84 position if (gps_is_good(gps) && !_NED_origin_initialised) { printf("gps is good - setting EKF origin\n"); - // Initialise projection + // Set the origin's WGS-84 position to the last gps fix double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); + // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating + if (_control_status.flags.opt_flow || _control_status.flags.gps) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); + map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu); + } // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); _NED_origin_initialised = true; From 109e0e6dfcfe012fe06db301e316f2ed470baf0c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 Mar 2016 15:14:50 +1100 Subject: [PATCH 23/23] EKF: When commencing GPS aiding, don't reset local position and velocity if using optical flow --- EKF/control.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index dc180d1aca..e2583f5af3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -121,12 +121,15 @@ void Ekf::controlFusionModes() _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } - // If the heading is valid, reset the positon and velocity and start using gps aiding + // If the heading is valid start using gps aiding if (_control_status.flags.yaw_align) { - resetPosition(); - resetVelocity(); _control_status.flags.gps = true; _time_last_gps = _time_last_imu; + // if we are not already aiding with optical flow, then we need to reset the position and velocity + if (!_control_status.flags.opt_flow) { + _control_status.flags.gps = resetPosition(); + _control_status.flags.gps = resetVelocity(); + } } }