dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix code style

This commit is contained in:
Paul Riseborough 2016-02-25 08:17:50 +11:00
parent 5b5bddebea
commit f55a0bff53
6 changed files with 152 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/common.h
View File

@ -156,7 +156,7 @@ struct parameters {
float req_hdrift; // maximum acceptable horizontal drift speed
float req_vdrift; // maximum acceptable vertical drift speed
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
mag_delay_ms = 0.0f;

8
EKF/estimator_interface.cpp
View File

@ -68,9 +68,9 @@ EstimatorInterface::EstimatorInterface():
_mag_declination_gps(0.0f),
_mag_declination_to_save_deg(0.0f)
{
_pos_ref = {};
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
_pos_ref = {};
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio));
}
EstimatorInterface::~EstimatorInterface()
@ -202,7 +202,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data)
if (time_usec > _time_last_airspeed) {
airspeedSample airspeed_sample_new;
airspeed_sample_new.airspeed = *data;
airspeed_sample_new.time_us = time_usec -_params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_airspeed = time_usec;

26
EKF/geo.cpp
View File

@ -211,13 +211,13 @@ uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
}
int map_projection_global_init(double lat_0, double lon_0,
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
}
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat_rad = lat_0 * M_DEG_TO_RAD;
@ -237,7 +237,7 @@ int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
}
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
double *ref_lon_rad)
double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
@ -256,7 +256,7 @@ int map_projection_global_project(double lat, double lon, float *x, float *y)
}
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y)
float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
@ -293,7 +293,7 @@ int map_projection_global_reproject(float x, float y, double *lat, double *lon)
}
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon)
double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
@ -419,7 +419,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n
}
void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target)
double *lat_target, double *lon_target)
{
if (fabsf(dist) < FLT_EPSILON) {
*lat_target = lat_A;
@ -437,7 +437,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
}
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target)
double *lat_target, double *lon_target)
{
bearing = _wrap_2pi(bearing);
double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
@ -472,7 +472,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
}
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
float *v_e)
float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -488,7 +488,7 @@ void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next
}
void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next,
float *v_n, float *v_e)
float *v_n, float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -504,7 +504,7 @@ void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat
}
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
double *lon_res)
double *lon_res)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@ -516,7 +516,7 @@ void add_vector_to_global_position(double lat_now, double lon_now, float v_n, fl
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_start, double lon_start, double lat_end, double lon_end)
double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
@ -568,8 +568,8 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep)
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and

24
EKF/geo.h
View File

