mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-08 18:10:35 +08:00
Merge remote-tracking branch 'private_swissfang/master' into launchpitchlimit_swissfang
This commit is contained in:
Thomas Gubler
@@ -21,6 +21,12 @@
|
||||
# Simulation setups
|
||||
#
|
||||
|
||||
if param compare SYS_AUTOSTART 901
|
||||
then
|
||||
sh /etc/init.d/901_bottle_drop_test.hil
|
||||
set MODE custom
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 1000
|
||||
then
|
||||
sh /etc/init.d/1000_rc_fw_easystar.hil
|
||||
|
||||
@@ -13,3 +13,5 @@ ekf_att_pos_estimator start
|
||||
#
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
bottle_drop start
|
||||
|
||||
@@ -68,6 +68,11 @@ else
|
||||
fi
|
||||
fi
|
||||
|
||||
# Check for flow sensor
|
||||
if px4flow start
|
||||
then
|
||||
fi
|
||||
|
||||
#
|
||||
# Start the sensor collection task.
|
||||
# IMPORTANT: this also loads param offsets
|
||||
|
||||
@@ -52,21 +52,18 @@ M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
Inputs to the mixer come from channel group 2 (payload), channels 0
|
||||
(bay servo 1), 1 (bay servo 2) and 3 (drop release).
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 4 10000 10000 0 -10000 10000
|
||||
S: 2 0 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
S: 2 1 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
S: 2 2 -10000 -10000 0 -10000 10000
|
||||
|
||||
@@ -67,17 +67,17 @@ MODULES += modules/gpio_led
|
||||
#
|
||||
# Estimation modules (EKF / other filters)
|
||||
#
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
#MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/ekf_att_pos_estimator
|
||||
MODULES += modules/position_estimator_inav
|
||||
#MODULES += modules/position_estimator_inav
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
#MODULES += modules/mc_att_control
|
||||
#MODULES += modules/mc_pos_control
|
||||
|
||||
#
|
||||
# Logging
|
||||
|
||||
@@ -128,6 +128,11 @@ MODULES += lib/geo_lookup
|
||||
MODULES += lib/conversion
|
||||
MODULES += lib/launchdetection
|
||||
|
||||
#
|
||||
# OBC challenge
|
||||
#
|
||||
MODULES += modules/bottle_drop
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
#
|
||||
|
||||
@@ -392,7 +392,8 @@ HIL::task_main()
|
||||
if (fds[0].revents & POLLIN) {
|
||||
|
||||
/* get controls - must always do this to avoid spinning */
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
|
||||
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
|
||||
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
|
||||
|
||||
/* can we mix? */
|
||||
if (_mixers != nullptr) {
|
||||
|
||||
@@ -236,9 +236,8 @@ void TECS::_update_height_demand(float demand, float state)
|
||||
// // _hgt_rate_dem);
|
||||
|
||||
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
|
||||
_hgt_dem_adj_last = _hgt_dem_adj;
|
||||
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
|
||||
// Limit height rate of change
|
||||
if (_hgt_rate_dem > _maxClimbRate) {
|
||||
_hgt_rate_dem = _maxClimbRate;
|
||||
|
||||
@@ -47,6 +47,7 @@ public:
|
||||
_rollComp(0.0f),
|
||||
_spdWeight(0.5f),
|
||||
_heightrate_p(0.0f),
|
||||
_heightrate_ff(0.0f),
|
||||
_speedrate_p(0.0f),
|
||||
_throttle_dem(0.0f),
|
||||
_pitch_dem(0.0f),
|
||||
@@ -220,6 +221,10 @@ public:
|
||||
_heightrate_p = heightrate_p;
|
||||
}
|
||||
|
||||
void set_heightrate_ff(float heightrate_ff) {
|
||||
_heightrate_ff = heightrate_ff;
|
||||
}
|
||||
|
||||
void set_speedrate_p(float speedrate_p) {
|
||||
_speedrate_p = speedrate_p;
|
||||
}
|
||||
@@ -256,6 +261,7 @@ private:
|
||||
float _rollComp;
|
||||
float _spdWeight;
|
||||
float _heightrate_p;
|
||||
float _heightrate_ff;
|
||||
float _speedrate_p;
|
||||
|
||||
// throttle demand in the range from 0.0 to 1.0
|
||||
|
||||
@@ -0,0 +1,899 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 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 bottle_drop.cpp
|
||||
*
|
||||
* Bottle drop module for Outback Challenge 2014, Team Swiss Fang
|
||||
*
|
||||
* @author Dominik Juchli <juchlid@ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <time.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/wind_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
#include <dataman/dataman.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
|
||||
/**
|
||||
* bottle_drop app start / stop handling function
|
||||
*
|
||||
* @ingroup apps
|
||||
*/
|
||||
extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]);
|
||||
|
||||
class BottleDrop
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
BottleDrop();
|
||||
|
||||
/**
|
||||
* Destructor, also kills task.
|
||||
*/
|
||||
~BottleDrop();
|
||||
|
||||
/**
|
||||
* Start the task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
int start();
|
||||
|
||||
/**
|
||||
* Display status.
|
||||
*/
|
||||
void status();
|
||||
|
||||
void open_bay();
|
||||
void close_bay();
|
||||
void drop();
|
||||
void lock_release();
|
||||
|
||||
private:
|
||||
bool _task_should_exit; /**< if true, task should exit */
|
||||
int _main_task; /**< handle for task */
|
||||
int _mavlink_fd;
|
||||
|
||||
int _command_sub;
|
||||
int _wind_estimate_sub;
|
||||
struct vehicle_command_s _command;
|
||||
struct vehicle_global_position_s _global_pos;
|
||||
map_projection_reference_s ref;
|
||||
|
||||
orb_advert_t _actuator_pub;
|
||||
struct actuator_controls_s _actuators;
|
||||
|
||||
bool _drop_approval;
|
||||
hrt_abstime _doors_opened;
|
||||
hrt_abstime _drop_time;
|
||||
|
||||
float _alt_clearance;
|
||||
|
||||
struct position_s {
|
||||
double lat; ///< degrees
|
||||
double lon; ///< degrees
|
||||
float alt; ///< m
|
||||
} _target_position, _drop_position;
|
||||
|
||||
enum DROP_STATE {
|
||||
DROP_STATE_INIT = 0,
|
||||
DROP_STATE_TARGET_VALID,
|
||||
DROP_STATE_TARGET_SET,
|
||||
DROP_STATE_BAY_OPEN,
|
||||
DROP_STATE_DROPPED,
|
||||
DROP_STATE_BAY_CLOSED
|
||||
} _drop_state;
|
||||
|
||||
struct mission_s _onboard_mission;
|
||||
orb_advert_t _onboard_mission_pub;
|
||||
|
||||
void task_main();
|
||||
|
||||
void handle_command(struct vehicle_command_s *cmd);
|
||||
|
||||
void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result);
|
||||
|
||||
/**
|
||||
* Set the actuators
|
||||
*/
|
||||
int actuators_publish();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
static void task_main_trampoline(int argc, char *argv[]);
|
||||
};
|
||||
|
||||
namespace bottle_drop
|
||||
{
|
||||
BottleDrop *g_bottle_drop;
|
||||
}
|
||||
|
||||
BottleDrop::BottleDrop() :
|
||||
|
||||
_task_should_exit(false),
|
||||
_main_task(-1),
|
||||
_mavlink_fd(-1),
|
||||
_command_sub(-1),
|
||||
_wind_estimate_sub(-1),
|
||||
_command {},
|
||||
_global_pos {},
|
||||
ref {},
|
||||
_actuator_pub(-1),
|
||||
_actuators {},
|
||||
_drop_approval(false),
|
||||
_doors_opened(0),
|
||||
_drop_time(0),
|
||||
_alt_clearance(70.0f),
|
||||
_target_position {},
|
||||
_drop_position {},
|
||||
_drop_state(DROP_STATE_INIT),
|
||||
_onboard_mission {},
|
||||
_onboard_mission_pub(-1)
|
||||
{
|
||||
}
|
||||
|
||||
BottleDrop::~BottleDrop()
|
||||
{
|
||||
if (_main_task != -1) {
|
||||
|
||||
/* task wakes up every 100ms or so at the longest */
|
||||
_task_should_exit = true;
|
||||
|
||||
/* wait for a second for the task to quit at our request */
|
||||
unsigned i = 0;
|
||||
|
||||
do {
|
||||
/* wait 20ms */
|
||||
usleep(20000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (++i > 50) {
|
||||
task_delete(_main_task);
|
||||
break;
|
||||
}
|
||||
} while (_main_task != -1);
|
||||
}
|
||||
|
||||
bottle_drop::g_bottle_drop = nullptr;
|
||||
}
|
||||
|
||||
int
|
||||
BottleDrop::start()
|
||||
{
|
||||
ASSERT(_main_task == -1);
|
||||
|
||||
/* start the task */
|
||||
_main_task = task_spawn_cmd("bottle_drop",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
2048,
|
||||
(main_t)&BottleDrop::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_main_task < 0) {
|
||||
warn("task start failed");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
BottleDrop::status()
|
||||
{
|
||||
warnx("drop state: %d", _drop_state);
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::open_bay()
|
||||
{
|
||||
_actuators.control[0] = -1.0f;
|
||||
_actuators.control[1] = 1.0f;
|
||||
|
||||
if (_doors_opened == 0) {
|
||||
_doors_opened = hrt_absolute_time();
|
||||
}
|
||||
warnx("open doors");
|
||||
|
||||
actuators_publish();
|
||||
|
||||
usleep(500 * 1000);
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::close_bay()
|
||||
{
|
||||
// closed door and locked survival kit
|
||||
_actuators.control[0] = 1.0f;
|
||||
_actuators.control[1] = -1.0f;
|
||||
|
||||
_doors_opened = 0;
|
||||
|
||||
actuators_publish();
|
||||
|
||||
// delay until the bay is closed
|
||||
usleep(500 * 1000);
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::drop()
|
||||
{
|
||||
|
||||
// update drop actuator, wait 0.5s until the doors are open before dropping
|
||||
hrt_abstime starttime = hrt_absolute_time();
|
||||
|
||||
// force the door open if we have to
|
||||
if (_doors_opened == 0) {
|
||||
open_bay();
|
||||
warnx("bay not ready, forced open");
|
||||
}
|
||||
|
||||
while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
|
||||
usleep(50000);
|
||||
warnx("delayed by door!");
|
||||
}
|
||||
|
||||
_actuators.control[2] = 1.0f;
|
||||
|
||||
_drop_time = hrt_absolute_time();
|
||||
actuators_publish();
|
||||
|
||||
warnx("dropping now");
|
||||
|
||||
// Give it time to drop
|
||||
usleep(1000 * 1000);
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::lock_release()
|
||||
{
|
||||
_actuators.control[2] = -1.0f;
|
||||
actuators_publish();
|
||||
|
||||
warnx("closing release");
|
||||
}
|
||||
|
||||
int
|
||||
BottleDrop::actuators_publish()
|
||||
{
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
|
||||
// lazily publish _actuators only once available
|
||||
if (_actuator_pub > 0) {
|
||||
return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
|
||||
|
||||
} else {
|
||||
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
|
||||
if (_actuator_pub > 0) {
|
||||
return OK;
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::task_main()
|
||||
{
|
||||
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
|
||||
|
||||
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
|
||||
|
||||
bool updated = false;
|
||||
|
||||
float z_0; // ground properties
|
||||
float turn_radius; // turn radius of the UAV
|
||||
float precision; // Expected precision of the UAV
|
||||
|
||||
float ground_distance = _alt_clearance; // Replace by closer estimate in loop
|
||||
|
||||
// constant
|
||||
float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2]
|
||||
float m = 0.5f; // mass of bottle [kg]
|
||||
float rho = 1.2f; // air density [kg/m^3]
|
||||
float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
|
||||
float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s]
|
||||
|
||||
// Has to be estimated by experiment
|
||||
float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 []
|
||||
float t_signal =
|
||||
0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
|
||||
float t_door =
|
||||
0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
|
||||
|
||||
|
||||
// Definition
|
||||
float h_0; // height over target
|
||||
float az; // acceleration in z direction[m/s^2]
|
||||
float vz; // velocity in z direction [m/s]
|
||||
float z; // fallen distance [m]
|
||||
float h; // height over target [m]
|
||||
float ax; // acceleration in x direction [m/s^2]
|
||||
float vx; // ground speed in x direction [m/s]
|
||||
float x; // traveled distance in x direction [m]
|
||||
float vw; // wind speed [m/s]
|
||||
float vrx; // relative velocity in x direction [m/s]
|
||||
float v; // relative speed vector [m/s]
|
||||
float Fd; // Drag force [N]
|
||||
float Fdx; // Drag force in x direction [N]
|
||||
float Fdz; // Drag force in z direction [N]
|
||||
float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED)
|
||||
float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
|
||||
float x_l, y_l; // local position in projected coordinates
|
||||
float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates
|
||||
double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED
|
||||
float distance_open_door; // The distance the UAV travels during its doors open [m]
|
||||
float distance_real = 0; // The distance between the UAVs position and the drop point [m]
|
||||
float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m]
|
||||
|
||||
unsigned counter = 0;
|
||||
|
||||
param_t param_gproperties = param_find("BD_GPROPERTIES");
|
||||
param_t param_turn_radius = param_find("BD_TURNRADIUS");
|
||||
param_t param_precision = param_find("BD_PRECISION");
|
||||
param_t param_cd = param_find("BD_OBJ_CD");
|
||||
param_t param_mass = param_find("BD_OBJ_MASS");
|
||||
param_t param_surface = param_find("BD_OBJ_SURFACE");
|
||||
|
||||
|
||||
param_get(param_precision, &precision);
|
||||
param_get(param_turn_radius, &turn_radius);
|
||||
param_get(param_gproperties, &z_0);
|
||||
param_get(param_cd, &cd);
|
||||
param_get(param_mass, &m);
|
||||
param_get(param_surface, &A);
|
||||
|
||||
int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
|
||||
struct parameter_update_s update;
|
||||
memset(&update, 0, sizeof(update));
|
||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
struct mission_item_s flight_vector_s {};
|
||||
struct mission_item_s flight_vector_e {};
|
||||
|
||||
flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
flight_vector_s.acceptance_radius = 50; // TODO: make parameter
|
||||
flight_vector_s.autocontinue = true;
|
||||
flight_vector_s.altitude_is_relative = false;
|
||||
|
||||
flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
flight_vector_e.acceptance_radius = 50; // TODO: make parameter
|
||||
flight_vector_e.autocontinue = true;
|
||||
flight_vector_s.altitude_is_relative = false;
|
||||
|
||||
struct wind_estimate_s wind;
|
||||
|
||||
// wakeup source(s)
|
||||
struct pollfd fds[1];
|
||||
|
||||
// Setup of loop
|
||||
fds[0].fd = _command_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
// Whatever state the bay is in, we want it closed on startup
|
||||
lock_release();
|
||||
close_bay();
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
/* wait for up to 100ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
|
||||
|
||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||
if (pret < 0) {
|
||||
warn("poll error %d, %d", pret, errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* vehicle commands updated */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
|
||||
handle_command(&_command);
|
||||
}
|
||||
|
||||
orb_check(vehicle_global_position_sub, &updated);
|
||||
if (updated) {
|
||||
/* copy global position */
|
||||
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
|
||||
}
|
||||
|
||||
if (_global_pos.timestamp == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const unsigned sleeptime_us = 50000;
|
||||
|
||||
hrt_abstime last_run = hrt_absolute_time();
|
||||
float dt_runs = 1e6f / sleeptime_us;
|
||||
|
||||
// switch to faster updates during the drop
|
||||
while (_drop_state > DROP_STATE_INIT) {
|
||||
|
||||
// Get wind estimate
|
||||
orb_check(_wind_estimate_sub, &updated);
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
|
||||
}
|
||||
|
||||
// Get vehicle position
|
||||
orb_check(vehicle_global_position_sub, &updated);
|
||||
if (updated) {
|
||||
// copy global position
|
||||
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
|
||||
}
|
||||
|
||||
// Get parameter updates
|
||||
orb_check(parameter_update_sub, &updated);
|
||||
if (updated) {
|
||||
// copy global position
|
||||
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
|
||||
|
||||
// update all parameters
|
||||
param_get(param_gproperties, &z_0);
|
||||
param_get(param_turn_radius, &turn_radius);
|
||||
param_get(param_precision, &precision);
|
||||
}
|
||||
|
||||
orb_check(_command_sub, &updated);
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
|
||||
handle_command(&_command);
|
||||
}
|
||||
|
||||
|
||||
float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
|
||||
float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
|
||||
distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
|
||||
ground_distance = _global_pos.alt - _target_position.alt;
|
||||
|
||||
if (counter % 90 == 0) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real);
|
||||
}
|
||||
|
||||
switch (_drop_state) {
|
||||
|
||||
case DROP_STATE_TARGET_VALID:
|
||||
{
|
||||
|
||||
// Update drop point at 10 Hz
|
||||
if (counter % 10 == 0) {
|
||||
|
||||
az = g; // acceleration in z direction[m/s^2]
|
||||
vz = 0; // velocity in z direction [m/s]
|
||||
z = 0; // fallen distance [m]
|
||||
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
|
||||
h = h_0; // height over target [m]
|
||||
ax = 0; // acceleration in x direction [m/s^2]
|
||||
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
|
||||
x = 0; // traveled distance in x direction [m]
|
||||
vw = 0; // wind speed [m/s]
|
||||
vrx = 0; // relative velocity in x direction [m/s]
|
||||
v = groundspeed_body; // relative speed vector [m/s]
|
||||
Fd = 0; // Drag force [N]
|
||||
Fdx = 0; // Drag force in x direction [N]
|
||||
Fdz = 0; // Drag force in z direction [N]
|
||||
|
||||
|
||||
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
|
||||
while (h > 0.05f) {
|
||||
// z-direction
|
||||
vz = vz + az * dt_freefall_prediction;
|
||||
z = z + vz * dt_freefall_prediction;
|
||||
h = h_0 - z;
|
||||
|
||||
// x-direction
|
||||
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
|
||||
vx = vx + ax * dt_freefall_prediction;
|
||||
x = x + vx * dt_freefall_prediction;
|
||||
vrx = vx + vw;
|
||||
|
||||
// drag force
|
||||
v = sqrtf(vz * vz + vrx * vrx);
|
||||
Fd = 0.5f * rho * A * cd * (v * v);
|
||||
Fdx = Fd * vrx / v;
|
||||
Fdz = Fd * vz / v;
|
||||
|
||||
// acceleration
|
||||
az = g - Fdz / m;
|
||||
ax = -Fdx / m;
|
||||
}
|
||||
|
||||
// compute drop vector
|
||||
x = groundspeed_body * t_signal + x;
|
||||
}
|
||||
|
||||
x_t = 0.0f;
|
||||
y_t = 0.0f;
|
||||
|
||||
float wind_direction_n, wind_direction_e;
|
||||
|
||||
if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen
|
||||
wind_direction_n = 1.0f;
|
||||
wind_direction_e = 0.0f;
|
||||
|
||||
} else {
|
||||
wind_direction_n = wind.windspeed_north / windspeed_norm;
|
||||
wind_direction_e = wind.windspeed_east / windspeed_norm;
|
||||
}
|
||||
|
||||
x_drop = x_t + x * wind_direction_n;
|
||||
y_drop = y_t + x * wind_direction_e;
|
||||
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
|
||||
_drop_position.alt = _target_position.alt + _alt_clearance;
|
||||
|
||||
// Compute flight vector
|
||||
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
|
||||
&(flight_vector_s.lat), &(flight_vector_s.lon));
|
||||
flight_vector_s.altitude = _drop_position.alt;
|
||||
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
|
||||
&flight_vector_e.lat, &flight_vector_e.lon);
|
||||
flight_vector_e.altitude = _drop_position.alt;
|
||||
|
||||
// Save WPs in datamanager
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
|
||||
warnx("ERROR: could not save onboard WP");
|
||||
}
|
||||
|
||||
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
|
||||
warnx("ERROR: could not save onboard WP");
|
||||
}
|
||||
|
||||
_onboard_mission.count = 2;
|
||||
_onboard_mission.current_seq = 0;
|
||||
|
||||
if (_onboard_mission_pub > 0) {
|
||||
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
|
||||
|
||||
} else {
|
||||
_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
|
||||
}
|
||||
|
||||
_drop_state = DROP_STATE_TARGET_SET;
|
||||
}
|
||||
break;
|
||||
|
||||
case DROP_STATE_TARGET_SET:
|
||||
{
|
||||
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
|
||||
|
||||
if (distance_wp2 < distance_real) {
|
||||
_onboard_mission.current_seq = 0;
|
||||
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
|
||||
} else {
|
||||
|
||||
// We're close enough - open the bay
|
||||
distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body));
|
||||
|
||||
if (isfinite(distance_real) && distance_real < distance_open_door) {
|
||||
open_bay();
|
||||
_drop_state = DROP_STATE_BAY_OPEN;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DROP_STATE_BAY_OPEN:
|
||||
{
|
||||
if (_drop_approval) {
|
||||
map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
|
||||
x_f = x_l + _global_pos.vel_n * dt_runs;
|
||||
y_f = y_l + _global_pos.vel_e * dt_runs;
|
||||
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
|
||||
future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
|
||||
|
||||
warnx("Distance real: %.2f", (double)distance_real);
|
||||
|
||||
if (isfinite(distance_real) &&
|
||||
(distance_real < precision) && ((distance_real < future_distance) ||
|
||||
(distance_real < precision / 10.0f))) {
|
||||
drop();
|
||||
_drop_state = DROP_STATE_DROPPED;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
|
||||
} else {
|
||||
|
||||
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
|
||||
|
||||
if (distance_wp2 < distance_real) {
|
||||
_onboard_mission.current_seq = 0;
|
||||
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DROP_STATE_DROPPED:
|
||||
/* 2s after drop, reset and close everything again */
|
||||
if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
|
||||
_drop_state = DROP_STATE_INIT;
|
||||
_drop_approval = false;
|
||||
lock_release();
|
||||
close_bay();
|
||||
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
|
||||
|
||||
// remove onboard mission
|
||||
_onboard_mission.current_seq = -1;
|
||||
_onboard_mission.count = 0;
|
||||
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
counter++;
|
||||
|
||||
// update_actuators();
|
||||
|
||||
// run at roughly 20 Hz
|
||||
usleep(sleeptime_us);
|
||||
|
||||
dt_runs = 1e6f / hrt_elapsed_time(&last_run);
|
||||
last_run = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
warnx("exiting.");
|
||||
|
||||
_main_task = -1;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::handle_command(struct vehicle_command_s *cmd)
|
||||
{
|
||||
switch (cmd->command) {
|
||||
case VEHICLE_CMD_CUSTOM_0:
|
||||
/*
|
||||
* param1 and param2 set to 1: open and drop
|
||||
* param1 set to 1: open
|
||||
* else: close (and don't drop)
|
||||
*/
|
||||
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
|
||||
open_bay();
|
||||
drop();
|
||||
mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
|
||||
|
||||
} else if (cmd->param1 > 0.5f) {
|
||||
open_bay();
|
||||
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
|
||||
|
||||
} else {
|
||||
lock_release();
|
||||
close_bay();
|
||||
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
|
||||
}
|
||||
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
|
||||
|
||||
switch ((int)(cmd->param1 + 0.5f)) {
|
||||
case 0:
|
||||
_drop_approval = false;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
|
||||
break;
|
||||
|
||||
case 1:
|
||||
_drop_approval = true;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
|
||||
break;
|
||||
|
||||
default:
|
||||
_drop_approval = false;
|
||||
warnx("param1 val unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
// XXX check all fields (2-3)
|
||||
_alt_clearance = cmd->param4;
|
||||
_target_position.lat = cmd->param5;
|
||||
_target_position.lon = cmd->param6;
|
||||
_target_position.alt = cmd->param7;
|
||||
_drop_state = DROP_STATE_TARGET_VALID;
|
||||
mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
|
||||
(double)_target_position.lon, (double)_target_position.alt);
|
||||
map_projection_init(&ref, _target_position.lat, _target_position.lon);
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
|
||||
|
||||
if (cmd->param1 < 0) {
|
||||
|
||||
// Clear internal states
|
||||
_drop_approval = false;
|
||||
_drop_state = DROP_STATE_INIT;
|
||||
|
||||
// Abort if mission is present
|
||||
_onboard_mission.current_seq = -1;
|
||||
|
||||
if (_onboard_mission_pub > 0) {
|
||||
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
|
||||
}
|
||||
|
||||
} else {
|
||||
switch ((int)(cmd->param1 + 0.5f)) {
|
||||
case 0:
|
||||
_drop_approval = false;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
_drop_approval = true;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
|
||||
break;
|
||||
|
||||
default:
|
||||
_drop_approval = false;
|
||||
break;
|
||||
// XXX handle other values
|
||||
}
|
||||
}
|
||||
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result)
|
||||
{
|
||||
switch (result) {
|
||||
case VEHICLE_CMD_RESULT_ACCEPTED:
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_DENIED:
|
||||
mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_FAILED:
|
||||
mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
|
||||
mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_UNSUPPORTED:
|
||||
mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
BottleDrop::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
bottle_drop::g_bottle_drop->task_main();
|
||||
}
|
||||
|
||||
static void usage()
|
||||
{
|
||||
errx(1, "usage: bottle_drop {start|stop|status}");
|
||||
}
|
||||
|
||||
int bottle_drop_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
usage();
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (bottle_drop::g_bottle_drop != nullptr) {
|
||||
errx(1, "already running");
|
||||
}
|
||||
|
||||
bottle_drop::g_bottle_drop = new BottleDrop;
|
||||
|
||||
if (bottle_drop::g_bottle_drop == nullptr) {
|
||||
errx(1, "alloc failed");
|
||||
}
|
||||
|
||||
if (OK != bottle_drop::g_bottle_drop->start()) {
|
||||
delete bottle_drop::g_bottle_drop;
|
||||
bottle_drop::g_bottle_drop = nullptr;
|
||||
err(1, "start failed");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (bottle_drop::g_bottle_drop == nullptr) {
|
||||
errx(1, "not running");
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
delete bottle_drop::g_bottle_drop;
|
||||
bottle_drop::g_bottle_drop = nullptr;
|
||||
|
||||
} else if (!strcmp(argv[1], "status")) {
|
||||
bottle_drop::g_bottle_drop->status();
|
||||
|
||||
} else if (!strcmp(argv[1], "drop")) {
|
||||
bottle_drop::g_bottle_drop->drop();
|
||||
|
||||
} else if (!strcmp(argv[1], "open")) {
|
||||
bottle_drop::g_bottle_drop->open_bay();
|
||||
|
||||
} else if (!strcmp(argv[1], "close")) {
|
||||
bottle_drop::g_bottle_drop->close_bay();
|
||||
|
||||
} else if (!strcmp(argv[1], "lock")) {
|
||||
bottle_drop::g_bottle_drop->lock_release();
|
||||
|
||||
} else {
|
||||
usage();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 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 bottle_drop_params.c
|
||||
* Bottle drop parameters
|
||||
*
|
||||
* @author Dominik Juchli <juchlid@ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Ground drag property
|
||||
*
|
||||
* This parameter encodes the ground drag coefficient and the corresponding
|
||||
* decrease in wind speed from the plane altitude to ground altitude.
|
||||
*
|
||||
* @unit unknown
|
||||
* @min 0.001
|
||||
* @max 0.1
|
||||
* @group Payload drop
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f);
|
||||
|
||||
/**
|
||||
* Plane turn radius
|
||||
*
|
||||
* The planes known minimal turn radius - use a higher value
|
||||
* to make the plane maneuver more distant from the actual drop
|
||||
* position. This is to ensure the wings are level during the drop.
|
||||
*
|
||||
* @unit meter
|
||||
* @min 30.0
|
||||
* @max 500.0
|
||||
* @group Payload drop
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f);
|
||||
|
||||
/**
|
||||
* Drop precision
|
||||
*
|
||||
* If the system is closer than this distance on passing over the
|
||||
* drop position, it will release the payload. This is a safeguard
|
||||
* to prevent a drop out of the required accuracy.
|
||||
*
|
||||
* @unit meter
|
||||
* @min 1.0
|
||||
* @max 80.0
|
||||
* @group Payload drop
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f);
|
||||
|
||||
/**
|
||||
* Payload drag coefficient of the dropped object
|
||||
*
|
||||
* The drag coefficient (cd) is the typical drag
|
||||
* constant for air. It is in general object specific,
|
||||
* but the closest primitive shape to the actual object
|
||||
* should give good results:
|
||||
* http://en.wikipedia.org/wiki/Drag_coefficient
|
||||
*
|
||||
* @unit meter
|
||||
* @min 1.0
|
||||
* @max 80.0
|
||||
* @group Payload drop
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f);
|
||||
|
||||
/**
|
||||
* Payload mass
|
||||
*
|
||||
* A typical small toy ball:
|
||||
* 0.025 kg
|
||||
*
|
||||
* OBC water bottle:
|
||||
* 0.6 kg
|
||||
*
|
||||
* @unit kilogram
|
||||
* @min 0.001
|
||||
* @max 5.0
|
||||
* @group Payload drop
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f);
|
||||
|
||||
/**
|
||||
* Payload front surface area
|
||||
*
|
||||
* A typical small toy ball:
|
||||
* (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2
|
||||
*
|
||||
* OBC water bottle:
|
||||
* (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2
|
||||
*
|
||||
* @unit m^2
|
||||
* @min 0.001
|
||||
* @max 0.5
|
||||
* @group Payload drop
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f);
|
||||
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013, 2014 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Daemon application
|
||||
#
|
||||
|
||||
MODULE_COMMAND = bottle_drop
|
||||
|
||||
SRCS = bottle_drop.cpp \
|
||||
bottle_drop_params.c
|
||||
@@ -58,6 +58,7 @@
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#ifdef SENSOR_COMBINED_SUB
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#endif
|
||||
@@ -96,7 +97,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
|
||||
|
||||
__EXPORT uint32_t millis();
|
||||
|
||||
__EXPORT uint64_t getMicros();
|
||||
|
||||
static uint64_t IMUmsec = 0;
|
||||
static uint64_t IMUusec = 0;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
|
||||
|
||||
uint32_t millis()
|
||||
@@ -104,6 +108,11 @@ uint32_t millis()
|
||||
return IMUmsec;
|
||||
}
|
||||
|
||||
uint64_t getMicros()
|
||||
{
|
||||
return IMUusec;
|
||||
}
|
||||
|
||||
class FixedwingEstimator
|
||||
{
|
||||
public:
|
||||
@@ -171,6 +180,7 @@ private:
|
||||
#else
|
||||
int _sensor_combined_sub;
|
||||
#endif
|
||||
int _distance_sub; /**< distance measurement */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
int _baro_sub; /**< barometer subscription */
|
||||
int _gps_sub; /**< GPS subscription */
|
||||
@@ -196,7 +206,8 @@ private:
|
||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
|
||||
struct vehicle_gps_position_s _gps; /**< GPS position */
|
||||
struct wind_estimate_s _wind; /**< Wind estimate */
|
||||
struct wind_estimate_s _wind; /**< wind estimate */
|
||||
struct range_finder_report _distance; /**< distance estimate */
|
||||
|
||||
struct gyro_scale _gyro_offsets;
|
||||
struct accel_scale _accel_offsets;
|
||||
@@ -226,6 +237,7 @@ private:
|
||||
hrt_abstime _filter_start_time;
|
||||
hrt_abstime _last_sensor_timestamp;
|
||||
hrt_abstime _last_run;
|
||||
hrt_abstime _distance_last_valid;
|
||||
bool _gyro_valid;
|
||||
bool _accel_valid;
|
||||
bool _mag_valid;
|
||||
@@ -342,6 +354,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
#else
|
||||
_sensor_combined_sub(-1),
|
||||
#endif
|
||||
_distance_sub(-1),
|
||||
_airspeed_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_gps_sub(-1),
|
||||
@@ -399,6 +412,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
@@ -549,6 +563,7 @@ FixedwingEstimator::parameters_update()
|
||||
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
|
||||
_ekf->accelProcessNoise = _parameters.acc_pnoise;
|
||||
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
|
||||
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
|
||||
}
|
||||
|
||||
return OK;
|
||||
@@ -704,6 +719,7 @@ FixedwingEstimator::task_main()
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
|
||||
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
@@ -753,6 +769,7 @@ FixedwingEstimator::task_main()
|
||||
bool newHgtData = false;
|
||||
bool newAdsData = false;
|
||||
bool newDataMag = false;
|
||||
bool newRangeData = false;
|
||||
|
||||
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
|
||||
|
||||
@@ -850,7 +867,8 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
_last_sensor_timestamp = _gyro.timestamp;
|
||||
IMUmsec = _gyro.timestamp / 1e3f;
|
||||
IMUmsec = _gyro.timestamp / 1e3;
|
||||
IMUusec = _gyro.timestamp;
|
||||
|
||||
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
|
||||
_last_run = _gyro.timestamp;
|
||||
@@ -914,7 +932,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
// Copy gyro and accel
|
||||
_last_sensor_timestamp = _sensor_combined.timestamp;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3f;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3;
|
||||
IMUusec = _sensor_combined.timestamp;
|
||||
|
||||
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
|
||||
|
||||
@@ -994,8 +1013,6 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (gps_updated) {
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
|
||||
perf_count(_perf_gps);
|
||||
|
||||
@@ -1008,11 +1025,17 @@ FixedwingEstimator::task_main()
|
||||
_gps_start_time = hrt_absolute_time();
|
||||
|
||||
/* check if we had a GPS outage for a long time */
|
||||
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
|
||||
float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
|
||||
|
||||
const float pos_reset_threshold = 5.0f; // seconds
|
||||
|
||||
/* timeout of 5 seconds */
|
||||
if (gps_elapsed > pos_reset_threshold) {
|
||||
_ekf->ResetPosition();
|
||||
_ekf->ResetVelocity();
|
||||
_ekf->ResetStoredStates();
|
||||
}
|
||||
_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
|
||||
|
||||
/* fuse GPS updates */
|
||||
|
||||
@@ -1044,6 +1067,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
newDataGps = true;
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1052,8 +1077,15 @@ FixedwingEstimator::task_main()
|
||||
orb_check(_baro_sub, &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
hrt_abstime baro_last = _baro.timestamp;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
|
||||
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
|
||||
|
||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
|
||||
if (!_baro_init) {
|
||||
@@ -1114,6 +1146,19 @@ FixedwingEstimator::task_main()
|
||||
newDataMag = false;
|
||||
}
|
||||
|
||||
orb_check(_distance_sub, &newRangeData);
|
||||
|
||||
if (newRangeData) {
|
||||
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
|
||||
|
||||
if (_distance.valid) {
|
||||
_ekf->rngMea = _distance.distance;
|
||||
_distance_last_valid = _distance.timestamp;
|
||||
} else {
|
||||
newRangeData = false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
|
||||
*/
|
||||
@@ -1197,6 +1242,7 @@ FixedwingEstimator::task_main()
|
||||
} else if (_ekf->statesInitialised) {
|
||||
|
||||
// We're apparently initialized in this case now
|
||||
// check (and reset the filter as needed)
|
||||
int check = check_filter_state();
|
||||
|
||||
if (check) {
|
||||
@@ -1206,21 +1252,7 @@ FixedwingEstimator::task_main()
|
||||
|
||||
// Run the strapdown INS equations every IMU update
|
||||
_ekf->UpdateStrapdownEquationsNED();
|
||||
#if 0
|
||||
// debug code - could be tunred into a filter mnitoring/watchdog function
|
||||
float tempQuat[4];
|
||||
|
||||
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
|
||||
|
||||
quat2eul(eulerEst, tempQuat);
|
||||
|
||||
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
|
||||
|
||||
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
|
||||
|
||||
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
|
||||
|
||||
#endif
|
||||
// store the predicted states for subsequent use by measurement fusion
|
||||
_ekf->StoreStates(IMUmsec);
|
||||
// Check if on ground - status is used by covariance prediction
|
||||
@@ -1334,6 +1366,13 @@ FixedwingEstimator::task_main()
|
||||
_ekf->fuseVtasData = false;
|
||||
}
|
||||
|
||||
if (newRangeData) {
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->useRangeFinder = true;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
|
||||
_ekf->GroundEKF();
|
||||
}
|
||||
|
||||
|
||||
// Output results
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
@@ -1447,6 +1486,10 @@ FixedwingEstimator::task_main()
|
||||
_global_pos.vel_d = _local_pos.vz;
|
||||
}
|
||||
|
||||
/* terrain altitude */
|
||||
_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
|
||||
_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
|
||||
(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
|
||||
|
||||
_global_pos.yaw = _local_pos.yaw;
|
||||
|
||||
@@ -1467,8 +1510,10 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
_wind.windspeed_north = _ekf->windSpdFiltNorth;
|
||||
_wind.windspeed_east = _ekf->windSpdFiltEast;
|
||||
// XXX we need to do something smart about the covariance here
|
||||
// but we default to the estimate covariance for now
|
||||
_wind.covariance_north = _ekf->P[14][14];
|
||||
_wind.covariance_east = _ekf->P[15][15];
|
||||
|
||||
|
||||
@@ -2,6 +2,11 @@
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifndef M_PI_F
|
||||
#define M_PI_F ((float)M_PI)
|
||||
#endif
|
||||
|
||||
#define EKF_COVARIANCE_DIVERGED 1.0e8f
|
||||
|
||||
@@ -38,13 +43,14 @@ AttPosEKF::AttPosEKF() :
|
||||
resetStates{},
|
||||
storedStates{},
|
||||
statetimeStamp{},
|
||||
lastVelPosFusion(millis()),
|
||||
statesAtVelTime{},
|
||||
statesAtPosTime{},
|
||||
statesAtHgtTime{},
|
||||
statesAtMagMeasTime{},
|
||||
statesAtVtasMeasTime{},
|
||||
statesAtRngTime{},
|
||||
statesAtOptFlowTime{},
|
||||
statesAtFlowTime{},
|
||||
correctedDelAng(),
|
||||
correctedDelVel(),
|
||||
summedDelAng(),
|
||||
@@ -59,7 +65,16 @@ AttPosEKF::AttPosEKF() :
|
||||
accel(),
|
||||
dVelIMU(),
|
||||
dAngIMU(),
|
||||
dtIMU(0),
|
||||
dtIMU(0.005f),
|
||||
dtIMUfilt(0.005f),
|
||||
dtVelPos(0.01f),
|
||||
dtVelPosFilt(0.01f),
|
||||
dtHgtFilt(0.01f),
|
||||
dtGpsFilt(0.1f),
|
||||
windSpdFiltNorth(0.0f),
|
||||
windSpdFiltEast(0.0f),
|
||||
windSpdFiltAltitude(0.0f),
|
||||
windSpdFiltClimb(0.0f),
|
||||
fusionModeGPS(0),
|
||||
innovVelPos{},
|
||||
varInnovVelPos{},
|
||||
@@ -103,13 +118,13 @@ AttPosEKF::AttPosEKF() :
|
||||
|
||||
inhibitWindStates(true),
|
||||
inhibitMagStates(true),
|
||||
inhibitGndHgtState(true),
|
||||
inhibitGndState(true),
|
||||
|
||||
onGround(true),
|
||||
staticMode(true),
|
||||
useAirspeed(true),
|
||||
useCompass(true),
|
||||
useRangeFinder(false),
|
||||
useRangeFinder(true),
|
||||
useOpticalFlow(false),
|
||||
|
||||
ekfDiverged(false),
|
||||
@@ -117,7 +132,24 @@ AttPosEKF::AttPosEKF() :
|
||||
current_ekf_state{},
|
||||
last_ekf_error{},
|
||||
numericalProtection(true),
|
||||
storeIndex(0)
|
||||
storeIndex(0),
|
||||
storedOmega{},
|
||||
Popt{},
|
||||
flowStates{},
|
||||
prevPosN(0.0f),
|
||||
prevPosE(0.0f),
|
||||
auxFlowObsInnov{},
|
||||
auxFlowObsInnovVar{},
|
||||
fScaleFactorVar(0.0f),
|
||||
Tnb_flow{},
|
||||
R_LOS(0.0f),
|
||||
auxFlowTestRatio{},
|
||||
auxRngTestRatio(0.0f),
|
||||
flowInnovGate(0.0f),
|
||||
auxFlowInnovGate(0.0f),
|
||||
rngInnovGate(0.0f),
|
||||
minFlowRng(0.0f),
|
||||
moCompR_LOS(0.0f)
|
||||
{
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
|
||||
@@ -260,6 +292,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
|
||||
|
||||
// Constrain states (to protect against filter divergence)
|
||||
ConstrainStates();
|
||||
|
||||
// update filtered IMU time step length
|
||||
dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
|
||||
}
|
||||
|
||||
void AttPosEKF::CovariancePrediction(float dt)
|
||||
@@ -312,7 +347,7 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
} else {
|
||||
for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
if (!inhibitGndState) {
|
||||
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
|
||||
} else {
|
||||
processNoise[22] = 0;
|
||||
@@ -962,6 +997,21 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtGpsFilt(float dt)
|
||||
{
|
||||
dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtHgtFilt(float dt)
|
||||
{
|
||||
dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtVelPosFilt(float dt)
|
||||
{
|
||||
dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::FuseVelposNED()
|
||||
{
|
||||
|
||||
@@ -998,6 +1048,18 @@ void AttPosEKF::FuseVelposNED()
|
||||
// associated with sequential fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData)
|
||||
{
|
||||
uint64_t tNow = getMicros();
|
||||
updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
|
||||
lastVelPosFusion = tNow;
|
||||
|
||||
// scaler according to the number of repetitions of the
|
||||
// same measurement in one fusion step
|
||||
float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
|
||||
|
||||
// scaler according to the number of repetitions of the
|
||||
// same measurement in one fusion step
|
||||
float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
|
||||
|
||||
// set the GPS data timeout depending on whether airspeed data is present
|
||||
if (useAirspeed) horizRetryTime = gpsRetryTime;
|
||||
else horizRetryTime = gpsRetryTimeNoTAS;
|
||||
@@ -1010,12 +1072,12 @@ void AttPosEKF::FuseVelposNED()
|
||||
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
|
||||
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
|
||||
R_OBS[0] = sq(vneSigma) + sq(velErr);
|
||||
R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
|
||||
R_OBS[1] = R_OBS[0];
|
||||
R_OBS[2] = sq(vdSigma) + sq(velErr);
|
||||
R_OBS[3] = sq(posNeSigma) + sq(posErr);
|
||||
R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
|
||||
R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
|
||||
R_OBS[4] = R_OBS[3];
|
||||
R_OBS[5] = sq(posDSigma) + sq(posErr);
|
||||
R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
|
||||
|
||||
// calculate innovations and check GPS data validity using an innovation consistency check
|
||||
if (fuseVelData)
|
||||
@@ -1173,7 +1235,7 @@ void AttPosEKF::FuseVelposNED()
|
||||
}
|
||||
}
|
||||
// Don't update terrain state if inhibited
|
||||
if (inhibitGndHgtState) {
|
||||
if (inhibitGndState) {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
|
||||
@@ -1356,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
if (!inhibitGndState) {
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
@@ -1430,11 +1492,6 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[1] = 1.0f/SK_MY[0];
|
||||
innovMag[1] = MagPred[1] - magData.y;
|
||||
}
|
||||
@@ -1505,11 +1562,6 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[2] = 1.0f/SK_MZ[0];
|
||||
innovMag[2] = MagPred[2] - magData.z;
|
||||
|
||||
@@ -1616,6 +1668,32 @@ void AttPosEKF::FuseAirspeed()
|
||||
// Perform fusion of True Airspeed measurement
|
||||
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
|
||||
{
|
||||
|
||||
float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
|
||||
|
||||
if (isfinite(windSpdFiltClimb)) {
|
||||
windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
|
||||
} else {
|
||||
windSpdFiltClimb = states[6];
|
||||
}
|
||||
|
||||
if (altDiff < 20.0f) {
|
||||
// Lowpass the output of the wind estimate - we want a long-term
|
||||
// stable estimate, but not start to load into the overall dynamics
|
||||
// of the system (which adjusting covariances would do)
|
||||
|
||||
// Change filter coefficient based on altitude change rate
|
||||
float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
|
||||
|
||||
windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
|
||||
windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
|
||||
} else {
|
||||
windSpdFiltNorth = vwn;
|
||||
windSpdFiltEast = vwe;
|
||||
}
|
||||
|
||||
windSpdFiltAltitude = hgtMea;
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
|
||||
@@ -1675,11 +1753,6 @@ void AttPosEKF::FuseAirspeed()
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovVtas = 1.0f/SK_TAS;
|
||||
|
||||
// Calculate the measurement innovation
|
||||
@@ -1811,8 +1884,11 @@ void AttPosEKF::FuseRangeFinder()
|
||||
rngPred = (ptd - pd)/cosRngTilt;
|
||||
innovRng = rngPred - rngMea;
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((innovRng*innovRng*SK_RNG[0]) < 25)
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (auxRngTestRatio < 1.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
states[22] = states[22] - Kfusion[22] * innovRng;
|
||||
@@ -1827,285 +1903,387 @@ void AttPosEKF::FuseRangeFinder()
|
||||
|
||||
void AttPosEKF::FuseOptFlow()
|
||||
{
|
||||
static uint8_t obsIndex;
|
||||
static float SH_LOS[13];
|
||||
static float SKK_LOS[15];
|
||||
static float SK_LOS[2];
|
||||
static float q0 = 0.0f;
|
||||
static float q1 = 0.0f;
|
||||
static float q2 = 0.0f;
|
||||
static float q3 = 1.0f;
|
||||
static float vn = 0.0f;
|
||||
static float ve = 0.0f;
|
||||
static float vd = 0.0f;
|
||||
static float pd = 0.0f;
|
||||
static float ptd = 0.0f;
|
||||
static float R_LOS = 0.01f;
|
||||
static float losPred[2];
|
||||
// static uint8_t obsIndex;
|
||||
// static float SH_LOS[13];
|
||||
// static float SKK_LOS[15];
|
||||
// static float SK_LOS[2];
|
||||
// static float q0 = 0.0f;
|
||||
// static float q1 = 0.0f;
|
||||
// static float q2 = 0.0f;
|
||||
// static float q3 = 1.0f;
|
||||
// static float vn = 0.0f;
|
||||
// static float ve = 0.0f;
|
||||
// static float vd = 0.0f;
|
||||
// static float pd = 0.0f;
|
||||
// static float ptd = 0.0f;
|
||||
// static float R_LOS = 0.01f;
|
||||
// static float losPred[2];
|
||||
|
||||
// Transformation matrix from nav to body axes
|
||||
Mat3f Tnb_local;
|
||||
// Transformation matrix from body to sensor axes
|
||||
// assume camera is aligned with Z body axis plus a misalignment
|
||||
// defined by 3 small angles about X, Y and Z body axis
|
||||
Mat3f Tbs;
|
||||
Tbs.x.y = a3;
|
||||
Tbs.y.x = -a3;
|
||||
Tbs.x.z = -a2;
|
||||
Tbs.z.x = a2;
|
||||
Tbs.y.z = a1;
|
||||
Tbs.z.y = -a1;
|
||||
// Transformation matrix from navigation to sensor axes
|
||||
Mat3f Tns;
|
||||
float H_LOS[n_states];
|
||||
for (uint8_t i = 0; i < n_states; i++) {
|
||||
H_LOS[i] = 0.0f;
|
||||
// // Transformation matrix from nav to body axes
|
||||
// Mat3f Tnb_local;
|
||||
// // Transformation matrix from body to sensor axes
|
||||
// // assume camera is aligned with Z body axis plus a misalignment
|
||||
// // defined by 3 small angles about X, Y and Z body axis
|
||||
// Mat3f Tbs;
|
||||
// Tbs.x.y = a3;
|
||||
// Tbs.y.x = -a3;
|
||||
// Tbs.x.z = -a2;
|
||||
// Tbs.z.x = a2;
|
||||
// Tbs.y.z = a1;
|
||||
// Tbs.z.y = -a1;
|
||||
// // Transformation matrix from navigation to sensor axes
|
||||
// Mat3f Tns;
|
||||
// float H_LOS[n_states];
|
||||
// for (uint8_t i = 0; i < n_states; i++) {
|
||||
// H_LOS[i] = 0.0f;
|
||||
// }
|
||||
// Vector3f velNED_local;
|
||||
// Vector3f relVelSensor;
|
||||
|
||||
// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
// {
|
||||
// // Sequential fusion of XY components to spread processing load across
|
||||
// // two prediction time steps.
|
||||
|
||||
// // Calculate observation jacobians and Kalman gains
|
||||
// if (fuseOptFlowData)
|
||||
// {
|
||||
// // Copy required states to local variable names
|
||||
// q0 = statesAtOptFlowTime[0];
|
||||
// q1 = statesAtOptFlowTime[1];
|
||||
// q2 = statesAtOptFlowTime[2];
|
||||
// q3 = statesAtOptFlowTime[3];
|
||||
// vn = statesAtOptFlowTime[4];
|
||||
// ve = statesAtOptFlowTime[5];
|
||||
// vd = statesAtOptFlowTime[6];
|
||||
// pd = statesAtOptFlowTime[9];
|
||||
// ptd = statesAtOptFlowTime[22];
|
||||
// velNED_local.x = vn;
|
||||
// velNED_local.y = ve;
|
||||
// velNED_local.z = vd;
|
||||
|
||||
// // calculate rotation from NED to body axes
|
||||
// float q00 = sq(q0);
|
||||
// float q11 = sq(q1);
|
||||
// float q22 = sq(q2);
|
||||
// float q33 = sq(q3);
|
||||
// float q01 = q0 * q1;
|
||||
// float q02 = q0 * q2;
|
||||
// float q03 = q0 * q3;
|
||||
// float q12 = q1 * q2;
|
||||
// float q13 = q1 * q3;
|
||||
// float q23 = q2 * q3;
|
||||
// Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
// Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
// Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
// Tnb_local.y.x = 2*(q12 - q03);
|
||||
// Tnb_local.z.x = 2*(q13 + q02);
|
||||
// Tnb_local.x.y = 2*(q12 + q03);
|
||||
// Tnb_local.z.y = 2*(q23 - q01);
|
||||
// Tnb_local.x.z = 2*(q13 - q02);
|
||||
// Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// // calculate transformation from NED to sensor axes
|
||||
// Tns = Tbs*Tnb_local;
|
||||
|
||||
// // calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// // calculate relative velocity in sensor frame
|
||||
// relVelSensor = Tns*velNED_local;
|
||||
|
||||
// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
// losPred[0] = relVelSensor.y/range;
|
||||
// losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// // Calculate observation jacobians
|
||||
// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
// SH_LOS[3] = 1/(pd - ptd);
|
||||
// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
// SH_LOS[7] = 1/sq(pd - ptd);
|
||||
// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// // Calculate Kalman gain
|
||||
// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
// SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
// innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
|
||||
// obsIndex = 1;
|
||||
// fuseOptFlowData = false;
|
||||
// }
|
||||
// else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
// {
|
||||
// // Calculate observation jacobians
|
||||
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// // Calculate Kalman gains
|
||||
// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
// innovOptFlow[1] = losPred[1] - losData[1];
|
||||
|
||||
// // reset the observation index
|
||||
// obsIndex = 0;
|
||||
// fuseOptFlowData = false;
|
||||
// }
|
||||
|
||||
// // Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
// {
|
||||
// // correct the state vector
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
// }
|
||||
// // normalise the quaternion states
|
||||
// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
// if (quatMag > 1e-12f)
|
||||
// {
|
||||
// for (uint8_t j= 0; j<=3; j++)
|
||||
// {
|
||||
// float quatMagInv = 1.0f/quatMag;
|
||||
// states[j] = states[j] * quatMagInv;
|
||||
// }
|
||||
// }
|
||||
// // correct the covariance P = (I - K*H)*P
|
||||
// // take advantage of the empty columns in KH to reduce the
|
||||
// // number of operations
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j <= 6; j++)
|
||||
// {
|
||||
// KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
// }
|
||||
// for (uint8_t j = 7; j <= 8; j++)
|
||||
// {
|
||||
// KH[i][j] = 0.0f;
|
||||
// }
|
||||
// KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
// for (uint8_t j = 10; j <= 21; j++)
|
||||
// {
|
||||
// KH[i][j] = 0.0f;
|
||||
// }
|
||||
// KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
// }
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// KHP[i][j] = 0.0f;
|
||||
// for (uint8_t k = 0; k <= 6; k++)
|
||||
// {
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
// }
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// P[i][j] = P[i][j] - KHP[i][j];
|
||||
// }
|
||||
// }
|
||||
// ForceSymmetry();
|
||||
// ConstrainVariances();
|
||||
// }
|
||||
}
|
||||
|
||||
/*
|
||||
Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
|
||||
This fiter requires optical flow rates that are not motion compensated
|
||||
Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
|
||||
*/
|
||||
void AttPosEKF::GroundEKF()
|
||||
{
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
if (!inhibitGndState) {
|
||||
float distanceTravelledSq;
|
||||
distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
|
||||
prevPosN = statesAtRngTime[7];
|
||||
prevPosE = statesAtRngTime[8];
|
||||
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
|
||||
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
|
||||
}
|
||||
Vector3f velNED_local;
|
||||
Vector3f relVelSensor;
|
||||
// we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems
|
||||
Popt[0][0] = 0.0f;
|
||||
Popt[0][1] = 0.0f;
|
||||
Popt[1][0] = 0.0f;
|
||||
|
||||
// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
{
|
||||
// Sequential fusion of XY components to spread processing load across
|
||||
// two prediction time steps.
|
||||
// Fuse range finder data
|
||||
// Need to check that our range finder tilt angle is less than 30 degrees
|
||||
float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch);
|
||||
if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) {
|
||||
float range; // range from camera to centre of image
|
||||
float q0; // quaternion at optical flow measurement time
|
||||
float q1; // quaternion at optical flow measurement time
|
||||
float q2; // quaternion at optical flow measurement time
|
||||
float q3; // quaternion at optical flow measurement time
|
||||
float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (fuseOptFlowData)
|
||||
{
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtOptFlowTime[0];
|
||||
q1 = statesAtOptFlowTime[1];
|
||||
q2 = statesAtOptFlowTime[2];
|
||||
q3 = statesAtOptFlowTime[3];
|
||||
vn = statesAtOptFlowTime[4];
|
||||
ve = statesAtOptFlowTime[5];
|
||||
vd = statesAtOptFlowTime[6];
|
||||
pd = statesAtOptFlowTime[9];
|
||||
ptd = statesAtOptFlowTime[22];
|
||||
velNED_local.x = vn;
|
||||
velNED_local.y = ve;
|
||||
velNED_local.z = vd;
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtRngTime[0];
|
||||
q1 = statesAtRngTime[1];
|
||||
q2 = statesAtRngTime[2];
|
||||
q3 = statesAtRngTime[3];
|
||||
|
||||
// calculate rotation from NED to body axes
|
||||
float q00 = sq(q0);
|
||||
float q11 = sq(q1);
|
||||
float q22 = sq(q2);
|
||||
float q33 = sq(q3);
|
||||
float q01 = q0 * q1;
|
||||
float q02 = q0 * q2;
|
||||
float q03 = q0 * q3;
|
||||
float q12 = q1 * q2;
|
||||
float q13 = q1 * q3;
|
||||
float q23 = q2 * q3;
|
||||
Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
Tnb_local.y.x = 2*(q12 - q03);
|
||||
Tnb_local.z.x = 2*(q13 + q02);
|
||||
Tnb_local.x.y = 2*(q12 + q03);
|
||||
Tnb_local.z.y = 2*(q23 - q01);
|
||||
Tnb_local.x.z = 2*(q13 - q02);
|
||||
Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// calculate transformation from NED to sensor axes
|
||||
Tns = Tbs*Tnb_local;
|
||||
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = Tns*velNED_local;
|
||||
|
||||
// divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
losPred[0] = relVelSensor.y/range;
|
||||
losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
//printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
//printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
//printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
SH_LOS[3] = 1/(pd - ptd);
|
||||
SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
SH_LOS[7] = 1/sq(pd - ptd);
|
||||
SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gain
|
||||
SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// reset the observation index to 0 (we start by fusing the X
|
||||
// measurement)
|
||||
obsIndex = 0;
|
||||
fuseOptFlowData = false;
|
||||
// calculate Kalman gains
|
||||
float SK_RNG[3];
|
||||
SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
|
||||
SK_RNG[2] = 1/SK_RNG[0];
|
||||
float K_RNG[2];
|
||||
if (!inhibitScaleState) {
|
||||
K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[0] = 0.0f;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
// Calculate observation jacobians
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gains
|
||||
SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
innovOptFlow[1] = losPred[1] - losData[1];
|
||||
if (!inhibitGndState) {
|
||||
K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[1] = 0.0f;
|
||||
}
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = 1.0f/SK_RNG[1];
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
|
||||
|
||||
// estimate range to centre of image
|
||||
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
|
||||
|
||||
// Calculate the measurement innovation
|
||||
innovRng = range - rngMea;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (auxRngTestRatio < 1.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
}
|
||||
// normalise the quaternion states
|
||||
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
if (quatMag > 1e-12f)
|
||||
{
|
||||
for (uint8_t j= 0; j<=3; j++)
|
||||
{
|
||||
float quatMagInv = 1.0f/quatMag;
|
||||
states[j] = states[j] * quatMagInv;
|
||||
}
|
||||
}
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= 6; j++)
|
||||
{
|
||||
KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
}
|
||||
for (uint8_t j = 7; j <= 8; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
for (uint8_t j = 10; j <= 21; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
KHP[i][j] = 0.0f;
|
||||
for (uint8_t k = 0; k <= 6; k++)
|
||||
{
|
||||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
}
|
||||
KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
// correct the state
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
flowStates[i] -= K_RNG[i] * innovRng;
|
||||
}
|
||||
// constrain the states
|
||||
|
||||
// constrain focal length to 0.1 to 10 mm
|
||||
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
|
||||
// constrain altitude
|
||||
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
|
||||
|
||||
// correct the covariance matrix
|
||||
float nextPopt[2][2];
|
||||
nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
// prevent the state variances from becoming negative and maintain symmetry
|
||||
Popt[0][0] = maxf(nextPopt[0][0],0.0f);
|
||||
Popt[1][1] = maxf(nextPopt[1][1],0.0f);
|
||||
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
|
||||
Popt[1][0] = Popt[0][1];
|
||||
}
|
||||
}
|
||||
obsIndex = obsIndex + 1;
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
@@ -2126,6 +2304,24 @@ float AttPosEKF::sq(float valIn)
|
||||
return valIn*valIn;
|
||||
}
|
||||
|
||||
float AttPosEKF::maxf(float valIn1, float valIn2)
|
||||
{
|
||||
if (valIn1 >= valIn2) {
|
||||
return valIn1;
|
||||
} else {
|
||||
return valIn2;
|
||||
}
|
||||
}
|
||||
|
||||
float AttPosEKF::min(float valIn1, float valIn2)
|
||||
{
|
||||
if (valIn1 <= valIn2) {
|
||||
return valIn1;
|
||||
} else {
|
||||
return valIn2;
|
||||
}
|
||||
}
|
||||
|
||||
// Store states in a history array along with time stamp
|
||||
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
|
||||
{
|
||||
@@ -2322,9 +2518,9 @@ void AttPosEKF::OnGroundCheck()
|
||||
}
|
||||
// don't update terrain offset state if on ground
|
||||
if (onGround) {
|
||||
inhibitGndHgtState = true;
|
||||
inhibitGndState = true;
|
||||
} else {
|
||||
inhibitGndHgtState = false;
|
||||
inhibitGndState = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3006,9 +3202,14 @@ void AttPosEKF::ZeroVariables()
|
||||
{
|
||||
|
||||
// Initialize on-init initialized variables
|
||||
|
||||
dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
|
||||
dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
|
||||
dtGpsFilt = 1.0f / 5.0f;
|
||||
dtHgtFilt = 1.0f / 100.0f;
|
||||
storeIndex = 0;
|
||||
|
||||
lastVelPosFusion = millis();
|
||||
|
||||
// Do the data structure init
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
@@ -3028,6 +3229,13 @@ void AttPosEKF::ZeroVariables()
|
||||
dVelIMU.zero();
|
||||
lastGyroOffset.zero();
|
||||
|
||||
windSpdFiltNorth = 0.0f;
|
||||
windSpdFiltEast = 0.0f;
|
||||
// setting the altitude to zero will give us a higher
|
||||
// gain to adjust faster in the first step
|
||||
windSpdFiltAltitude = 0.0f;
|
||||
windSpdFiltClimb = 0.0f;
|
||||
|
||||
for (unsigned i = 0; i < data_buffer_size; i++) {
|
||||
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
|
||||
@@ -80,6 +80,14 @@ public:
|
||||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
|
||||
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
|
||||
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
|
||||
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
|
||||
rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
|
||||
minFlowRng = 0.01f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
}
|
||||
|
||||
struct mag_state_struct {
|
||||
@@ -116,13 +124,16 @@ public:
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
// Times
|
||||
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
@@ -140,7 +151,16 @@ public:
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
|
||||
float dtIMUfilt; // average time between IMU measurements (sec)
|
||||
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
|
||||
float dtVelPosFilt; // average time between position / velocity fusion steps
|
||||
float dtHgtFilt; // average time between height measurement updates
|
||||
float dtGpsFilt; // average time between gps measurement updates
|
||||
float windSpdFiltNorth; // average wind speed north component
|
||||
float windSpdFiltEast; // average wind speed east component
|
||||
float windSpdFiltAltitude; // the last altitude used to filter wind speed
|
||||
float windSpdFiltClimb; // filtered climb rate
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
@@ -192,7 +212,8 @@ public:
|
||||
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
@@ -211,6 +232,30 @@ public:
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
// Optical Flow error estimation
|
||||
float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
|
||||
|
||||
// Two state EKF used to estimate focal length scale factor and terrain position
|
||||
float Popt[2][2]; // state covariance matrix
|
||||
float flowStates[2]; // flow states [scale factor, terrain position]
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
|
||||
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
|
||||
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
|
||||
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
|
||||
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
|
||||
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
|
||||
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
|
||||
float flowInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
float rngInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float minFlowRng; // minimum range over which to fuse optical flow measurements
|
||||
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
|
||||
void updateDtGpsFilt(float dt);
|
||||
|
||||
void updateDtHgtFilt(float dt);
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
@@ -226,6 +271,8 @@ void FuseRangeFinder();
|
||||
|
||||
void FuseOptFlow();
|
||||
|
||||
void GroundEKF();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
@@ -268,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
static float maxf(float valIn1, float valIn2);
|
||||
|
||||
static float min(float valIn1, float valIn2);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
@@ -300,6 +351,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
protected:
|
||||
|
||||
void updateDtVelPosFilt(float dt);
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
bool GyroOffsetsDiverged();
|
||||
@@ -314,3 +367,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
uint64_t getMicros();
|
||||
|
||||
|
||||
@@ -63,6 +63,7 @@
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
@@ -124,6 +125,7 @@ private:
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _manual_sub; /**< notification of manual control updates */
|
||||
int _global_pos_sub; /**< global position subscription */
|
||||
int _vehicle_status_sub; /**< vehicle status subscription */
|
||||
|
||||
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
|
||||
@@ -139,6 +141,7 @@ private:
|
||||
struct actuator_controls_s _actuators; /**< actuator control inputs */
|
||||
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
|
||||
struct vehicle_global_position_s _global_pos; /**< global position */
|
||||
struct vehicle_status_s _vehicle_status; /**< vehicle status */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
|
||||
@@ -275,6 +278,11 @@ private:
|
||||
*/
|
||||
void global_pos_poll();
|
||||
|
||||
/**
|
||||
* Check for vehicle status updates.
|
||||
*/
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
@@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
_params_sub(-1),
|
||||
_manual_sub(-1),
|
||||
_global_pos_sub(-1),
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_rate_sp_pub(-1),
|
||||
@@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
_actuators = {};
|
||||
_actuators_airframe = {};
|
||||
_global_pos = {};
|
||||
_vehicle_status = {};
|
||||
|
||||
|
||||
_parameter_handles.tconst = param_find("FW_ATT_TC");
|
||||
@@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll()
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
bool vehicle_status_updated;
|
||||
orb_check(_vehicle_status_sub, &vehicle_status_updated);
|
||||
|
||||
if (vehicle_status_updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
@@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main()
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vcontrol_mode_sub, 200);
|
||||
@@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main()
|
||||
vehicle_accel_poll();
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_manual_poll();
|
||||
vehicle_status_poll();
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[2];
|
||||
@@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main()
|
||||
|
||||
global_pos_poll();
|
||||
|
||||
vehicle_status_poll();
|
||||
|
||||
/* lock integrator until control is started */
|
||||
bool lock_integrator;
|
||||
|
||||
@@ -779,6 +805,13 @@ FixedwingAttitudeControl::task_main()
|
||||
}
|
||||
}
|
||||
|
||||
/* If the aircraft is on ground reset the integrators */
|
||||
if (_vehicle_status.condition_landed) {
|
||||
_roll_ctrl.reset_integrator();
|
||||
_pitch_ctrl.reset_integrator();
|
||||
_yaw_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
/* Prepare speed_body_u and speed_body_w */
|
||||
float speed_body_u = 0.0f;
|
||||
float speed_body_v = 0.0f;
|
||||
|
||||
@@ -211,6 +211,7 @@ private:
|
||||
float max_climb_rate;
|
||||
float climbout_diff;
|
||||
float heightrate_p;
|
||||
float heightrate_ff;
|
||||
float speedrate_p;
|
||||
float throttle_damp;
|
||||
float integrator_gain;
|
||||
@@ -256,6 +257,7 @@ private:
|
||||
param_t max_climb_rate;
|
||||
param_t climbout_diff;
|
||||
param_t heightrate_p;
|
||||
param_t heightrate_ff;
|
||||
param_t speedrate_p;
|
||||
param_t throttle_damp;
|
||||
param_t integrator_gain;
|
||||
@@ -494,6 +496,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
|
||||
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
|
||||
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
|
||||
_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
|
||||
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
@@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update()
|
||||
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
|
||||
|
||||
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
|
||||
param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
|
||||
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
|
||||
|
||||
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
|
||||
@@ -600,6 +604,7 @@ FixedwingPositionControl::parameters_update()
|
||||
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
|
||||
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
|
||||
_tecs.set_heightrate_p(_parameters.heightrate_p);
|
||||
_tecs.set_heightrate_ff(_parameters.heightrate_ff);
|
||||
_tecs.set_speedrate_p(_parameters.speedrate_p);
|
||||
|
||||
/* sanity check parameters */
|
||||
|
||||
@@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
|
||||
/**
|
||||
* Throttle limit max
|
||||
*
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
|
||||
*
|
||||
* @group L1 Control
|
||||
@@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
|
||||
/**
|
||||
* Throttle limit min
|
||||
*
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* some aerodynamic drag from a turning prop to improve the descent rate.
|
||||
*
|
||||
* For aircraft with internal combustion engine this parameter should be set
|
||||
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
||||
/**
|
||||
* Throttle limit value before flare
|
||||
*
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* before arcraft will flare.
|
||||
*
|
||||
* @group L1 Control
|
||||
@@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
|
||||
/**
|
||||
* Maximum climb rate
|
||||
*
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* FW_THR_MAX reduced.
|
||||
*
|
||||
* @group L1 Control
|
||||
@@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
|
||||
/**
|
||||
* Minimum descent rate
|
||||
*
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* to measure FW_T_CLMB_MAX.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
|
||||
/**
|
||||
* Maximum descent rate
|
||||
*
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* the aircraft.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
|
||||
/**
|
||||
* TECS time constant
|
||||
*
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
@@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
|
||||
/**
|
||||
* TECS Throttle time constant
|
||||
*
|
||||
* This is the time constant of the TECS throttle control algorithm (in seconds).
|
||||
* This is the time constant of the TECS throttle control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
|
||||
/**
|
||||
* Throttle damping factor
|
||||
*
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* Increase to add damping to correct for oscillations in speed and height.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
|
||||
/**
|
||||
* Integrator gain
|
||||
*
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* increases overshoot.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
|
||||
* Maximum vertical acceleration
|
||||
*
|
||||
* This is the maximum vertical acceleration (in metres/second square)
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* from under-speed conditions.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
|
||||
/**
|
||||
* Complementary filter "omega" parameter for height
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* the solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
|
||||
/**
|
||||
* Complementary filter "omega" parameter for speed
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* improved airspeed estimate. Increasing this frequency weights the solution
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
|
||||
/**
|
||||
* Roll -> Throttle feedforward
|
||||
*
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
|
||||
/**
|
||||
* Speed <--> Altitude priority
|
||||
*
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
|
||||
/**
|
||||
* Pitch damping factor
|
||||
*
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* properly.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
|
||||
|
||||
/**
|
||||
* Height rate FF factor
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Speed rate P factor
|
||||
*
|
||||
|
||||
@@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
|
||||
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
|
||||
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
|
||||
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <v1.0/pixhawk/mavlink.h>
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
||||
@@ -1387,23 +1387,26 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
configure_stream("SYS_STATUS", 1.0f);
|
||||
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
|
||||
configure_stream("HIGHRES_IMU", 1.0f);
|
||||
configure_stream("ATTITUDE", 10.0f);
|
||||
configure_stream("ATTITUDE", 15.0f);
|
||||
configure_stream("VFR_HUD", 8.0f);
|
||||
configure_stream("GPS_RAW_INT", 1.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 3.0f);
|
||||
configure_stream("LOCAL_POSITION_NED", 3.0f);
|
||||
configure_stream("RC_CHANNELS_RAW", 1.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 3.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 15.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||
configure_stream("OPTICAL_FLOW", 20.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_ONBOARD:
|
||||
configure_stream("SYS_STATUS", 1.0f);
|
||||
// XXX OBC change back: We need to be bandwidth-efficient here too
|
||||
configure_stream("ATTITUDE", 50.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 50.0f);
|
||||
configure_stream("CAMERA_CAPTURE", 2.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 50.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
@@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached()
|
||||
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
|
||||
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
|
||||
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
|
||||
* coordinates with a radius equal to the loiter_radius field. It is not flying
|
||||
* through the waypoint center.
|
||||
* Therefore the item is marked as reached once the system reaches the loiter
|
||||
* radius (+ some margin). Time inside and turn count is handled elsewhere.
|
||||
*/
|
||||
if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else {
|
||||
/* for normal mission items used their acceptance radius */
|
||||
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
|
||||
|
||||
@@ -1432,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
|
||||
log_msg.body.log_GPOS.eph = buf.global_pos.eph;
|
||||
log_msg.body.log_GPOS.epv = buf.global_pos.epv;
|
||||
if (buf.global_pos.terrain_alt_valid) {
|
||||
log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt;
|
||||
} else {
|
||||
log_msg.body.log_GPOS.terrain_alt = -1.0f;
|
||||
}
|
||||
LOGBUFFER_WRITE_AND_COUNT(GPOS);
|
||||
}
|
||||
|
||||
@@ -1464,7 +1469,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
|
||||
LOGBUFFER_WRITE_AND_COUNT(VICN);
|
||||
}
|
||||
|
||||
|
||||
/* --- VISION POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
|
||||
log_msg.msg_type = LOG_VISN_MSG;
|
||||
|
||||
@@ -220,6 +220,7 @@ struct log_GPOS_s {
|
||||
float vel_d;
|
||||
float eph;
|
||||
float epv;
|
||||
float terrain_alt;
|
||||
};
|
||||
|
||||
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
|
||||
@@ -449,7 +450,7 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
|
||||
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
|
||||
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
|
||||
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
|
||||
@@ -72,6 +72,8 @@ struct vehicle_global_position_s {
|
||||
float yaw; /**< Yaw in radians -PI..+PI. */
|
||||
float eph; /**< Standard deviation of position estimate horizontally */
|
||||
float epv; /**< Standard deviation of position vertically */
|
||||
float terrain_alt; /**< Terrain altitude in m, WGS84 */
|
||||
bool terrain_alt_valid; /**< Terrain altitude estimate is valid */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user