jgoppert
9 years ago
committed by
Lorenz Meier
33 changed files with 3080 additions and 64 deletions
@ -1,2 +1,2 @@
@@ -1,2 +1,2 @@
|
||||
handle SIGCONT nostop |
||||
handle SIGCONT nostop noprint nopass |
||||
run |
||||
|
||||
@ -0,0 +1,5 @@
@@ -0,0 +1,5 @@
|
||||
include(cmake/configs/nuttx_px4fmu-v2_default.cmake) |
||||
|
||||
list(APPEND config_module_list |
||||
modules/local_position_estimator |
||||
) |
||||
@ -0,0 +1,9 @@
@@ -0,0 +1,9 @@
|
||||
include(cmake/configs/posix_sitl_simple.cmake) |
||||
|
||||
list(APPEND config_module_list |
||||
modules/local_position_estimator |
||||
) |
||||
|
||||
set(config_sitl_rcS |
||||
posix-configs/SITL/init/rcS_lpe |
||||
) |
||||
@ -0,0 +1,65 @@
@@ -0,0 +1,65 @@
|
||||
uorb start |
||||
simulator start -s |
||||
param load |
||||
param set MAV_TYPE 2 |
||||
param set MC_PITCHRATE_P 0.15 |
||||
param set MC_ROLLRATE_P 0.15 |
||||
param set MC_YAW_P 2.0 |
||||
param set MC_YAWRATE_P 0.35 |
||||
param set SYS_AUTOSTART 4010 |
||||
param set SYS_RESTART_TYPE 2 |
||||
dataman start |
||||
param set CAL_GYRO0_ID 2293760 |
||||
param set CAL_ACC0_ID 1376256 |
||||
param set CAL_ACC1_ID 1310720 |
||||
param set CAL_MAG0_ID 196608 |
||||
param set CAL_GYRO0_XOFF 0.01 |
||||
param set CAL_ACC0_XOFF 0.01 |
||||
param set CAL_ACC0_YOFF -0.01 |
||||
param set CAL_ACC0_ZOFF 0.01 |
||||
param set CAL_ACC0_XSCALE 1.01 |
||||
param set CAL_ACC0_YSCALE 1.01 |
||||
param set CAL_ACC0_ZSCALE 1.01 |
||||
param set CAL_ACC1_XOFF 0.01 |
||||
param set CAL_MAG0_XOFF 0.01 |
||||
param set MPC_XY_P 0.4 |
||||
param set MPC_XY_VEL_P 0.2 |
||||
param set MPC_XY_VEL_D 0.005 |
||||
param set SENS_BOARD_ROT 0 |
||||
|
||||
# changes for LPE |
||||
param set COM_RC_IN_MODE 1 |
||||
param set LPE_BETA_MAX 10000 |
||||
|
||||
rgbled start |
||||
tone_alarm start |
||||
gyrosim start |
||||
accelsim start |
||||
barosim start |
||||
adcsim start |
||||
gpssim start |
||||
pwm_out_sim mode_pwm |
||||
sleep 1 |
||||
sensors start |
||||
commander start |
||||
land_detector start multicopter |
||||
navigator start |
||||
attitude_estimator_q start |
||||
mc_pos_control start |
||||
mc_att_control start |
||||
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix |
||||
mavlink start -u 14556 -r 2000000 |
||||
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 |
||||
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 |
||||
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 |
||||
mavlink stream -r 80 -s ATTITUDE -u 14556 |
||||
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 |
||||
mavlink stream -r 20 -s RC_CHANNELS -u 14556 |
||||
mavlink stream -r 250 -s HIGHRES_IMU -u 14556 |
||||
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 |
||||
mavlink boot_complete |
||||
sdlog2 start -r 100 -e -t -a |
||||
|
||||
# start LPE at end, when we know it is ok to init sensors |
||||
sleep 5 |
||||
local_position_estimator start |
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,310 @@
@@ -0,0 +1,310 @@
|
||||
#pragma once |
||||
|
||||
#include <controllib/uorb/blocks.hpp> |
||||
#include <mathlib/mathlib.h> |
||||
#include <systemlib/perf_counter.h> |
||||
#include <lib/geo/geo.h> |
||||
|
||||
#ifdef USE_MATRIX_LIB |
||||
#include "matrix/src/Matrix.hpp" |
||||
using namespace matrix; |
||||
#else |
||||
#include <Eigen/Eigen> |
||||
using namespace Eigen; |
||||
#endif |
||||
|
||||
// uORB Subscriptions
|
||||
#include <uORB/Subscription.hpp> |
||||
#include <uORB/topics/vehicle_status.h> |
||||
#include <uORB/topics/actuator_armed.h> |
||||
#include <uORB/topics/vehicle_control_mode.h> |
||||
#include <uORB/topics/vehicle_attitude.h> |
||||
#include <uORB/topics/vehicle_attitude_setpoint.h> |
||||
#include <uORB/topics/optical_flow.h> |
||||
#include <uORB/topics/sensor_combined.h> |
||||
#include <uORB/topics/distance_sensor.h> |
||||
#include <uORB/topics/parameter_update.h> |
||||
#include <uORB/topics/manual_control_setpoint.h> |
||||
#include <uORB/topics/home_position.h> |
||||
#include <uORB/topics/vehicle_gps_position.h> |
||||
#include <uORB/topics/vision_position_estimate.h> |
||||
#include <uORB/topics/att_pos_mocap.h> |
||||
|
||||
// uORB Publications
|
||||
#include <uORB/Publication.hpp> |
||||
#include <uORB/topics/vehicle_local_position.h> |
||||
#include <uORB/topics/vehicle_global_position.h> |
||||
#include <uORB/topics/filtered_bottom_flow.h> |
||||
#include <uORB/topics/estimator_status.h> |
||||
|
||||
#define CBRK_NO_VISION_KEY 328754 |
||||
|
||||
using namespace control; |
||||
|
||||
enum fault_t { |
||||
FAULT_NONE = 0, |
||||
FAULT_MINOR, |
||||
FAULT_SEVERE |
||||
}; |
||||
|
||||
enum sensor_t { |
||||
SENSOR_BARO = 0, |
||||
SENSOR_GPS, |
||||
SENSOR_LIDAR, |
||||
SENSOR_FLOW, |
||||
SENSOR_SONAR, |
||||
SENSOR_VISION, |
||||
SENSOR_MOCAP |
||||
}; |
||||
|
||||
class BlockLocalPositionEstimator : public control::SuperBlock |
||||
{ |
||||
//
|
||||
// The purpose of this estimator is to provide a robust solution for
|
||||
// indoor flight.
|
||||
//
|
||||
// dynamics:
|
||||
//
|
||||
// x(+) = A * x(-) + B * u(+)
|
||||
// y_i = C_i*x
|
||||
//
|
||||
// kalman filter
|
||||
//
|
||||
// E[xx'] = P
|
||||
// E[uu'] = W
|
||||
// E[y_iy_i'] = R_i
|
||||
//
|
||||
// prediction
|
||||
// x(+|-) = A*x(-|-) + B*u(+)
|
||||
// P(+|-) = A*P(-|-)*A' + B*W*B'
|
||||
//
|
||||
// correction
|
||||
// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) )
|
||||
//
|
||||
//
|
||||
// input:
|
||||
// ax, ay, az (acceleration NED)
|
||||
//
|
||||
// states:
|
||||
// px, py, pz , ( position NED)
|
||||
// vx, vy, vz ( vel NED),
|
||||
// bx, by, bz ( TODO accelerometer bias)
|
||||
// tz (TODO terrain altitude)
|
||||
//
|
||||
// measurements:
|
||||
//
|
||||
// sonar: pz (measured d*cos(phi)*cos(theta))
|
||||
//
|
||||
// baro: pz
|
||||
//
|
||||
// flow: vx, vy (flow is in body x, y frame)
|
||||
//
|
||||
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
|
||||
//
|
||||
// lidar: px (actual measured d*cos(phi)*cos(theta))
|
||||
//
|
||||
// vision: px, py, pz, vx, vy, vz
|
||||
//
|
||||
// mocap: px, py, pz
|
||||
//
|
||||
public: |
||||
BlockLocalPositionEstimator(); |
||||
void update(); |
||||
virtual ~BlockLocalPositionEstimator(); |
||||
|
||||
private: |
||||
// prevent copy and assignment
|
||||
BlockLocalPositionEstimator(const BlockLocalPositionEstimator &); |
||||
BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &); |
||||
|
||||
// constants
|
||||
static const uint8_t n_x = 9; |
||||
static const uint8_t n_u = 3; // 3 accelerations
|
||||
static const uint8_t n_y_flow = 2; |
||||
static const uint8_t n_y_sonar = 1; |
||||
static const uint8_t n_y_baro = 1; |
||||
static const uint8_t n_y_lidar = 1; |
||||
static const uint8_t n_y_gps = 6; |
||||
static const uint8_t n_y_vision = 3; |
||||
static const uint8_t n_y_mocap = 3; |
||||
enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz}; |
||||
enum {U_ax = 0, U_ay, U_az}; |
||||
enum {Y_baro_z = 0}; |
||||
enum {Y_lidar_z = 0}; |
||||
enum {Y_flow_x = 0, Y_flow_y}; |
||||
enum {Y_sonar_z = 0}; |
||||
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz}; |
||||
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz}; |
||||
enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z}; |
||||
enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM}; |
||||
|
||||
// methods
|
||||
// ----------------------------
|
||||
void initP(); |
||||
|
||||
// predict the next state
|
||||
void predict(); |
||||
|
||||
// correct the state prediction wtih a measurement
|
||||
void correctBaro(); |
||||
void correctGps(); |
||||
void correctLidar(); |
||||
void correctFlow(); |
||||
void correctSonar(); |
||||
void correctVision(); |
||||
void correctmocap(); |
||||
|
||||
// sensor initialization
|
||||
void updateHome(); |
||||
void initBaro(); |
||||
void initGps(); |
||||
void initLidar(); |
||||
void initSonar(); |
||||
void initFlow(); |
||||
void initVision(); |
||||
void initmocap(); |
||||
|
||||
// publications
|
||||
void publishLocalPos(); |
||||
void publishGlobalPos(); |
||||
void publishFilteredFlow(); |
||||
void publishEstimatorStatus(); |
||||
|
||||
// attributes
|
||||
// ----------------------------
|
||||
|
||||
// subscriptions
|
||||
uORB::Subscription<vehicle_status_s> _sub_status; |
||||
uORB::Subscription<actuator_armed_s> _sub_armed; |
||||
uORB::Subscription<vehicle_control_mode_s> _sub_control_mode; |
||||
uORB::Subscription<vehicle_attitude_s> _sub_att; |
||||
uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp; |
||||
uORB::Subscription<optical_flow_s> _sub_flow; |
||||
uORB::Subscription<sensor_combined_s> _sub_sensor; |
||||
uORB::Subscription<distance_sensor_s> _sub_distance; |
||||
uORB::Subscription<parameter_update_s> _sub_param_update; |
||||
uORB::Subscription<manual_control_setpoint_s> _sub_manual; |
||||
uORB::Subscription<home_position_s> _sub_home; |
||||
uORB::Subscription<vehicle_gps_position_s> _sub_gps; |
||||
uORB::Subscription<vision_position_estimate_s> _sub_vision_pos; |
||||
uORB::Subscription<att_pos_mocap_s> _sub_mocap; |
||||
|
||||
// publications
|
||||
uORB::Publication<vehicle_local_position_s> _pub_lpos; |
||||
uORB::Publication<vehicle_global_position_s> _pub_gpos; |
||||
uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow; |
||||
uORB::Publication<estimator_status_s> _pub_est_status; |
||||
|
||||
// map projection
|
||||
struct map_projection_reference_s _map_ref; |
||||
|
||||
// parameters
|
||||
BlockParamInt _integrate; |
||||
|
||||
BlockParamFloat _flow_xy_stddev; |
||||
BlockParamFloat _sonar_z_stddev; |
||||
|
||||
BlockParamFloat _lidar_z_stddev; |
||||
|
||||
BlockParamFloat _accel_xy_stddev; |
||||
BlockParamFloat _accel_z_stddev; |
||||
|
||||
BlockParamFloat _baro_stddev; |
||||
|
||||
BlockParamFloat _gps_xy_stddev; |
||||
BlockParamFloat _gps_z_stddev; |
||||
|
||||
BlockParamFloat _gps_vxy_stddev; |
||||
BlockParamFloat _gps_vz_stddev; |
||||
|
||||
BlockParamFloat _gps_eph_max; |
||||
|
||||
BlockParamFloat _vision_xy_stddev; |
||||
BlockParamFloat _vision_z_stddev; |
||||
BlockParamInt _no_vision; |
||||
BlockParamFloat _beta_max; |
||||
|
||||
BlockParamFloat _mocap_p_stddev; |
||||
|
||||
// process noise
|
||||
BlockParamFloat _pn_p_noise_power; |
||||
BlockParamFloat _pn_v_noise_power; |
||||
BlockParamFloat _pn_b_noise_power; |
||||
|
||||
// misc
|
||||
struct pollfd _polls[3]; |
||||
uint64_t _timeStamp; |
||||
uint64_t _time_last_xy; |
||||
uint64_t _time_last_flow; |
||||
uint64_t _time_last_baro; |
||||
uint64_t _time_last_gps; |
||||
uint64_t _time_last_lidar; |
||||
uint64_t _time_last_sonar; |
||||
uint64_t _time_last_vision_p; |
||||
uint64_t _time_last_mocap; |
||||
int _mavlink_fd; |
||||
|
||||
// initialization flags
|
||||
bool _baroInitialized; |
||||
bool _gpsInitialized; |
||||
bool _lidarInitialized; |
||||
bool _sonarInitialized; |
||||
bool _flowInitialized; |
||||
bool _visionInitialized; |
||||
bool _mocapInitialized; |
||||
|
||||
// init counts
|
||||
int _baroInitCount; |
||||
int _gpsInitCount; |
||||
int _lidarInitCount; |
||||
int _sonarInitCount; |
||||
int _flowInitCount; |
||||
int _visionInitCount; |
||||
int _mocapInitCount; |
||||
|
||||
// reference altitudes
|
||||
float _altHome; |
||||
bool _altHomeInitialized; |
||||
float _baroAltHome; |
||||
float _gpsAltHome; |
||||
float _lidarAltHome; |
||||
float _sonarAltHome; |
||||
float _flowAltHome; |
||||
Vector3f _visionHome; |
||||
Vector3f _mocapHome; |
||||
|
||||
// flow integration
|
||||
float _flowX; |
||||
float _flowY; |
||||
float _flowMeanQual; |
||||
|
||||
// referene lat/lon
|
||||
double _gpsLatHome; |
||||
double _gpsLonHome; |
||||
|
||||
// status
|
||||
bool _canEstimateXY; |
||||
bool _canEstimateZ; |
||||
bool _xyTimeout; |
||||
|
||||
// sensor faults
|
||||
fault_t _baroFault; |
||||
fault_t _gpsFault; |
||||
fault_t _lidarFault; |
||||
fault_t _flowFault; |
||||
fault_t _sonarFault; |
||||
fault_t _visionFault; |
||||
fault_t _mocapFault; |
||||
|
||||
bool _visionTimeout; |
||||
bool _mocapTimeout; |
||||
|
||||
perf_counter_t _loop_perf; |
||||
perf_counter_t _interval_perf; |
||||
perf_counter_t _err_perf; |
||||
|
||||
// state space
|
||||
Matrix<float, n_x, 1> _x; // state vector
|
||||
Matrix<float, n_u, 1> _u; // input vector
|
||||
Matrix<float, n_x, n_x> _P; // state covariance matrix
|
||||
}; |
||||
@ -0,0 +1,57 @@
@@ -0,0 +1,57 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 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. |
||||
# |
||||
############################################################################ |
||||
if(${OS} STREQUAL "nuttx") |
||||
list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) |
||||
elseif(${OS} STREQUAL "posix") |
||||
list(APPEND MODULE_CFLAGS -Wno-error) |
||||
# add matrix tests |
||||
add_subdirectory(matrix) |
||||
endif() |
||||
|
||||
# use custom matrix lib instead of Eigen |
||||
add_definitions(-DUSE_MATRIX_LIB) |
||||
|
||||
|
||||
px4_add_module( |
||||
MODULE modules__local_position_estimator |
||||
MAIN local_position_estimator |
||||
STACK 9216 |
||||
COMPILE_FLAGS ${MODULE_CFLAGS} |
||||
SRCS |
||||
local_position_estimator_main.cpp |
||||
BlockLocalPositionEstimator.cpp |
||||
params.c |
||||
DEPENDS |
||||
platforms__common |
||||
) |
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix : |
||||
@ -0,0 +1,169 @@
@@ -0,0 +1,169 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2015 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 local_position_estimator.cpp |
||||
* @author James Goppert <james.goppert@gmail.com> |
||||
* @author Mohammed Kabir |
||||
* @author Nuno Marques <n.marques21@hotmail.com> |
||||
* |
||||
* Local position estimator |
||||
*/ |
||||
|
||||
#include <unistd.h> |
||||
#include <stdio.h> |
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
#include <systemlib/systemlib.h> |
||||
#include <systemlib/param/param.h> |
||||
#include <systemlib/err.h> |
||||
#include <drivers/drv_hrt.h> |
||||
#include <math.h> |
||||
#include <fcntl.h> |
||||
#include <px4_posix.h> |
||||
|
||||
#include "BlockLocalPositionEstimator.hpp" |
||||
|
||||
static volatile bool thread_should_exit = false; /**< Deamon exit flag */ |
||||
static volatile bool thread_running = false; /**< Deamon status flag */ |
||||
static int deamon_task; /**< Handle of deamon task / thread */ |
||||
|
||||
/**
|
||||
* Deamon management function. |
||||
*/ |
||||
extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]); |
||||
|
||||
/**
|
||||
* Mainloop of deamon. |
||||
*/ |
||||
int local_position_estimator_thread_main(int argc, char *argv[]); |
||||
|
||||
/**
|
||||
* Print the correct usage. |
||||
*/ |
||||
static int usage(const char *reason); |
||||
|
||||
static int |
||||
usage(const char *reason) |
||||
{ |
||||
if (reason) { |
||||
fprintf(stderr, "%s\n", reason); |
||||
} |
||||
|
||||
fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p <additional params>]\n\n"); |
||||
return 1; |
||||
} |
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start |
||||
* the background job. The stack size assigned in the |
||||
* Makefile does only apply to this management task. |
||||
* |
||||
* The actual stack size should be set in the call |
||||
* to task_create(). |
||||
*/ |
||||
int local_position_estimator_main(int argc, char *argv[]) |
||||
{ |
||||
|
||||
if (argc < 1) { |
||||
usage("missing command"); |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "start")) { |
||||
|
||||
if (thread_running) { |
||||
warnx("already running"); |
||||
/* this is not an error */ |
||||
return 0; |
||||
} |
||||
|
||||
thread_should_exit = false; |
||||
|
||||
deamon_task = px4_task_spawn_cmd("lp_estimator", |
||||
SCHED_DEFAULT, |
||||
SCHED_PRIORITY_MAX - 5, |
||||
10240, |
||||
local_position_estimator_thread_main, |
||||
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); |
||||
return 0; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "stop")) { |
||||
if (thread_running) { |
||||
warnx("stop"); |
||||
thread_should_exit = true; |
||||
|
||||
} else { |
||||
warnx("not started"); |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "status")) { |
||||
if (thread_running) { |
||||
warnx("is running"); |
||||
|
||||
} else { |
||||
warnx("not started"); |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
usage("unrecognized command"); |
||||
return 1; |
||||
} |
||||
|
||||
int local_position_estimator_thread_main(int argc, char *argv[]) |
||||
{ |
||||
|
||||
warnx("starting"); |
||||
|
||||
using namespace control; |
||||
|
||||
|
||||
thread_running = true; |
||||
|
||||
BlockLocalPositionEstimator est; |
||||
|
||||
while (!thread_should_exit) { |
||||
est.update(); |
||||
} |
||||
|
||||
warnx("exiting."); |
||||
|
||||
thread_running = false; |
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
build*/ |
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
cmake_minimum_required(VERSION 2.8) |
||||
|
||||
project(matrix CXX) |
||||
|
||||
set(CMAKE_BUILD_TYPE "RelWithDebInfo") |
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++") |
||||
|
||||
enable_testing() |
||||
|
||||
include_directories(src) |
||||
|
||||
add_subdirectory(test) |
||||
@ -0,0 +1,459 @@
@@ -0,0 +1,459 @@
|
||||
#pragma once |
||||
|
||||
#include <stdio.h> |
||||
#include <stddef.h> |
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
#include <math.h> |
||||
|
||||
namespace matrix |
||||
{ |
||||
|
||||
template<typename T, size_t M, size_t N> |
||||
class Matrix |
||||
{ |
||||
|
||||
private: |
||||
T _data[M][N]; |
||||
size_t _rows; |
||||
size_t _cols; |
||||
|
||||
public: |
||||
|
||||
Matrix() : |
||||
_data(), |
||||
_rows(M), |
||||
_cols(N) |
||||
{ |
||||
} |
||||
|
||||
Matrix(const T *data) : |
||||
_data(), |
||||
_rows(M), |
||||
_cols(N) |
||||
{ |
||||
memcpy(_data, data, sizeof(_data)); |
||||
} |
||||
|
||||
Matrix(const Matrix &other) : |
||||
_data(), |
||||
_rows(M), |
||||
_cols(N) |
||||
{ |
||||
memcpy(_data, other._data, sizeof(_data)); |
||||
} |
||||
|
||||
Matrix(T x, T y, T z) : |
||||
_data(), |
||||
_rows(M), |
||||
_cols(N) |
||||
{ |
||||
// TODO, limit to only 3x1 matrices
|
||||
_data[0][0] = x; |
||||
_data[1][0] = y; |
||||
_data[2][0] = z; |
||||
} |
||||
|
||||
/**
|
||||
* Accessors/ Assignment etc. |
||||
*/ |
||||
|
||||
inline size_t rows() const |
||||
{ |
||||
return _rows; |
||||
} |
||||
|
||||
inline size_t cols() const |
||||
{ |
||||
return _rows; |
||||
} |
||||
|
||||
inline T operator()(size_t i) const |
||||
{ |
||||
return _data[i][0]; |
||||
} |
||||
|
||||
inline T operator()(size_t i, size_t j) const |
||||
{ |
||||
return _data[i][j]; |
||||
} |
||||
|
||||
inline T &operator()(size_t i) |
||||
{ |
||||
return _data[i][0]; |
||||
} |
||||
|
||||
inline T &operator()(size_t i, size_t j) |
||||
{ |
||||
return _data[i][j]; |
||||
} |
||||
|
||||
/**
|
||||
* Matrix Operations |
||||
*/ |
||||
|
||||
// this might use a lot of programming memory
|
||||
// since it instantiates a class for every
|
||||
// required mult pair, but it provides
|
||||
// compile time size_t checking
|
||||
template<size_t P> |
||||
Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const |
||||
{ |
||||
const Matrix<T, M, N> &self = *this; |
||||
Matrix<T, M, P> res; |
||||
res.setZero(); |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t k = 0; k < P; k++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
res(i, k) += self(i, j) * other(j, k); |
||||
} |
||||
} |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const |
||||
{ |
||||
Matrix<T, M, N> res; |
||||
const Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
res(i , j) = self(i, j) + other(i, j); |
||||
} |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
bool operator==(const Matrix<T, M, N> &other) const |
||||
{ |
||||
Matrix<T, M, N> res; |
||||
const Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
if (self(i , j) != other(i, j)) { |
||||
return false; |
||||
} |
||||
} |
||||
} |
||||
|
||||
return true; |
||||
} |
||||
|
||||
Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const |
||||
{ |
||||
Matrix<T, M, N> res; |
||||
const Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
res(i , j) = self(i, j) - other(i, j); |
||||
} |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
void operator+=(const Matrix<T, M, N> &other) |
||||
{ |
||||
Matrix<T, M, N> &self = *this; |
||||
self = self + other; |
||||
} |
||||
|
||||
void operator-=(const Matrix<T, M, N> &other) |
||||
{ |
||||
Matrix<T, M, N> &self = *this; |
||||
self = self - other; |
||||
} |
||||
|
||||
void operator*=(const Matrix<T, M, N> &other) |
||||
{ |
||||
Matrix<T, M, N> &self = *this; |
||||
self = self * other; |
||||
} |
||||
|
||||
/**
|
||||
* Scalar Operations |
||||
*/ |
||||
|
||||
Matrix<T, M, N> operator*(T scalar) const |
||||
{ |
||||
Matrix<T, M, N> res; |
||||
const Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
res(i , j) = self(i, j) * scalar; |
||||
} |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
Matrix<T, M, N> operator+(T scalar) const |
||||
{ |
||||
Matrix<T, M, N> res; |
||||
Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
res(i , j) = self(i, j) + scalar; |
||||
} |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
void operator*=(T scalar) |
||||
{ |
||||
Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
self(i, j) = self(i, j) * scalar; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void operator/=(T scalar) |
||||
{ |
||||
Matrix<T, M, N> &self = *this; |
||||
self = self * (1.0f / scalar); |
||||
} |
||||
|
||||
/**
|
||||
* Misc. Functions |
||||
*/ |
||||
|
||||
void print() |
||||
{ |
||||
Matrix<T, M, N> &self = *this; |
||||
printf("\n"); |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
printf("["); |
||||
|
||||
for (size_t j = 0; j < N; j++) { |
||||
printf("%10g\t", double(self(i, j))); |
||||
} |
||||
|
||||
printf("]\n"); |
||||
} |
||||
} |
||||
|
||||
Matrix<T, N, M> transpose() const |
||||
{ |
||||
Matrix<T, N, M> res; |
||||
const Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
for (size_t j = 0; j < N; j++) { |
||||
res(j, i) = self(i, j); |
||||
} |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
Matrix<T, M, M> expm(float dt, size_t n) const |
||||
{ |
||||
Matrix<float, M, M> res; |
||||
res.setIdentity(); |
||||
Matrix<float, M, M> A_pow = *this; |
||||
float k_fact = 1; |
||||
size_t k = 1; |
||||
|
||||
while (k < n) { |
||||
res += A_pow * (float(pow(dt, k)) / k_fact); |
||||
|
||||
if (k == n) { break; } |
||||
|
||||
A_pow *= A_pow; |
||||
k_fact *= k; |
||||
k++; |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
Matrix<T, M, 1> diagonal() const |
||||
{ |
||||
Matrix<T, M, 1> res; |
||||
// force square for now
|
||||
const Matrix<T, M, M> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M; i++) { |
||||
res(i) = self(i, i); |
||||
} |
||||
|
||||
return res; |
||||
} |
||||
|
||||
void setZero() |
||||
{ |
||||
memset(_data, 0, sizeof(_data)); |
||||
} |
||||
|
||||
void setIdentity() |
||||
{ |
||||
setZero(); |
||||
Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < M and i < N; i++) { |
||||
self(i, i) = 1; |
||||
} |
||||
} |
||||
|
||||
inline void swapRows(size_t a, size_t b) |
||||
{ |
||||
if (a == b) { return; } |
||||
|
||||
Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t j = 0; j < cols(); j++) { |
||||
T tmp = self(a, j); |
||||
self(a, j) = self(b, j); |
||||
self(b, j) = tmp; |
||||
} |
||||
} |
||||
|
||||
inline void swapCols(size_t a, size_t b) |
||||
{ |
||||
if (a == b) { return; } |
||||
|
||||
Matrix<T, M, N> &self = *this; |
||||
|
||||
for (size_t i = 0; i < rows(); i++) { |
||||
T tmp = self(i, a); |
||||
self(i, a) = self(i, b); |
||||
self(i, b) = tmp; |
||||
} |
||||
} |
||||
|
||||
/**
|
||||
* inverse based on LU factorization with partial pivotting |
||||
*/ |
||||
Matrix <T, M, M> inverse() const |
||||
{ |
||||
Matrix<T, M, M> L; |
||||
L.setIdentity(); |
||||
const Matrix<T, M, M> &A = (*this); |
||||
Matrix<T, M, M> U = A; |
||||
Matrix<T, M, M> P; |
||||
P.setIdentity(); |
||||
|
||||
//printf("A:\n"); A.print();
|
||||
|
||||
// for all diagonal elements
|
||||
for (size_t n = 0; n < N; n++) { |
||||
|
||||
// if diagonal is zero, swap with row below
|
||||
if (fabsf(U(n, n)) < 1e-8f) { |
||||
//printf("trying pivot for row %d\n",n);
|
||||
for (size_t i = 0; i < N; i++) { |
||||
if (i == n) { continue; } |
||||
|
||||
//printf("\ttrying row %d\n",i);
|
||||
if (fabsf(U(i, n)) > 1e-8f) { |
||||
//printf("swapped %d\n",i);
|
||||
U.swapRows(i, n); |
||||
P.swapRows(i, n); |
||||
} |
||||
} |
||||
} |
||||
|
||||
#ifdef MATRIX_ASSERT |
||||
//printf("A:\n"); A.print();
|
||||
//printf("U:\n"); U.print();
|
||||
//printf("P:\n"); P.print();
|
||||
//fflush(stdout);
|
||||
ASSERT(fabsf(U(n, n)) > 1e-8f); |
||||
#endif |
||||
|
||||
// failsafe, return zero matrix
|
||||
if (fabsf(U(n, n)) < 1e-8f) { |
||||
Matrix<T, M, M> zero; |
||||
zero.setZero(); |
||||
return zero; |
||||
} |
||||
|
||||
// for all rows below diagonal
|
||||
for (size_t i = (n + 1); i < N; i++) { |
||||
L(i, n) = U(i, n) / U(n, n); |
||||
|
||||
// add i-th row and n-th row
|
||||
// multiplied by: -a(i,n)/a(n,n)
|
||||
for (size_t k = n; k < N; k++) { |
||||
U(i, k) -= L(i, n) * U(n, k); |
||||
} |
||||
} |
||||
} |
||||
|
||||
//printf("L:\n"); L.print();
|
||||
//printf("U:\n"); U.print();
|
||||
|
||||
// solve LY=P*I for Y by forward subst
|
||||
Matrix<T, M, M> Y = P; |
||||
|
||||
// for all columns of Y
|
||||
for (size_t c = 0; c < N; c++) { |
||||
// for all rows of L
|
||||
for (size_t i = 0; i < N; i++) { |
||||
// for all columns of L
|
||||
for (size_t j = 0; j < i; j++) { |
||||
// for all existing y
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
Y(i, c) -= L(i, j) * Y(j, c); |
||||
} |
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
// Y(i,c) /= L(i,i);
|
||||
// but L(i,i) = 1.0
|
||||
} |
||||
} |
||||
|
||||
//printf("Y:\n"); Y.print();
|
||||
|
||||
// solve Ux=y for x by back subst
|
||||
Matrix<T, M, M> X = Y; |
||||
|
||||
// for all columns of X
|
||||
for (size_t c = 0; c < N; c++) { |
||||
// for all rows of U
|
||||
for (size_t k = 0; k < N; k++) { |
||||
// have to go in reverse order
|
||||
size_t i = N - 1 - k; |
||||
|
||||
// for all columns of U
|
||||
for (size_t j = i + 1; j < N; j++) { |
||||
// for all existing x
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
X(i, c) -= U(i, j) * X(j, c); |
||||
} |
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
X(i, c) /= U(i, i); |
||||
} |
||||
} |
||||
|
||||
//printf("X:\n"); X.print();
|
||||
return X; |
||||
} |
||||
|
||||
}; |
||||
|
||||
typedef Matrix<float, 2, 1> Vector2f; |
||||
typedef Matrix<float, 3, 1> Vector3f; |
||||
typedef Matrix<float, 3, 3> Matrix3f; |
||||
|
||||
}; // namespace matrix
|
||||
@ -0,0 +1,15 @@
@@ -0,0 +1,15 @@
|
||||
set(tests |
||||
setIdentity |
||||
inverse |
||||
matrixMult |
||||
vectorAssignment |
||||
matrixAssignment |
||||
matrixScalarMult |
||||
transpose |
||||
) |
||||
|
||||
foreach(test ${tests}) |
||||
add_executable(${test} |
||||
${test}.cpp) |
||||
add_test(${test} ${test}) |
||||
endforeach() |
||||
@ -0,0 +1,30 @@
@@ -0,0 +1,30 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
#include <stdio.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; |
||||
Matrix3f A(data); |
||||
Matrix3f A_I = A.inverse(); |
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; |
||||
Matrix3f A_I_check(data_check); |
||||
(void)A_I; |
||||
assert(A_I == A_I_check); |
||||
|
||||
// stess test
|
||||
static const size_t n = 100; |
||||
Matrix<float, n, n> A_large; |
||||
A_large.setIdentity(); |
||||
Matrix<float, n, n> A_large_I; |
||||
A_large_I.setZero(); |
||||
|
||||
for (size_t i = 0; i < 100; i++) { |
||||
A_large_I = A_large.inverse(); |
||||
assert(A_large == A_large_I); |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1,29 @@
@@ -0,0 +1,29 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
Matrix3f m; |
||||
m.setZero(); |
||||
m(0, 0) = 1; |
||||
m(0, 1) = 2; |
||||
m(0, 2) = 3; |
||||
m(1, 0) = 4; |
||||
m(1, 1) = 5; |
||||
m(1, 2) = 6; |
||||
m(2, 0) = 7; |
||||
m(2, 1) = 8; |
||||
m(2, 2) = 9; |
||||
|
||||
m.print(); |
||||
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; |
||||
Matrix3f m2(data); |
||||
m2.print(); |
||||
|
||||
assert(m == m2); |
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1,26 @@
@@ -0,0 +1,26 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
#include <stdio.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; |
||||
Matrix3f A(data); |
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; |
||||
Matrix3f A_I(data_check); |
||||
Matrix3f I; |
||||
I.setIdentity(); |
||||
A.print(); |
||||
A_I.print(); |
||||
Matrix3f R = A * A_I; |
||||
R.print(); |
||||
assert(R == I); |
||||
|
||||
Matrix3f R2 = A; |
||||
R2 *= A_I; |
||||
R2.print(); |
||||
assert(R2 == I); |
||||
return 0; |
||||
} |
||||
@ -0,0 +1,18 @@
@@ -0,0 +1,18 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
#include <stdio.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; |
||||
Matrix3f A(data); |
||||
A = A * 2; |
||||
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; |
||||
Matrix3f A_check(data_check); |
||||
A.print(); |
||||
A_check.print(); |
||||
assert(A == A_check); |
||||
return 0; |
||||
} |
||||
@ -0,0 +1,25 @@
@@ -0,0 +1,25 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
Matrix3f A; |
||||
A.setIdentity(); |
||||
assert(A.rows() == 3); |
||||
assert(A.cols() == 3); |
||||
|
||||
for (int i = 0; i < 3; i++) { |
||||
for (int j = 0; j < 3; j++) { |
||||
if (i == j) { |
||||
assert(A(i, j) == 1); |
||||
|
||||
} else { |
||||
assert(A(i, j) == 0); |
||||
} |
||||
} |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1,18 @@
@@ -0,0 +1,18 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
#include <stdio.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
float data[9] = {1, 2, 3, 4, 5, 6}; |
||||
Matrix<float, 2, 3> A(data); |
||||
Matrix<float, 3, 2> A_T = A.transpose(); |
||||
float data_check[9] = {1, 4, 2, 5, 3, 6}; |
||||
Matrix<float, 3, 2> A_T_check(data_check); |
||||
A_T.print(); |
||||
A_T_check.print(); |
||||
assert(A_T == A_T_check); |
||||
return 0; |
||||
} |
||||
@ -0,0 +1,28 @@
@@ -0,0 +1,28 @@
|
||||
#include "Matrix.hpp" |
||||
#include <assert.h> |
||||
|
||||
using namespace matrix; |
||||
|
||||
int main() |
||||
{ |
||||
Vector3f v; |
||||
v(0) = 1; |
||||
v(1) = 2; |
||||
v(2) = 3; |
||||
|
||||
v.print(); |
||||
|
||||
assert(v(0) == 1); |
||||
assert(v(1) == 2); |
||||
assert(v(2) == 3); |
||||
|
||||
Vector3f v2(4, 5, 6); |
||||
|
||||
v2.print(); |
||||
|
||||
assert(v2(0) == 4); |
||||
assert(v2(1) == 5); |
||||
assert(v2(2) == 6); |
||||
|
||||
return 0; |
||||
} |
||||
@ -0,0 +1,222 @@
@@ -0,0 +1,222 @@
|
||||
#include <systemlib/param/param.h> |
||||
|
||||
// 16 is max name length
|
||||
|
||||
|
||||
/**
|
||||
* Enable local position estimator. |
||||
* |
||||
* @group Local Position Estimator |
||||
*/ |
||||
PARAM_DEFINE_INT32(LPE_ENABLED, 1); |
||||
|
||||
/**
|
||||
* Enable accelerometer integration for prediction. |
||||
* |
||||
* @group Local Position Estimator |
||||
*/ |
||||
PARAM_DEFINE_INT32(LPE_INTEGRATE, 1); |
||||
|
||||
/**
|
||||
* Optical flow xy standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f); |
||||
|
||||
/**
|
||||
* Sonar z standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f); |
||||
|
||||
/**
|
||||
* Lidar z standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); |
||||
|
||||
/**
|
||||
* Accelerometer xy standard deviation |
||||
* |
||||
* Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) |
||||
* std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 |
||||
* Since accels sampled at 1000 Hz. |
||||
* |
||||
* should be 0.0464 |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m/s^2 |
||||
* @min 0.00001 |
||||
* @max 2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); |
||||
|
||||
/**
|
||||
* Accelerometer z standard deviation |
||||
* |
||||
* (see Accel x comments) |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m/s^2 |
||||
* @min 0.00001 |
||||
* @max 2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); |
||||
|
||||
/**
|
||||
* Barometric presssure altitude z standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 3 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); |
||||
|
||||
/**
|
||||
* GPS xy standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 5 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); |
||||
|
||||
/**
|
||||
* GPS z standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 20 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); |
||||
|
||||
/**
|
||||
* GPS xy velocity standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m/s |
||||
* @min 0.01 |
||||
* @max 2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f); |
||||
|
||||
/**
|
||||
* GPS z velocity standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m/s |
||||
* @min 0.01 |
||||
* @max 2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f); |
||||
|
||||
/**
|
||||
* GPS max eph |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 1.0 |
||||
* @max 5.0 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); |
||||
|
||||
|
||||
|
||||
/**
|
||||
* Vision xy standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); |
||||
|
||||
/**
|
||||
* Vision z standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); |
||||
|
||||
/**
|
||||
* Circuit breaker to disable vision input. |
||||
* |
||||
* Set to the appropriate key (328754) to disable vision input. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @min 0 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_INT32(LPE_NO_VISION, 0); |
||||
|
||||
/**
|
||||
* Vicon position standard deviation. |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit m |
||||
* @min 0.01 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); |
||||
|
||||
/**
|
||||
* Position propagation process noise power (variance*sampling rate). |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit (m/s^2)-s |
||||
* @min 0 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); |
||||
|
||||
/**
|
||||
* Velocity propagation process noise power (variance*sampling rate). |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit (m/s)-s |
||||
* @min 0 |
||||
* @max 5 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); |
||||
|
||||
/**
|
||||
* Accel bias propagation process noise power (variance*sampling rate). |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit (m/s)-s |
||||
* @min 0 |
||||
* @max 1 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f); |
||||
|
||||
/**
|
||||
* Fault detection threshold, for chi-squared dist. |
||||
* |
||||
* TODO add separate params for 1 dof, 3 dof, and 6 dof beta |
||||
* or false alarm rate in false alarms/hr |
||||
* |
||||
* @group Local Position Estimator |
||||
* @unit |
||||
* @min 3 |
||||
* @max 1000 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f); |
||||
Loading…
Reference in new issue