mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
ekf2: terrain flow - migrate to Symforce
This commit is contained in:
bresch
Daniel Agar
@@ -0,0 +1,91 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""
|
||||||
|
Copyright (c) 2023 PX4 Development Team
|
||||||
|
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 PX4 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: derivation_terrain_estimator.py
|
||||||
|
Description:
|
||||||
|
"""
|
||||||
|
|
||||||
|
import symforce.symbolic as sf
|
||||||
|
from derivation_utils import *
|
||||||
|
|
||||||
|
def predict_opt_flow(
|
||||||
|
terrain_vpos: sf.Scalar,
|
||||||
|
q_att: sf.V4,
|
||||||
|
v: sf.V3,
|
||||||
|
pos_z: sf.Scalar,
|
||||||
|
epsilon : sf.Scalar
|
||||||
|
):
|
||||||
|
R_to_earth = quat_to_rot(q_att)
|
||||||
|
flow_pred = sf.V2()
|
||||||
|
dist = - (pos_z - terrain_vpos)
|
||||||
|
dist = add_epsilon_sign(dist, dist, epsilon)
|
||||||
|
flow_pred[0] = -v[1] / dist * R_to_earth[2, 2]
|
||||||
|
flow_pred[1] = v[0] / dist * R_to_earth[2, 2]
|
||||||
|
return flow_pred
|
||||||
|
|
||||||
|
def terr_est_compute_flow_xy_innov_var_and_hx(
|
||||||
|
terrain_vpos: sf.Scalar,
|
||||||
|
P: sf.Scalar,
|
||||||
|
q_att: sf.V4,
|
||||||
|
v: sf.V3,
|
||||||
|
pos_z: sf.Scalar,
|
||||||
|
R: sf.Scalar,
|
||||||
|
epsilon : sf.Scalar
|
||||||
|
):
|
||||||
|
flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon)
|
||||||
|
Hx = sf.V1(flow_pred[0]).jacobian(terrain_vpos)
|
||||||
|
Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos)
|
||||||
|
|
||||||
|
innov_var = sf.V2()
|
||||||
|
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||||
|
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||||
|
|
||||||
|
return (innov_var, Hx[0, 0])
|
||||||
|
|
||||||
|
def terr_est_compute_flow_y_innov_var_and_h(
|
||||||
|
terrain_vpos: sf.Scalar,
|
||||||
|
P: sf.Scalar,
|
||||||
|
q_att: sf.V4,
|
||||||
|
v: sf.V3,
|
||||||
|
pos_z: sf.Scalar,
|
||||||
|
R: sf.Scalar,
|
||||||
|
epsilon : sf.Scalar
|
||||||
|
):
|
||||||
|
flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon)
|
||||||
|
Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos)
|
||||||
|
|
||||||
|
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||||
|
|
||||||
|
return (innov_var, Hy[0, 0])
|
||||||
|
|
||||||
|
print("Derive terrain estimator equations...")
|
||||||
|
generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
|
||||||
|
generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
|
||||||
+66
@@ -0,0 +1,66 @@
|
|||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// This file was autogenerated by symforce from template:
|
||||||
|
// backends/cpp/templates/function/FUNCTION.h.jinja
|
||||||
|
// Do NOT modify by hand.
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <matrix/math.hpp>
|
||||||
|
|
||||||
|
namespace sym {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||||
|
*
|
||||||
|
* Symbolic function: terr_est_compute_flow_xy_innov_var_and_hx
|
||||||
|
*
|
||||||
|
* Args:
|
||||||
|
* terrain_vpos: Scalar
|
||||||
|
* P: Scalar
|
||||||
|
* q_att: Matrix41
|
||||||
|
* v: Matrix31
|
||||||
|
* pos_z: Scalar
|
||||||
|
* R: Scalar
|
||||||
|
* epsilon: Scalar
|
||||||
|
*
|
||||||
|
* Outputs:
|
||||||
|
* innov_var: Matrix21
|
||||||
|
* H: Scalar
|
||||||
|
*/
|
||||||
|
template <typename Scalar>
|
||||||
|
void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P,
|
||||||
|
const matrix::Matrix<Scalar, 4, 1>& q_att,
|
||||||
|
const matrix::Matrix<Scalar, 3, 1>& v, const Scalar pos_z,
|
||||||
|
const Scalar R, const Scalar epsilon,
|
||||||
|
matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
|
||||||
|
Scalar* const H = nullptr) {
|
||||||
|
// Total ops: 28
|
||||||
|
|
||||||
|
// Input arrays
|
||||||
|
|
||||||
|
// Intermediate terms (4)
|
||||||
|
const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) -
|
||||||
|
std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2));
|
||||||
|
const Scalar _tmp1 = pos_z - terrain_vpos;
|
||||||
|
const Scalar _tmp2 =
|
||||||
|
-_tmp1 + epsilon * (2 * math::min<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);
|
||||||
|
const Scalar _tmp3 = P * std::pow(_tmp0, Scalar(2)) / std::pow(_tmp2, Scalar(4));
|
||||||
|
|
||||||
|
// Output terms (2)
|
||||||
|
if (innov_var != nullptr) {
|
||||||
|
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||||
|
|
||||||
|
_innov_var(0, 0) = R + _tmp3 * std::pow(v(1, 0), Scalar(2));
|
||||||
|
_innov_var(1, 0) = R + _tmp3 * std::pow(v(0, 0), Scalar(2));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (H != nullptr) {
|
||||||
|
Scalar& _H = (*H);
|
||||||
|
|
||||||
|
_H = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
|
||||||
|
}
|
||||||
|
} // NOLINT(readability/fn_size)
|
||||||
|
|
||||||
|
// NOLINTNEXTLINE(readability/fn_size)
|
||||||
|
} // namespace sym
|
||||||
+65
@@ -0,0 +1,65 @@
|
|||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
// This file was autogenerated by symforce from template:
|
||||||
|
// backends/cpp/templates/function/FUNCTION.h.jinja
|
||||||
|
// Do NOT modify by hand.
|
||||||
|
// -----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <matrix/math.hpp>
|
||||||
|
|
||||||
|
namespace sym {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||||
|
*
|
||||||
|
* Symbolic function: terr_est_compute_flow_y_innov_var_and_h
|
||||||
|
*
|
||||||
|
* Args:
|
||||||
|
* terrain_vpos: Scalar
|
||||||
|
* P: Scalar
|
||||||
|
* q_att: Matrix41
|
||||||
|
* v: Matrix31
|
||||||
|
* pos_z: Scalar
|
||||||
|
* R: Scalar
|
||||||
|
* epsilon: Scalar
|
||||||
|
*
|
||||||
|
* Outputs:
|
||||||
|
* innov_var: Scalar
|
||||||
|
* H: Scalar
|
||||||
|
*/
|
||||||
|
template <typename Scalar>
|
||||||
|
void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P,
|
||||||
|
const matrix::Matrix<Scalar, 4, 1>& q_att,
|
||||||
|
const matrix::Matrix<Scalar, 3, 1>& v, const Scalar pos_z,
|
||||||
|
const Scalar R, const Scalar epsilon,
|
||||||
|
Scalar* const innov_var = nullptr, Scalar* const H = nullptr) {
|
||||||
|
// Total ops: 26
|
||||||
|
|
||||||
|
// Input arrays
|
||||||
|
|
||||||
|
// Intermediate terms (3)
|
||||||
|
const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) -
|
||||||
|
std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2));
|
||||||
|
const Scalar _tmp1 = pos_z - terrain_vpos;
|
||||||
|
const Scalar _tmp2 =
|
||||||
|
-_tmp1 + epsilon * (2 * math::min<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);
|
||||||
|
|
||||||
|
// Output terms (2)
|
||||||
|
if (innov_var != nullptr) {
|
||||||
|
Scalar& _innov_var = (*innov_var);
|
||||||
|
|
||||||
|
_innov_var =
|
||||||
|
P * std::pow(_tmp0, Scalar(2)) * std::pow(v(0, 0), Scalar(2)) / std::pow(_tmp2, Scalar(4)) +
|
||||||
|
R;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (H != nullptr) {
|
||||||
|
Scalar& _H = (*H);
|
||||||
|
|
||||||
|
_H = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2));
|
||||||
|
}
|
||||||
|
} // NOLINT(readability/fn_size)
|
||||||
|
|
||||||
|
// NOLINTNEXTLINE(readability/fn_size)
|
||||||
|
} // namespace sym
|
||||||
@@ -1,97 +0,0 @@
|
|||||||
"""
|
|
||||||
|
|
||||||
This script calculates the observation scalars (H matrix) for fusing optical flow
|
|
||||||
measurements for terrain estimation.
|
|
||||||
|
|
||||||
@author: roman
|
|
||||||
"""
|
|
||||||
|
|
||||||
from sympy import *
|
|
||||||
|
|
||||||
# q: quaternion describing rotation from frame 1 to frame 2
|
|
||||||
# returns a rotation matrix derived form q which describes the same
|
|
||||||
# rotation
|
|
||||||
def quat2Rot(q):
|
|
||||||
q0 = q[0]
|
|
||||||
q1 = q[1]
|
|
||||||
q2 = q[2]
|
|
||||||
q3 = q[3]
|
|
||||||
|
|
||||||
Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
|
|
||||||
[2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
|
|
||||||
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
|
|
||||||
|
|
||||||
return Rot
|
|
||||||
|
|
||||||
# take an expression calculated by the cse() method and write the expression
|
|
||||||
# into a text file in C format
|
|
||||||
def write_simplified(P_touple, filename, out_name):
|
|
||||||
subs = P_touple[0]
|
|
||||||
P = Matrix(P_touple[1])
|
|
||||||
fd = open(filename, 'a')
|
|
||||||
|
|
||||||
is_vector = P.shape[0] == 1 or P.shape[1] == 1
|
|
||||||
|
|
||||||
# write sub expressions
|
|
||||||
for index, item in enumerate(subs):
|
|
||||||
fd.write('float ' + str(item[0]) + ' = ' + str(item[1]) + ';\n')
|
|
||||||
|
|
||||||
# write actual matrix values
|
|
||||||
fd.write('\n')
|
|
||||||
|
|
||||||
if not is_vector:
|
|
||||||
iterator = range(0,sqrt(len(P)), 1)
|
|
||||||
for row in iterator:
|
|
||||||
for column in iterator:
|
|
||||||
fd.write(out_name + '(' + str(row) + ',' + str(column) + ') = ' + str(P[row, column]) + ';\n')
|
|
||||||
else:
|
|
||||||
iterator = range(0, len(P), 1)
|
|
||||||
|
|
||||||
for item in iterator:
|
|
||||||
fd.write(out_name + '(' + str(item) + ') = ' + str(P[item]) + ';\n')
|
|
||||||
|
|
||||||
fd.write('\n\n')
|
|
||||||
fd.close()
|
|
||||||
|
|
||||||
########## Symbolic variable definition #######################################
|
|
||||||
|
|
||||||
|
|
||||||
# vehicle velocity
|
|
||||||
v_x = Symbol("v_x", real=True) # vehicle body x velocity
|
|
||||||
v_y = Symbol("v_y", real=True) # vehicle body y velocity
|
|
||||||
|
|
||||||
# unit quaternion describing vehicle attitude, qw is real part
|
|
||||||
qw = Symbol("q0", real=True)
|
|
||||||
qx = Symbol("q1", real=True)
|
|
||||||
qy = Symbol("q2", real=True)
|
|
||||||
qz = Symbol("q3", real=True)
|
|
||||||
q_att = Matrix([qw, qx, qy, qz])
|
|
||||||
|
|
||||||
# terrain vertial position in local NED frame
|
|
||||||
_terrain_vpos = Symbol("_terrain_vpos", real=True)
|
|
||||||
|
|
||||||
_terrain_var = Symbol("_terrain_var", real=True)
|
|
||||||
|
|
||||||
# vehicle vertical position in local NED frame
|
|
||||||
pos_z = Symbol("z", real=True)
|
|
||||||
|
|
||||||
R_body_to_earth = quat2Rot(q_att)
|
|
||||||
|
|
||||||
# Optical flow around x axis
|
|
||||||
flow_x = -v_y / (_terrain_vpos - pos_z) * R_body_to_earth[2,2]
|
|
||||||
|
|
||||||
# Calculate observation scalar
|
|
||||||
H_x = Matrix([flow_x]).jacobian(Matrix([_terrain_vpos]))
|
|
||||||
|
|
||||||
H_x_simple = cse(H_x, symbols('t0:30'))
|
|
||||||
|
|
||||||
# Optical flow around y axis
|
|
||||||
flow_y = v_x / (_terrain_vpos - pos_z) * R_body_to_earth[2,2]
|
|
||||||
|
|
||||||
# Calculate observation scalar
|
|
||||||
H_y = Matrix([flow_y]).jacobian(Matrix([_terrain_vpos]))
|
|
||||||
|
|
||||||
H_y_simple = cse(H_y, symbols('t0:30'))
|
|
||||||
|
|
||||||
write_simplified(H_x_simple, "flow_x_observation.txt", "Hx")
|
|
||||||
write_simplified(H_y_simple, "flow_y_observation.txt", "Hy")
|
|
||||||
@@ -1,6 +1,6 @@
|
|||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
* Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
@@ -12,7 +12,7 @@
|
|||||||
* notice, this list of conditions and the following disclaimer in
|
* notice, this list of conditions and the following disclaimer in
|
||||||
* the documentation and/or other materials provided with the
|
* the documentation and/or other materials provided with the
|
||||||
* distribution.
|
* distribution.
|
||||||
* 3. Neither the name ECL nor the names of its contributors may be
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
* used to endorse or promote products derived from this software
|
* used to endorse or promote products derived from this software
|
||||||
* without specific prior written permission.
|
* without specific prior written permission.
|
||||||
*
|
*
|
||||||
@@ -33,13 +33,13 @@
|
|||||||
|
|
||||||
/**
|
/**
|
||||||
* @file terrain_estimator.cpp
|
* @file terrain_estimator.cpp
|
||||||
* Function for fusing rangefinder measurements to estimate terrain vertical position/
|
* Function for fusing rangefinder and optical flow measurements
|
||||||
*
|
* to estimate terrain vertical position
|
||||||
* @author Paul Riseborough <p_riseborough@live.com.au>
|
|
||||||
*
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
#include "python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
|
||||||
|
#include "python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
@@ -254,6 +254,8 @@ void Ekf::controlHaglFlowFusion()
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (_flow_data_ready) {
|
if (_flow_data_ready) {
|
||||||
|
updateOptFlow(_aid_src_optical_flow);
|
||||||
|
|
||||||
const bool continuing_conditions_passing = _control_status.flags.in_air
|
const bool continuing_conditions_passing = _control_status.flags.in_air
|
||||||
&& !_control_status.flags.opt_flow
|
&& !_control_status.flags.opt_flow
|
||||||
&& _control_status.flags.gps
|
&& _control_status.flags.gps
|
||||||
@@ -320,90 +322,71 @@ void Ekf::fuseFlowForTerrain()
|
|||||||
{
|
{
|
||||||
_aid_src_optical_flow.fusion_enabled = true;
|
_aid_src_optical_flow.fusion_enabled = true;
|
||||||
|
|
||||||
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
|
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
|
||||||
// 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 rotation of the scene about that axis.
|
|
||||||
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
|
|
||||||
|
|
||||||
// get latest estimated orientation
|
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
|
||||||
const float q0 = _state.quat_nominal(0);
|
// a positive offset in earth frame leads to a smaller height above the ground.
|
||||||
const float q1 = _state.quat_nominal(1);
|
float range = predictFlowRange();
|
||||||
const float q2 = _state.quat_nominal(2);
|
|
||||||
const float q3 = _state.quat_nominal(3);
|
|
||||||
|
|
||||||
// calculate the optical flow observation variance
|
const float state = _terrain_vpos; // linearize both axes using the same state value
|
||||||
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
|
Vector2f innov_var;
|
||||||
|
float H;
|
||||||
|
sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||||
|
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
|
||||||
|
|
||||||
// get rotation matrix from earth to body
|
if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
|
||||||
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
|| (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
|
||||||
|
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||||
// calculate the sensor position relative to the IMU
|
ECL_ERR("Opt flow error - covariance reset");
|
||||||
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
|
_terrain_var = 100.0f;
|
||||||
|
return;
|
||||||
// calculate the velocity of the sensor relative to the imu in body frame
|
}
|
||||||
// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
|
|
||||||
const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;
|
|
||||||
|
|
||||||
// calculate the velocity of the sensor in the earth frame
|
|
||||||
const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
|
|
||||||
|
|
||||||
// rotate into body frame
|
|
||||||
const Vector3f vel_body = earth_to_body * vel_rel_earth;
|
|
||||||
|
|
||||||
// constrain terrain to minimum allowed value and predict height above ground
|
|
||||||
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
|
|
||||||
const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
|
|
||||||
|
|
||||||
// calculate prediced optical flow
|
|
||||||
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
|
|
||||||
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
|
|
||||||
|
|
||||||
// calculate flow innovation
|
|
||||||
_aid_src_optical_flow.innovation[0] = pred_flow_x - opt_flow_rate(0);
|
|
||||||
_aid_src_optical_flow.innovation[1] = pred_flow_y - opt_flow_rate(1);
|
|
||||||
|
|
||||||
const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
|
|
||||||
|
|
||||||
// Calculate observation matrix for flow around
|
|
||||||
const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
|
|
||||||
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
|
|
||||||
|
|
||||||
// Constrain terrain variance to be non-negative
|
|
||||||
_terrain_var = fmaxf(_terrain_var, sq(0.01f));
|
|
||||||
|
|
||||||
// Cacluate innovation variance
|
|
||||||
_aid_src_optical_flow.innovation_variance[0] = Hx * Hx * _terrain_var + R_LOS;
|
|
||||||
_aid_src_optical_flow.innovation_variance[1] = Hy * Hy * _terrain_var + R_LOS;
|
|
||||||
|
|
||||||
// run the innovation consistency check and record result
|
// run the innovation consistency check and record result
|
||||||
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
|
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
|
||||||
|
|
||||||
// do not perform measurement update if badly conditioned
|
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
|
||||||
|
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
|
||||||
|
|
||||||
|
// if either axis fails we abort the fusion
|
||||||
if (_aid_src_optical_flow.innovation_rejected) {
|
if (_aid_src_optical_flow.innovation_rejected) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate the kalman gain for the flow measurement
|
// fuse observation axes sequentially
|
||||||
const float Kx = _terrain_var * Hx / _aid_src_optical_flow.innovation_variance[0];
|
for (uint8_t index = 0; index <= 1; index++) {
|
||||||
|
if (index == 0) {
|
||||||
|
// everything was already computed above
|
||||||
|
|
||||||
// calculate correction term for terrain variance
|
} else if (index == 1) {
|
||||||
const float KxHxP = Kx * Hx * _terrain_var;
|
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||||
|
sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
|
||||||
|
|
||||||
_terrain_vpos += Kx * _aid_src_optical_flow.innovation[0];
|
// recalculate the innovation using the updated state
|
||||||
// guard against negative variance
|
const Vector2f vel_body = predictFlowVelBody();
|
||||||
_terrain_var = fmaxf(_terrain_var - KxHxP, sq(0.01f));
|
range = predictFlowRange();
|
||||||
|
_aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
|
||||||
|
|
||||||
|
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
|
||||||
|
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||||
|
ECL_ERR("Opt flow error - covariance reset");
|
||||||
|
_terrain_var = 100.0f;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// calculate the kalman gain for the flow measurement
|
float Kfusion = _terrain_var * H / _aid_src_optical_flow.innovation_variance[index];
|
||||||
const float Ky = _terrain_var * Hy / _aid_src_optical_flow.innovation_variance[1];
|
|
||||||
|
|
||||||
// calculate correction term for terrain variance
|
_terrain_vpos += Kfusion * _aid_src_optical_flow.innovation[0];
|
||||||
const float KyHyP = Ky * Hy * _terrain_var;
|
// constrain terrain to minimum allowed value and predict height above ground
|
||||||
|
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
|
||||||
|
|
||||||
_terrain_vpos += Ky * _aid_src_optical_flow.innovation_variance[1];
|
// guard against negative variance
|
||||||
// guard against negative variance
|
_terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f));
|
||||||
_terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f));
|
}
|
||||||
|
|
||||||
|
_fault_status.flags.bad_optflow_X = false;
|
||||||
|
_fault_status.flags.bad_optflow_Y = false;
|
||||||
|
|
||||||
_time_last_flow_terrain_fuse = _time_delayed_us;
|
_time_last_flow_terrain_fuse = _time_delayed_us;
|
||||||
//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?
|
//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?
|
||||||
|
|||||||
Reference in New Issue
Block a user