Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: add ekf to estimate vehicle-relative target velocity

Browse Source
This commit is contained in:
Jonathan Challinger 2016-07-15 10:30:21 -07:00 committed by Randy Mackay
parent 5a8db4f271
commit 7405f82d61
4 changed files with 239 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -22,6 +22,33 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @User: Advanced
AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0),
// @Param: YAW_ALIGN
// @DisplayName: Sensor yaw alignment
// @Description: Yaw angle from body x-axis to sensor x-axis.
// @Range: 0 360
// @Increment: 1
// @User: Advanced
// @Units: Centi-degrees
AP_GROUPINFO("YAW_ALIGN", 2, AC_PrecLand, _yaw_align, 0),
// @Param: LAND_OFS_X
// @DisplayName: Land offset forward
// @Description: Desired landing position of the camera forward of the target in vehicle body frame
// @Range: -20 20
// @Increment: 1
// @User: Advanced
// @Units: Centimeters
AP_GROUPINFO("LAND_OFS_X", 3, AC_PrecLand, _land_ofs_cm_x, 0),
// @Param: LAND_OFS_Y
// @DisplayName: Land offset right
// @Description: desired landing position of the camera right of the target in vehicle body frame
// @Range: -20 20
// @Increment: 1
// @User: Advanced
// @Units: Centimeters
AP_GROUPINFO("LAND_OFS_Y", 4, AC_PrecLand, _land_ofs_cm_y, 0),
AP_GROUPEND
};
@ -81,81 +108,126 @@ void AC_PrecLand::init()
}
// update - give chance to driver to get updates from sensor
void AC_PrecLand::update(float alt_above_terrain_cm)
void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
{
_attitude_history.push_back(_ahrs.get_rotation_body_to_ned());
// run backend update
if (_backend != NULL && _enabled) {
// read from sensor
_backend->update();
Vector3f vehicleVelocityNED = _inav.get_velocity()*0.01f;
vehicleVelocityNED.z = -vehicleVelocityNED.z;
if (target_acquired()) {
// EKF prediction step
float dt;
Vector3f targetDelVel;
_ahrs.getCorrectedDeltaVelocityNED(targetDelVel, dt);
targetDelVel = -targetDelVel;
_ekf_x.predict(dt, targetDelVel.x, 0.5f*dt);
_ekf_y.predict(dt, targetDelVel.y, 0.5f*dt);
if (_inav.get_filter_status().flags.horiz_pos_rel) {
_ekf_x.fuseVel(-vehicleVelocityNED.x, sq(1.0f));
_ekf_y.fuseVel(-vehicleVelocityNED.y, sq(1.0f));
}
}
if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
// we have a new, unique los measurement
_last_backend_los_meas_ms = _backend->los_meas_time_ms();
Vector3f target_vec_unit_body;
_backend->get_los_body(target_vec_unit_body);
calc_angles_and_pos(target_vec_unit_body, alt_above_terrain_cm);
// Apply sensor yaw alignment rotation
float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
Matrix3f Rz = Matrix3f(
cos_yaw_align, -sin_yaw_align, 0,
sin_yaw_align, cos_yaw_align, 0,
0, 0, 1
);
Vector3f target_vec_unit_ned = _attitude_history.front() * Rz * target_vec_unit_body;
bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
if (target_vec_valid && rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) {
float alt = MAX(rangefinder_alt_cm*0.01f, 0.0f);
float dist = alt/target_vec_unit_ned.z;
Vector3f targetPosRelMeasNED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
float xy_pos_var = sq(targetPosRelMeasNED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
if (!target_acquired()) {
// reset filter state
if (_inav.get_filter_status().flags.horiz_pos_rel) {
_ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, -vehicleVelocityNED.x, sq(1.0f));
_ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, -vehicleVelocityNED.y, sq(1.0f));
} else {
_ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, 0.0f, sq(10.0f));
_ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, 0.0f, sq(10.0f));
}
_last_update_ms = AP_HAL::millis();
} else {
float NIS_x = _ekf_x.getPosNIS(targetPosRelMeasNED.x, xy_pos_var);
float NIS_y = _ekf_y.getPosNIS(targetPosRelMeasNED.y, xy_pos_var);
if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
_outlier_reject_count = 0;
_ekf_x.fusePos(targetPosRelMeasNED.x, xy_pos_var);
_ekf_y.fusePos(targetPosRelMeasNED.y, xy_pos_var);
_last_update_ms = AP_HAL::millis();
} else {
_outlier_reject_count++;
}
}
}
}
}
}
bool AC_PrecLand::target_acquired()
bool AC_PrecLand::target_acquired() const
{
return (AP_HAL::millis()-_last_update_ms) < 1000;
return (AP_HAL::millis()-_last_update_ms) < 2000;
}
bool AC_PrecLand::get_target_position(Vector3f& ret)
bool AC_PrecLand::get_target_position_cm(Vector2f& ret) const
{
if (!target_acquired()) {
return false;
}
ret = _target_pos;
Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
ret.x = _ekf_x.getPos()*100.0f + _inav.get_position().x + land_ofs_ned_cm.x;
ret.y = _ekf_y.getPos()*100.0f + _inav.get_position().y + land_ofs_ned_cm.y;
return true;
}
bool AC_PrecLand::get_target_position_relative(Vector3f& ret)
bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret) const
{
if (!target_acquired()) {
return false;
}
ret = _target_pos_rel;
Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
ret.x = _ekf_x.getPos()*100.0f + land_ofs_ned_cm.x;
ret.y = _ekf_y.getPos()*100.0f + land_ofs_ned_cm.y;
return true;
}
bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret)
bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const
{
return false;
}
// converts sensor's body-frame angles to earth-frame angles and position estimate
// raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame)
// earth-frame angles stored in _ef_angle_to_target
// position estimate is stored in _target_pos
void AC_PrecLand::calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm)
{
// rotate into NED frame
Vector3f target_vec_unit_ned = _ahrs.get_rotation_body_to_ned()*target_vec_unit_body;
// extract the angles to target (logging only)
_angle_to_target.x = atan2f(-target_vec_unit_body.y, target_vec_unit_body.z);
_angle_to_target.y = atan2f( target_vec_unit_body.x, target_vec_unit_body.z);
_ef_angle_to_target.x = atan2f(-target_vec_unit_ned.y, target_vec_unit_ned.z);
_ef_angle_to_target.y = atan2f( target_vec_unit_ned.x, target_vec_unit_ned.z);
if (target_vec_unit_ned.z > 0.0f) {
// get current altitude (constrained to be positive)
float alt = MAX(alt_above_terrain_cm, 0.0f);
float dist = alt/target_vec_unit_ned.z;
_target_pos_rel.x = target_vec_unit_ned.x*dist;
_target_pos_rel.y = target_vec_unit_ned.y*dist;
_target_pos_rel.z = alt; // not used
_target_pos = _inav.get_position()+_target_pos_rel;
_last_update_ms = AP_HAL::millis();
if (!target_acquired()) {
return false;
}
ret.x = _ekf_x.getVel()*100.0f;
ret.y = _ekf_y.getVel()*100.0f;
return true;
}
// handle_msg - Process a LANDING_TARGET mavlink message

43
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -6,6 +6,8 @@
#include <AP_InertialNav/AP_InertialNav.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdint.h>
#include "PosVelEKF.h"
#include <AP_Buffer/AP_Buffer.h>
// declare backend classes
class AC_PrecLand_Backend;
@ -42,49 +44,36 @@ public:
void init();
// returns true if precision landing is healthy
bool healthy() { return _backend_state.healthy; }
bool healthy() const { return _backend_state.healthy; }
// returns time of last update
uint32_t last_update_ms() { return _last_update_ms; }
uint32_t last_update_ms() const { return _last_update_ms; }
// give chance to driver to get updates from sensor
void update(float alt_above_terrain_cm);
// returns 3D vector of earth-frame position adjustments to target
Vector3f get_target_shift(const Vector3f& orig_target);
void update(float rangefinder_alt_cm, bool rangefinder_alt_valid);
// returns target position relative to origin
bool get_target_position(Vector3f& ret);
bool get_target_position_cm(Vector2f& ret) const;
// returns target position relative to vehicle
bool get_target_position_relative(Vector3f& ret);
bool get_target_position_relative_cm(Vector2f& ret) const;
// returns target velocity relative to vehicle
bool get_target_velocity_relative(Vector3f& ret);
bool get_target_velocity_relative_cms(Vector2f& ret) const;
// returns true when the landing target has been detected
bool target_acquired();
bool target_acquired() const;
// process a LANDING_TARGET mavlink message
void handle_msg(mavlink_message_t* msg);
// accessors for logging
bool enabled() const { return _enabled; }
const Vector2f& last_bf_angle_to_target() const { return _angle_to_target; }
const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; }
const Vector3f& last_target_pos_offset() const { return _target_pos_rel; }
// parameter var table
static const struct AP_Param::GroupInfo var_info[];
private:
// converts sensor's body-frame angles to earth-frame angles and position estimate
// angles stored in _angle_to_target
// earth-frame angles stored in _ef_angle_to_target
// position estimate is stored in _target_pos
void calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm);
// returns enabled parameter as an behaviour
enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
@ -95,17 +84,17 @@ private:
// parameters
AP_Int8 _enabled; // enabled/disabled and behaviour
AP_Int8 _type; // precision landing controller type
AP_Float _yaw_align; // Yaw angle from body x-axis to sensor x-axis.
AP_Float _land_ofs_cm_x; // Desired landing position of the camera forward of the target in vehicle body frame
AP_Float _land_ofs_cm_y; // Desired landing position of the camera right of the target in vehicle body frame
uint32_t _last_update_ms; // epoch time in millisecond when update is called
uint32_t _last_backend_los_meas_ms;
// output from sensor (stored for logging)
Vector2f _angle_to_target; // last raw sensor angle to target
Vector2f _ef_angle_to_target;// last earth-frame angle to target
// estimator output
Vector3f _target_pos_rel; // estimate target position relative to vehicle in NEU cm
Vector3f _target_pos; // estimate target position in NEU cm
PosVelEKF _ekf_x, _ekf_y;
uint32_t _outlier_reject_count;
AP_Buffer<Matrix3f,8> _attitude_history;
// backend state
struct precland_state {

93
libraries/AC_PrecLand/PosVelEKF.cpp Normal file
View File

@ -0,0 +1,93 @@
#include "PosVelEKF.h"
#include <math.h>
#include <string.h>
#define POSVELEKF_POS_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \
__RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R);
#define POSVELEKF_POS_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \
__RET_STATE[0] = __P[0]*(-__X[0] + __Z)/(__P[0] + __R) + __X[0]; __RET_STATE[1] = __P[1]*(-__X[0] + \
__Z)/(__P[0] + __R) + __X[1];
#define POSVELEKF_POS_CALC_COV(__P, __R, __X, __Z, __RET_COV) \
__RET_COV[0] = ((__P[0])*(__P[0]))*__R/((__P[0] + __R)*(__P[0] + __R)) + __P[0]*((-__P[0]/(__P[0] + \
__R) + 1)*(-__P[0]/(__P[0] + __R) + 1)); __RET_COV[1] = __P[0]*__P[1]*__R/((__P[0] + __R)*(__P[0] + \
__R)) - __P[0]*__P[1]*(-__P[0]/(__P[0] + __R) + 1)/(__P[0] + __R) + __P[1]*(-__P[0]/(__P[0] + __R) + \
1); __RET_COV[2] = ((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - \
((__P[1])*(__P[1]))/(__P[0] + __R) - __P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + \
__P[2];
#define POSVELEKF_PREDICTION_CALC_STATE(__P, __DT, __DV, __DV_NOISE, __X, __RET_STATE) \
__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1];
#define POSVELEKF_PREDICTION_CALC_COV(__P, __DT, __DV, __DV_NOISE, __X, __RET_COV) \
__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[2] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[2] + \
__P[1]; __RET_COV[2] = ((__DV_NOISE)*(__DV_NOISE)) + __P[2];
#define POSVELEKF_VEL_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \
__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R);
#define POSVELEKF_VEL_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \
__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[2] + __R) + __X[0]; __RET_STATE[1] = __P[2]*(-__X[1] + \
__Z)/(__P[2] + __R) + __X[1];
#define POSVELEKF_VEL_CALC_COV(__P, __R, __X, __Z, __RET_COV) \
__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[2] + __R)*(__P[2] + __R)) - \
((__P[1])*(__P[1]))/(__P[2] + __R) - __P[1]*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1])/(__P[2] + __R); \
__RET_COV[1] = __P[1]*__P[2]*__R/((__P[2] + __R)*(__P[2] + __R)) + (-__P[2]/(__P[2] + __R) + \
1)*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1]); __RET_COV[2] = ((__P[2])*(__P[2]))*__R/((__P[2] + \
__R)*(__P[2] + __R)) + __P[2]*((-__P[2]/(__P[2] + __R) + 1)*(-__P[2]/(__P[2] + __R) + 1));
void PosVelEKF::init(float pos, float posVar, float vel, float velVar)
{
_state[0] = pos;
_state[1] = vel;
_cov[0] = posVar;
_cov[1] = 0.0f;
_cov[2] = velVar;
}
void PosVelEKF::predict(float dt, float dVel, float dVelNoise)
{
float newState[2];
float newCov[3];
POSVELEKF_PREDICTION_CALC_STATE(_cov, dt, dVel, dVelNoise, _state, newState)
POSVELEKF_PREDICTION_CALC_COV(_cov, dt, dVel, dVelNoise, _state, newCov)
memcpy(_state,newState,sizeof(_state));
memcpy(_cov,newCov,sizeof(_cov));
}
void PosVelEKF::fusePos(float pos, float posVar)
{
float newState[2];
float newCov[3];
POSVELEKF_POS_CALC_STATE(_cov, posVar, _state, pos, newState)
POSVELEKF_POS_CALC_COV(_cov, posVar, _state, pos, newCov)
memcpy(_state,newState,sizeof(_state));
memcpy(_cov,newCov,sizeof(_cov));
}
void PosVelEKF::fuseVel(float vel, float velVar)
{
float newState[2];
float newCov[3];
POSVELEKF_VEL_CALC_STATE(_cov, velVar, _state, vel, newState)
POSVELEKF_VEL_CALC_COV(_cov, velVar, _state, vel, newCov)
memcpy(_state,newState,sizeof(_state));
memcpy(_cov,newCov,sizeof(_cov));
}
float PosVelEKF::getPosNIS(float pos, float posVar)
{
float ret;
POSVELEKF_POS_CALC_NIS(_cov, posVar, _state, pos, ret)
return ret;
}

19
libraries/AC_PrecLand/PosVelEKF.h Normal file
View File

@ -0,0 +1,19 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
class PosVelEKF {
public:
void init(float pos, float posVar, float vel, float velVar);
void predict(float dt, float dVel, float dVelNoise);
void fusePos(float pos, float posVar);
void fuseVel(float vel, float velVar);
float getPos() const { return _state[0]; }
float getVel() const { return _state[1]; }
float getPosNIS(float pos, float posVar);
private:
float _state[2];
float _cov[3];
};