fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 05:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Expand auto-format coverage and tiny style changes

Browse Source
This commit is contained in:
kamilritz
2020-01-27 16:54:25 +01:00
committed by Mathieu Bresciani
parent fbdd75da2e
commit f20726d47f
12 changed files with 178 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/RingBuffer.h
+2
View File
@@ -60,6 +60,7 @@ public:
RingBuffer &operator=(RingBuffer &&) = delete;
bool allocate(uint8_t size) {
if (_buffer != nullptr) {
delete[] _buffer;
}
@@ -92,6 +93,7 @@ public:
}
void push(const data_type &sample) {
uint8_t head_new = _head;
if (!_first_write) {
EKF/imu_down_sampler.cpp
+9 -17
View File
@@ -1,24 +1,18 @@
#include "imu_down_sampler.hpp"
ImuDownSampler::ImuDownSampler(float target_dt_sec):
_target_dt{target_dt_sec},
_imu_collection_time_adj{0.0f}
{
ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec}, _imu_collection_time_adj{0.0f} {
reset();
_imu_down_sampled.time_us = 0.0f;
}
ImuDownSampler::~ImuDownSampler()
{
}
ImuDownSampler::~ImuDownSampler() {}
// integrate imu samples until target dt reached
// assumes that dt of the gyroscope is close to the dt of the accelerometer
// returns true if target dt is reached
bool ImuDownSampler::update(imuSample imu_sample_new)
{
if(_do_reset)
{
bool ImuDownSampler::update(imuSample imu_sample_new) {
if (_do_reset) {
reset();
}
// accumulate time deltas
@@ -42,11 +36,11 @@ bool ImuDownSampler::update(imuSample imu_sample_new)
// check if the target time delta between filter prediction steps has been exceeded
if (_imu_down_sampled.delta_ang_dt >= _target_dt - _imu_collection_time_adj) {
// accumulate the amount of time to advance the IMU collection time so that we meet the
// average EKF update rate requirement
_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - _target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt, 0.5f * _target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt,
0.5f * _target_dt);
_imu_down_sampled.delta_ang = _delta_angle_accumulated.to_axis_angle();
@@ -57,14 +51,12 @@ bool ImuDownSampler::update(imuSample imu_sample_new)
}
}
imuSample ImuDownSampler::getDownSampledImuAndTriggerReset()
{
imuSample ImuDownSampler::getDownSampledImuAndTriggerReset() {
_do_reset = true;
return _imu_down_sampled;
}
void ImuDownSampler::reset()
{
void ImuDownSampler::reset() {
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
EKF/imu_down_sampler.hpp
+3 -6
View File
@@ -37,15 +37,14 @@
*/
#pragma once
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include "common.h"
using namespace estimator;
class ImuDownSampler
{
class ImuDownSampler {
public:
ImuDownSampler(float target_dt_sec);
~ImuDownSampler();
@@ -56,11 +55,9 @@ public:
private:
imuSample _imu_down_sampled;
Quatf _delta_angle_accumulated;
const float _target_dt; // [sec]
const float _target_dt; // [sec]
float _imu_collection_time_adj;
bool _do_reset;
void reset();
};
EKF/vel_pos_fusion.cpp
+38 -43
View File
@@ -41,39 +41,37 @@
*
*/
#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
#include "ekf.h"
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(4,4) + obs_var(0);
innov_var(1) = P(5,5) + obs_var(1);
innov_var(0) = P(4, 4) + obs_var(0);
innov_var(1) = P(5, 5) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f);
if (innov_check_pass)
{
if (innov_check_pass) {
_time_last_hor_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hor_vel = false;
fuseVelPosHeight(innov(0),innov_var(0),0);
fuseVelPosHeight(innov(1),innov_var(1),1);
fuseVelPosHeight(innov(0), innov_var(0), 0);
fuseVelPosHeight(innov(1), innov_var(1), 1);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_hor_vel = true;
return false;
}
}
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(6,6) + obs_var(2);
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(2) = P(6, 6) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
const bool innov_check_pass = (test_ratio(1) <= 1.0f);
@@ -81,20 +79,20 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
_time_last_ver_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_vel = false;
fuseVelPosHeight(innov(2),innov_var(2),2);
fuseVelPosHeight(innov(2), innov_var(2), 2);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_ver_vel = true;
return false;
}
}
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(7,7) + obs_var(0);
innov_var(1) = P(8,8) + obs_var(1);
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(0) = P(7, 7) + obs_var(0);
innov_var(1) = P(8, 8) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
@@ -108,20 +106,20 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
}
_innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0),innov_var(0),3);
fuseVelPosHeight(innov(1),innov_var(1),4);
fuseVelPosHeight(innov(0), innov_var(0), 3);
fuseVelPosHeight(innov(1), innov_var(1), 4);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_hor_pos = true;
return false;
}
}
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(9,9) + obs_var(2);
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
const bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align;
@@ -129,31 +127,31 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innov(2),innov_var(2),5);
fuseVelPosHeight(innov(2), innov_var(2), 5);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_ver_pos = true;
return false;
}
}
// Helper function that fuses a single velocity or position measurement
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) {
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < _k_num_states; row++) {
Kfusion[row] = P(row,state_index) / innov_var;
Kfusion[row] = P(row, state_index) / innov_var;
}
matrix::SquareMatrix<float, _k_num_states> KHP;
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
KHP(row,column) = Kfusion[row] * P(state_index,column);
KHP(row, column) = Kfusion[row] * P(state_index, column);
}
}
@@ -162,7 +160,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
bool healthy = true;
for (int i = 0; i < _k_num_states; i++) {
if (P(i,i) < KHP(i,i)) {
if (P(i, i) < KHP(i, i)) {
// zero rows and columns
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
@@ -175,13 +173,12 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
}
}
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
P(row, column) = P(row, column) - KHP(row, column);
}
}
@@ -190,12 +187,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
// apply the state corrections
fuse(Kfusion, innov);
}
}
void Ekf::setVelPosFaultStatus(const int index, const bool status)
{
void Ekf::setVelPosFaultStatus(const int index, const bool status) {
if (index == 0) {
_fault_status.flags.bad_vel_N = status;