@ -116,7 +116,7 @@ int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
double *ref_lon_rad);
double *ref_lon_rad);
/**
@ -128,7 +128,7 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl
* @param lon in degrees (8.1234567°, not 81234567°)
*/
int map_projection_init_timestamped(struct map_projection_reference_s *ref,
double lat_0, double lon_0, uint64_t timestamp);
double lat_0, double lon_0, uint64_t timestamp);
/**
* Initializes the map transformation given by the argument and sets the timestamp to now.
@ -161,7 +161,7 @@ int map_projection_global_project(double lat, double lon, float *x, float *y);
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y);
float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the
@ -186,7 +186,7 @@ int map_projection_global_reproject(float x, float y, double *lat, double *lon);
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon);
double *lon);
/**
* Get reference position of the global map projection
@ -243,7 +243,7 @@ float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_n
* @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
*/
void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target);
double *lat_target, double *lon_target);
/**
* Creates a waypoint from given waypoint, distance and bearing
@ -257,7 +257,7 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
*/
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target);
double *lat_target, double *lon_target);
/**
* Returns the bearing to the next waypoint in radians.
@ -270,20 +270,20 @@ void waypoint_from_heading_and_distance(double lat_start, double lon_start, floa
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
float *v_e);
float *v_e);
void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next,
float *v_n, float *v_e);
float *v_n, float *v_e);
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
double *lon_res);
double *lon_res);
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_start, double lon_start, double lat_end, double lon_end);
double lat_start, double lon_start, double lat_end, double lon_end);
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
/*
* Calculate distance in global frame

13
EKF/mathlib.h
View File

@ -45,13 +45,14 @@
#include <algorithm>
#define M_PI_F 3.14159265358979323846f
namespace math {
using namespace Eigen;
using namespace std;
namespace math
{
using namespace Eigen;
using namespace std;
float constrain(float &val, float min, float max);
float radians(float degrees);
float degrees(float radians);
float constrain(float &val, float min, float max);
float radians(float degrees);
float degrees(float radians);
}
#else
#include <mathlib/mathlib.h>

217
EKF/vel_pos_fusion.cpp
View File

@ -43,139 +43,148 @@
void Ekf::fuseVelPosHeight()
{
bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available
bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available
bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
// calculate innovations, innovations gate sizes and observation variances
// calculate innovations, innovations gate sizes and observation variances
if (_fuse_hor_vel) {
fuse_map[0] = fuse_map[1] = true;
// horizontal velocity innovations
// horizontal velocity innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
R[0] = fmaxf(R[0], _gps_speed_accuracy);
R[0] = R[0] * R[0];
R[1] = R[0];
// innovation gate sizes
gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
gate_size[1] = gate_size[0];
}
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
R[0] = fmaxf(R[0], _gps_speed_accuracy);
R[0] = R[0] * R[0];
R[1] = R[0];
// innovation gate sizes
gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
gate_size[1] = gate_size[0];
}
if (_fuse_vert_vel) {
fuse_map[2] = true;
// vertical velocity innovation
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy);
R[2] = R[2] * R[2];
// innovation gate size
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
}
// vertical velocity innovation
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy);
R[2] = R[2] * R[2];
// innovation gate size
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
}
if (_fuse_pos) {
fuse_map[3] = fuse_map[4] = true;
// horizontal position innovations
// horizontal position innovations
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
// observation variance - user parameter defined
// if we are in flight and not using GPS, then use a specific parameter
if (!_control_status.flags.gps && _control_status.flags.in_air) {
R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
} else {
R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
}
R[3] = R[3] * R[3];
R[4] = R[3];
// innovation gate sizes
gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
gate_size[4] = gate_size[3];
// observation variance - user parameter defined
// if we are in flight and not using GPS, then use a specific parameter
if (!_control_status.flags.gps && _control_status.flags.in_air) {
R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
} else {
R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
}
R[3] = R[3] * R[3];
R[4] = R[3];
// innovation gate sizes
gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
gate_size[4] = gate_size[3];
}
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);
}
// 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);
}
// calculate innovation test ratios
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (fuse_map[obs_index]) {
// compute the innovation variance SK = HPH + R
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[obs_index]);
}
}
// calculate innovation test ratios
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
if (fuse_map[obs_index]) {
// compute the innovation variance SK = HPH + R
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[obs_index]);
}
}
// check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors
// always pass position checks if using synthetic position measurements
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || using_synthetic_measurements;
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
// check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors
// always pass position checks if using synthetic position measurements
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
&& (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f))
|| using_synthetic_measurements;
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
}
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
}
// record the successful position fusion time
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
}
// record the successful position fusion time
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
}
// record the successful height fusion time
if (innov_check_pass_map[5] && _fuse_height) {
_time_last_hgt_fuse = _time_last_imu;
}
// record the successful height fusion time
if (innov_check_pass_map[5] && _fuse_height) {
_time_last_hgt_fuse = _time_last_imu;
}
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
// skip fusion if not requested or checks have failed
if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
// skip fusion if not requested or checks have failed
if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
continue;
}
unsigned state_index = obs_index + 3; // 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 <= 15; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row <= 15; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
// only update magnetic field states if we are fusing 3-axis observations
if (_control_status.flags.mag_3D) {
for (int row = 16; row <= 21; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
} else {
for (int row = 16; row <= 21; row++) {
Kfusion[row] = 0.0f;
}
}
// only update wind states if we are doing wind estimation
if (_control_status.flags.wind) {
for (int row = 22; row <= 23; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
} else {
for (int row = 22; row <= 23; row++) {
Kfusion[row] = 0.0f;
}
}
// only update magnetic field states if we are fusing 3-axis observations
if (_control_status.flags.mag_3D) {
for (int row = 16; row <= 21; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
} else {
for (int row = 16; row <= 21; row++) {
Kfusion[row] = 0.0f;
}
}
// only update wind states if we are doing wind estimation
if (_control_status.flags.wind) {
for (int row = 22; row <= 23; row++) {
Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
}
} else {
for (int row = 22; row <= 23; row++) {
Kfusion[row] = 0.0f;
}
}
// by definition the angle error state is zero at the fusion time
_state.ang_error.setZero();