mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 13:24:06 +08:00
added airdata module which contains wind & airspeed scale estimator (#405)
* added airdata module which contains wind & airspeed scale estimator * airdata: support gate sizes for innovation consistency checks
This commit is contained in:
Roman Bapst
Daniel Agar
parent
f6d23cc621
commit
95c4777b35
@ -37,6 +37,7 @@ px4_add_module(
|
||||
STACK_MAX 6000
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
airdata/WindEstimator.cpp
|
||||
attitude_fw/ecl_controller.cpp
|
||||
attitude_fw/ecl_pitch_controller.cpp
|
||||
attitude_fw/ecl_roll_controller.cpp
|
||||
|
||||
356
airdata/WindEstimator.cpp
Normal file
356
airdata/WindEstimator.cpp
Normal file
@ -0,0 +1,356 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. 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 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 WindEstimator.cpp
|
||||
* A wind and airspeed scale estimator.
|
||||
*/
|
||||
|
||||
#include "WindEstimator.hpp"
|
||||
|
||||
|
||||
bool
|
||||
WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas)
|
||||
{
|
||||
// do no initialise if ground velocity is low
|
||||
// this should prevent the filter from initialising on the ground
|
||||
if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float v_n = velI(0);
|
||||
const float v_e = velI(1);
|
||||
|
||||
// estimate heading from ground velocity
|
||||
const float heading_est = atan2f(v_e, v_n);
|
||||
|
||||
// initilaise wind states assuming zero side slip and horizontal flight
|
||||
_state(w_n) = velI(w_n) - tas_meas * cosf(heading_est);
|
||||
_state(w_e) = velI(w_e) - tas_meas * sinf(heading_est);
|
||||
_state(tas) = 1.0f;
|
||||
|
||||
// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
|
||||
float L0 = v_e * v_e;
|
||||
float L1 = v_n * v_n;
|
||||
float L2 = L0 + L1;
|
||||
float L3 = tas_meas / (L2 * sqrtf(L2));
|
||||
float L4 = L3 * v_e * v_n + 1.0f;
|
||||
float L5 = 1.0f / sqrtf(L2);
|
||||
float L6 = -L5 * tas_meas;
|
||||
|
||||
matrix::Matrix3f L;
|
||||
L.setZero();
|
||||
L(0, 0) = L4;
|
||||
L(0, 1) = L0 * L3 + L6;
|
||||
L(1, 0) = L1 * L3 + L6;
|
||||
L(1, 1) = L4;
|
||||
L(2, 2) = 1.0f;
|
||||
|
||||
// get an estimate of the state covariance matrix given the estimated variance of ground velocity
|
||||
// and measured airspeed
|
||||
_P.setZero();
|
||||
_P(w_n, w_n) = velIvar(0);
|
||||
_P(w_e, w_e) = velIvar(1);
|
||||
_P(tas, tas) = 0.0001f;
|
||||
|
||||
_P = L * _P * L.transpose();
|
||||
|
||||
// reset the timestamp for measurement rejection
|
||||
_time_rejected_tas = 0;
|
||||
_time_rejected_beta = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
WindEstimator::update(uint64_t time_now)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
// run covariance prediction at 1Hz
|
||||
if (time_now - _time_last_update < 1000 * 1000 || _time_last_update == 0) {
|
||||
if (_time_last_update == 0) {
|
||||
_time_last_update = time_now;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
float dt = (float)(time_now - _time_last_update) * 1e-6f;
|
||||
_time_last_update = time_now;
|
||||
|
||||
float q_w = _wind_p_var;
|
||||
float q_k_tas = _tas_scale_p_var;
|
||||
|
||||
float SPP0 = dt * dt;
|
||||
float SPP1 = SPP0 * q_w;
|
||||
float SPP2 = SPP1 + _P(0, 1);
|
||||
|
||||
matrix::Matrix3f P_next;
|
||||
|
||||
P_next(0, 0) = SPP1 + _P(0, 0);
|
||||
P_next(0, 1) = SPP2;
|
||||
P_next(0, 2) = _P(0, 2);
|
||||
P_next(1, 0) = SPP2;
|
||||
P_next(1, 1) = SPP1 + _P(1, 1);
|
||||
P_next(1, 2) = _P(1, 2);
|
||||
P_next(2, 0) = _P(0, 2);
|
||||
P_next(2, 1) = _P(1, 2);
|
||||
P_next(2, 2) = SPP0 * q_k_tas + _P(2, 2);
|
||||
_P = P_next;
|
||||
}
|
||||
|
||||
void
|
||||
WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
|
||||
const matrix::Vector2f &velIvar)
|
||||
{
|
||||
matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) };
|
||||
|
||||
if (!_initialised) {
|
||||
// try to initialise
|
||||
_initialised = initialise(velI, velIvar_constrained, true_airspeed);
|
||||
return;
|
||||
}
|
||||
|
||||
// don't fuse faster than 10Hz
|
||||
if (time_now - _time_last_airspeed_fuse < 100 * 1000) {
|
||||
return;
|
||||
}
|
||||
|
||||
_time_last_airspeed_fuse = time_now;
|
||||
|
||||
// assign helper variables
|
||||
const float v_n = velI(0);
|
||||
const float v_e = velI(1);
|
||||
const float v_d = velI(2);
|
||||
|
||||
const float k_tas = _state(tas);
|
||||
|
||||
// compute kalman gain K
|
||||
const float HH0 = sqrtf(v_d * v_d + (v_e - w_e) * (v_e - w_e) + (v_n - w_n) * (v_n - w_n));
|
||||
const float HH1 = k_tas / HH0;
|
||||
|
||||
matrix::Matrix<float, 1, 3> H_tas;
|
||||
H_tas(0, 0) = HH1 * (-v_n + w_n);
|
||||
H_tas(0, 1) = HH1 * (-v_e + w_e);
|
||||
H_tas(0, 2) = HH0;
|
||||
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
|
||||
|
||||
const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
|
||||
|
||||
K /= (S._data[0][0]);
|
||||
// compute innovation
|
||||
const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) *
|
||||
(v_e - _state(w_e)) + v_d * v_d);
|
||||
|
||||
_tas_innov = true_airspeed - airspeed_pred;
|
||||
|
||||
// innovation variance
|
||||
_tas_innov_var = S._data[0][0];
|
||||
|
||||
bool reinit_filter = false;
|
||||
bool meas_is_rejected = false;
|
||||
|
||||
meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas, reinit_filter);
|
||||
|
||||
reinit_filter |= _tas_innov_var < 0.0f;
|
||||
|
||||
if (meas_is_rejected || reinit_filter) {
|
||||
if (reinit_filter) {
|
||||
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed);
|
||||
}
|
||||
|
||||
// we either did a filter reset or the current measurement was rejected so do not fuse
|
||||
return;
|
||||
}
|
||||
|
||||
// apply correction to state
|
||||
_state(w_n) += _tas_innov * K(0, 0);
|
||||
_state(w_e) += _tas_innov * K(1, 0);
|
||||
_state(tas) += _tas_innov * K(2, 0);
|
||||
|
||||
// update covariance matrix
|
||||
_P = _P - K * H_tas * _P;
|
||||
|
||||
run_sanity_checks();
|
||||
}
|
||||
|
||||
void
|
||||
WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
|
||||
{
|
||||
if (!_initialised) {
|
||||
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length());
|
||||
return;
|
||||
}
|
||||
|
||||
// don't fuse faster than 10Hz
|
||||
if (time_now - _time_last_beta_fuse < 100 * 1000) {
|
||||
return;
|
||||
}
|
||||
|
||||
_time_last_beta_fuse = time_now;
|
||||
|
||||
const float v_n = velI(0);
|
||||
const float v_e = velI(1);
|
||||
const float v_d = velI(2);
|
||||
|
||||
// compute sideslip observation vector
|
||||
float HB0 = 2.0f * q_att(0);
|
||||
float HB1 = HB0 * q_att(3);
|
||||
float HB2 = 2.0f * q_att(1);
|
||||
float HB3 = HB2 * q_att(2);
|
||||
float HB4 = v_e - w_e;
|
||||
float HB5 = HB1 + HB3;
|
||||
float HB6 = v_n - w_n;
|
||||
float HB7 = q_att(0) * q_att(0);
|
||||
float HB8 = q_att(3) * q_att(3);
|
||||
float HB9 = HB7 - HB8;
|
||||
float HB10 = q_att(1) * q_att(1);
|
||||
float HB11 = q_att(2) * q_att(2);
|
||||
float HB12 = HB10 - HB11;
|
||||
float HB13 = HB12 + HB9;
|
||||
float HB14 = HB13 * HB6 + HB4 * HB5 + v_d * (-HB0 * q_att(2) + HB2 * q_att(3));
|
||||
float HB15 = 1.0f / HB14;
|
||||
float HB16 = (HB4 * (-HB10 + HB11 + HB9) + HB6 * (-HB1 + HB3) + v_d * (HB0 * q_att(1) + 2.0f * q_att(2) * q_att(3))) /
|
||||
(HB14 * HB14);
|
||||
|
||||
matrix::Matrix<float, 1, 3> H_beta;
|
||||
H_beta(0, 0) = HB13 * HB16 + HB15 * (HB1 - HB3);
|
||||
H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
|
||||
H_beta(0, 2) = 0;
|
||||
|
||||
// compute kalman gain
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
|
||||
|
||||
const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
|
||||
|
||||
K /= (S._data[0][0]);
|
||||
|
||||
// compute predicted side slip angle
|
||||
matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2));
|
||||
matrix::Dcmf R_body_to_earth(q_att);
|
||||
rel_wind = R_body_to_earth.transpose() * rel_wind;
|
||||
|
||||
if (fabsf(rel_wind(0)) < 0.1f) {
|
||||
return;
|
||||
}
|
||||
|
||||
// use small angle approximation, sin(x) = x for small x
|
||||
const float beta_pred = rel_wind(1) / rel_wind(0);
|
||||
|
||||
_beta_innov = 0.0f - beta_pred;
|
||||
_beta_innov_var = S._data[0][0];
|
||||
|
||||
bool reinit_filter = false;
|
||||
bool meas_is_rejected = false;
|
||||
|
||||
meas_is_rejected = check_if_meas_is_rejected(time_now, _beta_innov, _beta_innov_var, _beta_gate, _time_rejected_beta,
|
||||
reinit_filter);
|
||||
|
||||
reinit_filter |= _beta_innov_var < 0.0f;
|
||||
|
||||
if (meas_is_rejected || reinit_filter) {
|
||||
if (reinit_filter) {
|
||||
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length());
|
||||
}
|
||||
|
||||
// we either did a filter reset or the current measurement was rejected so do not fuse
|
||||
return;
|
||||
}
|
||||
|
||||
// apply correction to state
|
||||
_state(w_n) += _beta_innov * K(0, 0);
|
||||
_state(w_e) += _beta_innov * K(1, 0);
|
||||
_state(tas) += _beta_innov * K(2, 0);
|
||||
|
||||
// update covariance matrix
|
||||
_P = _P - K * H_beta * _P;
|
||||
|
||||
run_sanity_checks();
|
||||
}
|
||||
|
||||
void
|
||||
WindEstimator::run_sanity_checks()
|
||||
{
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
if (_P(i, i) < 0.0f) {
|
||||
// ill-conditioned covariance matrix, reset filter
|
||||
_initialised = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// limit covariance diagonals if they grow too large
|
||||
if (i < 2) {
|
||||
_P(i, i) = _P(i, i) > 25.0f ? 25.0f : _P(i, i);
|
||||
|
||||
} else if (i == 2) {
|
||||
_P(i, i) = _P(i, i) > 0.1f ? 0.1f : _P(i, i);
|
||||
}
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_state(w_n)) || !PX4_ISFINITE(_state(w_e)) || !PX4_ISFINITE(_state(tas))) {
|
||||
_initialised = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// constrain airspeed scale factor, negative values physically do not make sense
|
||||
_state(tas) = math::max(0.0f, _state(tas));
|
||||
|
||||
// attain symmetry
|
||||
for (unsigned row = 0; row < 3; row++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
float tmp = (_P(row, column) + _P(column, row)) / 2;
|
||||
_P(row, column) = tmp;
|
||||
_P(column, row) = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected,
|
||||
bool &reinit_filter)
|
||||
{
|
||||
if (innov * innov > gate_size * gate_size * innov_var) {
|
||||
time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected;
|
||||
|
||||
} else {
|
||||
time_meas_rejected = 0;
|
||||
}
|
||||
|
||||
reinit_filter = time_now - time_meas_rejected > 5 * 1000 * 1000 && time_meas_rejected != 0;
|
||||
|
||||
return time_meas_rejected != 0;
|
||||
}
|
||||
126
airdata/WindEstimator.hpp
Normal file
126
airdata/WindEstimator.hpp
Normal file
@ -0,0 +1,126 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. 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 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 WindEstimator.hpp
|
||||
* A wind and airspeed scale estimator.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
class WindEstimator
|
||||
{
|
||||
public:
|
||||
WindEstimator() = default;
|
||||
~WindEstimator() = default;
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
WindEstimator(const WindEstimator &) = delete;
|
||||
WindEstimator &operator=(const WindEstimator &) = delete;
|
||||
WindEstimator(WindEstimator &&) = delete;
|
||||
WindEstimator &operator=(WindEstimator &&) = delete;
|
||||
|
||||
void update(uint64_t time_now);
|
||||
|
||||
void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
|
||||
const matrix::Vector2f &velIvar);
|
||||
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);
|
||||
|
||||
void get_wind(float wind[2])
|
||||
{
|
||||
wind[0] = _state(w_n);
|
||||
wind[1] = _state(w_e);
|
||||
}
|
||||
|
||||
bool is_estimate_valid() { return _initialised; }
|
||||
|
||||
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected,
|
||||
bool &reinit_filter);
|
||||
|
||||
float get_tas_scale() { return _state(tas); }
|
||||
float get_tas_innov() { return _tas_innov; }
|
||||
float get_tas_innov_var() { return _tas_innov_var; }
|
||||
float get_beta_innov() { return _beta_innov; }
|
||||
float get_beta_innov_var() { return _beta_innov_var; }
|
||||
void get_wind_var(float wind_var[2])
|
||||
{
|
||||
wind_var[0] = _P(0, 0);
|
||||
wind_var[1] = _P(1, 1);
|
||||
}
|
||||
|
||||
void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; }
|
||||
void set_tas_scale_p_noise(float tas_scale_sigma) { _tas_scale_p_var = tas_scale_sigma * tas_scale_sigma; }
|
||||
void set_tas_noise(float tas_sigma) { _tas_var = tas_sigma * tas_sigma; }
|
||||
void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
|
||||
void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
|
||||
void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
|
||||
|
||||
private:
|
||||
enum {
|
||||
w_n = 0,
|
||||
w_e,
|
||||
tas
|
||||
}; ///< enum which can be used to access state.
|
||||
|
||||
matrix::Vector3f _state; ///< state vector
|
||||
matrix::Matrix3f _P; ///< state covariance matrix
|
||||
|
||||
float _tas_innov{0.0f}; ///< true airspeed innovation
|
||||
float _tas_innov_var{0.0f}; ///< true airspeed innovation variance
|
||||
|
||||
float _beta_innov{0.0f}; ///< sideslip innovation
|
||||
float _beta_innov_var{0.0f}; ///< sideslip innovation variance
|
||||
|
||||
bool _initialised{false}; ///< True: filter has been initialised
|
||||
|
||||
float _wind_p_var{0.1f}; ///< wind process noise variance
|
||||
float _tas_scale_p_var{0.0001f}; ///< true airspeed scale process noise variance
|
||||
float _tas_var{1.4f}; ///< true airspeed measurement noise variance
|
||||
float _beta_var{0.5f}; ///< sideslip measurement noise variance
|
||||
uint8_t _tas_gate{3}; ///< airspeed fusion gate size
|
||||
uint8_t _beta_gate{1}; ///< sideslip fusion gate size
|
||||
|
||||
uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion
|
||||
uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion
|
||||
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
|
||||
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
|
||||
uint64_t _time_rejected_tas =
|
||||
0; ///<timestamp of when true airspeed measurements have consistently started to be rejected
|
||||
|
||||
// initialise state and state covariance matrix
|
||||
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);
|
||||
|
||||
void run_sanity_checks();
|
||||
};
|
||||
188
airdata/python/wind_est_derivation.py
Normal file
188
airdata/python/wind_est_derivation.py
Normal file
@ -0,0 +1,188 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Tue Nov 1 19:14:39 2016
|
||||
|
||||
@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 #######################################
|
||||
|
||||
# model state
|
||||
w_n = Symbol("w_n", real=True) # wind in north direction
|
||||
w_e = Symbol("w_e", real=True) # wind in east direction
|
||||
k_tas = Symbol("k_tas", real=True) # true airspeed scale factor
|
||||
state = Matrix([w_n, w_e, k_tas])
|
||||
|
||||
# process noise
|
||||
q_w = Symbol("q_w", real=True) # process noise for wind states
|
||||
q_k_tas = Symbol("q_k_tas", real=True) # process noise for airspeed scale state
|
||||
|
||||
# airspeed measurement noise
|
||||
r_tas = Symbol("r_tas", real=True)
|
||||
|
||||
# sideslip measurement noise
|
||||
r_beta = Symbol("r_beta", real=True)
|
||||
|
||||
# true airspeed measurement
|
||||
tas_meas = Symbol("tas_meas", real=True)
|
||||
|
||||
# ground velocity variance
|
||||
v_n_var = Symbol("v_n_var", real=True)
|
||||
v_e_var = Symbol("v_e_var", real=True)
|
||||
|
||||
#################### time varying parameters ##################################
|
||||
|
||||
# vehicle velocity
|
||||
v_n = Symbol("v_n", real=True) # north velocity in earth fixed frame
|
||||
v_e = Symbol("v_e", real=True) # east velocity in earth fixed frame
|
||||
v_d = Symbol("v_d", real=True) # down velocity in earth fixed frame
|
||||
|
||||
# unit quaternion describing vehicle attitude, qw is real part
|
||||
qw = Symbol("q_att[0]", real=True)
|
||||
qx = Symbol("q_att[1]", real=True)
|
||||
qy = Symbol("q_att[2]", real=True)
|
||||
qz = Symbol("q_att[3]", real=True)
|
||||
q_att = Matrix([qw, qx, qy, qz])
|
||||
|
||||
# sampling time in seconds
|
||||
dt = Symbol("dt", real=True)
|
||||
|
||||
######################## State and covariance prediction ######################
|
||||
|
||||
# state transition matrix is zero because we are using a stationary
|
||||
# process model. We only need to provide formula for covariance prediction
|
||||
|
||||
# create process noise matrix for covariance prediction
|
||||
state_new = state + Matrix([q_w, q_w, q_k_tas]) * dt
|
||||
Q = diag(q_w, q_k_tas)
|
||||
L = state_new.jacobian([q_w, q_k_tas])
|
||||
Q = L * Q * Transpose(L)
|
||||
|
||||
# define symbolic covariance matrix
|
||||
p00 = Symbol('_P(0,0)', real=True)
|
||||
p01 = Symbol('_P(0,1)', real=True)
|
||||
p02 = Symbol('_P(0,2)', real=True)
|
||||
p12 = Symbol('_P(1,2)', real=True)
|
||||
p11 = Symbol('_P(1,1)', real=True)
|
||||
p22 = Symbol('_P(2,2)', real=True)
|
||||
P = Matrix([[p00, p01, p02], [p01, p11, p12], [p02, p12, p22]])
|
||||
|
||||
# covariance prediction equation
|
||||
P_next = P + Q
|
||||
|
||||
# simplify the result and write it to a text file in C format
|
||||
PP_simple = cse(P_next, symbols('SPP0:30'))
|
||||
P_pred = Matrix(PP_simple[1])
|
||||
write_simplified(PP_simple, "cov_pred.txt", 'P_next')
|
||||
|
||||
|
||||
############################ Measurement update ###############################
|
||||
|
||||
# airspeed fusion
|
||||
|
||||
tas_pred = Matrix([((v_n - w_n)**2 + (v_e - w_e)**2 + v_d**2)**0.5]) * k_tas
|
||||
# compute true airspeed observation matrix
|
||||
H_tas = tas_pred.jacobian(state)
|
||||
# simplify the result and write it to a text file in C format
|
||||
H_tas_simple = cse(H_tas, symbols('HH0:30'))
|
||||
write_simplified(H_tas_simple, "airspeed_fusion.txt", 'H_tas')
|
||||
K = P * Transpose(H_tas)
|
||||
denom = H_tas * P * Transpose(H_tas) + Matrix([r_tas])
|
||||
denom = 1/denom.values()[0]
|
||||
K = K * denom
|
||||
|
||||
K_simple = cse(K, symbols('KTAS0:30'))
|
||||
write_simplified(K_simple, "airspeed_fusion.txt", "K")
|
||||
|
||||
P_m = P - K*H_tas*P
|
||||
P_m_simple = cse(P_m, symbols('PM0:50'))
|
||||
write_simplified(P_m_simple, "airspeed_fusion.txt", "P_next")
|
||||
|
||||
# sideslip fusion
|
||||
|
||||
# compute relative wind vector in vehicle body frame
|
||||
relative_wind_earth = Matrix([v_n - w_n, v_e - w_e, v_d])
|
||||
R_body_to_earth = quat2Rot(q_att)
|
||||
relative_wind_body = Transpose(R_body_to_earth) * relative_wind_earth
|
||||
# small angle approximation of side slip model
|
||||
beta_pred = relative_wind_body[1] / relative_wind_body[0]
|
||||
# compute side slip observation matrix
|
||||
H_beta = Matrix([beta_pred]).jacobian(state)
|
||||
# simplify the result and write it to a text file in C format
|
||||
H_beta_simple = cse(H_beta, symbols('HB0:30'))
|
||||
write_simplified(H_beta_simple, "beta_fusion.txt", 'H_beta')
|
||||
K = P * Transpose(H_beta)
|
||||
denom = H_beta * P * Transpose(H_beta) + Matrix([r_beta])
|
||||
denom = 1/denom.values()[0]
|
||||
K = K*denom
|
||||
K_simple = cse(K, symbols('KB0:30'))
|
||||
write_simplified(K_simple, "beta_fusion.txt", 'K')
|
||||
|
||||
P_m = P - K*H_beta*P
|
||||
P_m_simple = cse(P_m, symbols('PM0:50'))
|
||||
write_simplified(P_m_simple, "beta_fusion.txt", "P_next")
|
||||
|
||||
# wind covariance initialisation via velocity
|
||||
|
||||
# estimate heading from ground velocity
|
||||
heading_est = atan2(v_n, v_e)
|
||||
|
||||
# calculate wind speed estimate from vehicle ground velocity, heading and
|
||||
# airspeed measurement
|
||||
w_n_est = v_n - tas_meas * cos(heading_est)
|
||||
w_e_est = v_e - tas_meas * sin(heading_est)
|
||||
wind_est = Matrix([w_n_est, w_e_est])
|
||||
|
||||
# calculate estimate of state covariance matrix
|
||||
P_wind = diag(v_n_var, v_e_var, r_tas)
|
||||
|
||||
wind_jac = wind_est.jacobian([v_n, v_e, tas_meas])
|
||||
wind_jac_simple = cse(wind_jac, symbols('L0:30'))
|
||||
write_simplified(wind_jac_simple, "cov_init.txt", "L")
|
||||
Loading…
x
Reference in New Issue
Block a user