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

Merge upstream and resolve merge conflicts.

Browse Source
This commit is contained in:
mcsauder
2016-02-23 19:29:30 -07:00
parent 4ce4724105 237ecfef1a
commit fad1c87631
26 changed files with 1867 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -1,2 +1,3 @@
*.DS_Store
Build/
*~
.gitmodules
+3
View File
@@ -0,0 +1,3 @@
[submodule "matrix"]
path = matrix
url = https://github.com/PX4/Matrix
.travis.yml
+16
View File
@@ -0,0 +1,16 @@
language: cpp
sudo: required
compiler: g++
before_install:
- sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test
- sudo apt-get update -qq
install:
- sudo apt-get install -qq g++-4.8
- export CXX="g++-4.8"
before_script:
- chmod +x build.sh
script: ./build.sh
EKF/CMakeLists.txt
+72
View File
@@ -0,0 +1,72 @@
############################################################################
#
# Copyright (c) 2015 ECL 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 ECL 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.
#
############################################################################
cmake_minimum_required(VERSION 2.8)
project (ECL CXX)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CURRENT_SOURCE_DIR ./)
set(CMAKE_CXX_FLAGS "-DPOSIX_SHARED")
set (EIGEN3_INCLUDE_DIR "/usr/local/include/eigen3/")
IF( NOT EIGEN3_INCLUDE_DIR )
MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.")
ENDIF()
INCLUDE_DIRECTORIES ( "${EIGEN3_INCLUDE_DIR}" )
if( NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/../matrix/.git" )
message( SEND_ERROR "The git submodules are not available. Please run
git submodule update --init --recursive"
)
endif()
include_directories(
./
../
../matrix
EIGEN3_INCLUDE_DIR
)
set(SRCS
estimator_interface.cpp
ekf.cpp
ekf_helper.cpp
covariance.cpp
vel_pos_fusion.cpp
mag_fusion.cpp
gps_checks.cpp
control.cpp
geo.cpp
mathlib.cpp
)
add_definitions(-std=c++11)
add_library(ecl SHARED ${SRCS})
EKF/common.h
+10 -8
View File
@@ -212,8 +212,9 @@ struct parameters {
// Integer definitions for mag_fusion_type
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_HEADING 1 // Magnetic heading fusion will always be used. This is less accurate, but less affected by earth field distortions
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localized earth field distortions
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
struct stateSample {
Vector3f ang_error; // attitude axis angle error (error state formulation)
@@ -264,12 +265,13 @@ union filter_control_status_u {
uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete
uint8_t gps : 1; // 2 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic heading is being fused
uint8_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 7 - true when the vehicle is airborne
uint8_t armed : 1; // 8 - true when the vehicle motors are armed
uint8_t wind : 1; // 9 - true when wind velocity is being estimated
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused
uint8_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
uint8_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 8 - true when the vehicle is airborne
uint8_t armed : 1; // 9 - true when the vehicle motors are armed
uint8_t wind : 1; // 10 - true when wind velocity is being estimated
} flags;
uint16_t value;
};
EKF/control.cpp
+26 -7
View File
@@ -103,36 +103,55 @@ void Ekf::controlFusionModes()
// or the more accurate 3-axis fusion
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
if (!_control_status.flags.armed) {
// always use simple mag fusion for initial startup
_control_status.flags.mag_hdg = true;
// always use 2D mag fusion for initial startup
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
} else {
if (_control_status.flags.in_air) {
// always use 3-axis mag fusion when airborne
// always use 3D mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = true;
} else {
// always use simple heading fusion when on the ground
_control_status.flags.mag_hdg = true;
// always use 2D mag fusion when on the ground
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
}
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use simple heading fusion
_control_status.flags.mag_hdg = true;
// always use yaw fusion unless tilt is over 45 deg, otherwise use 2D fusion
if (_R_prev(2, 2) > 0.7071f) {
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_2D = false;
} else {
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
}
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {
// always use 2D mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = true;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// always use 3-axis mag fusion
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = true;
} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_2D = false;
_control_status.flags.mag_3D = false;
}
EKF/covariance.cpp
+1 -1
View File
@@ -42,7 +42,7 @@
#include "ekf.h"
#include <math.h>
#include <mathlib/mathlib.h>
#include "mathlib.h"
void Ekf::initialiseCovariance()
{
EKF/ekf.cpp
+3
View File
@@ -168,6 +168,9 @@ bool Ekf::update()
fuseDeclination();
}
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
fuseMag2D();
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
fuseHeading();
EKF/ekf.h
+5 -1
View File
@@ -41,6 +41,7 @@
*/
#include "estimator_interface.h"
#include "geo.h"
class Ekf : public EstimatorInterface
{
@@ -183,9 +184,12 @@ private:
// ekf sequential fusion of magnetometer measurements
void fuseMag();
// fuse magnetometer heading measurement
// fuse heading measurement (currently uses estimate from magnetometer)
void fuseHeading();
// fuse projecton of magnetometer onto horizontal plane
void fuseMag2D();
// fuse magnetometer declination measurement
void fuseDeclination();
EKF/ekf_helper.cpp
+1 -1
View File
@@ -45,7 +45,7 @@
#include <fstream>
#endif
#include <iomanip>
#include <mathlib/mathlib.h>
#include "mathlib.h"
// Reset the velocity states. If we have a recent and valid
// gps measurement then use for velocity initialisation
EKF/estimator_interface.cpp
+2 -2
View File
@@ -44,7 +44,7 @@
#include <inttypes.h>
#include <math.h>
#include "estimator_interface.h"
#include <mathlib/mathlib.h>
#include "mathlib.h"
EstimatorInterface::EstimatorInterface():
@@ -237,7 +237,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
_flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
_output_buffer.allocate(IMU_BUFFER_LENGTH))) {
// PX4_WARN("Estimator Buffer Allocation failed!");
printf("Estimator Buffer Allocation failed!");
unallocate_buffers();
return false;
}
EKF/estimator_interface.h
+1 -2
View File
@@ -41,9 +41,8 @@
#include <stdint.h>
#include <matrix/matrix/math.hpp>
#include <geo/geo.h>
#include "RingBuffer.h"
#include "geo.h"
#include "common.h"
using namespace estimator;
EKF/geo.cpp
+827
View File
@@ -0,0 +1,827 @@
/****************************************************************************
*
* Copyright (c) 2012-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 geo.c
*
* Geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifdef POSIX_SHARED
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
/****************************************************************************
*
* Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c
*
* Calculation / lookup table for earth magnetic field declination.
*
* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
*
* XXX Lookup table currently too coarse in resolution (only full degrees)
* and lat/lon res - needs extension medium term.
*
*/
#include "geo.h"
/** set this always to the sampling in degrees for the table below */
#define SAMPLING_RES 10.0f
#define SAMPLING_MIN_LAT -60.0f
#define SAMPLING_MAX_LAT 60.0f
#define SAMPLING_MIN_LON -180.0f
#define SAMPLING_MAX_LON 180.0f
static const int8_t declination_table[13][37] = \
{
{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
{ 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
{ 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
{ 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
{ 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
{ 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
{ 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
{ 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
{ 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
{ 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
{ 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
};
static float get_lookup_table_val(unsigned lat, unsigned lon);
float get_mag_declination(float lat, float lon)
{
/*
* If the values exceed valid ranges, return zero as default
* as we have no way of knowing what the closest real value
* would be.
*/
if (lat < -90.0f || lat > 90.0f ||
lon < -180.0f || lon > 180.0f) {
return 0.0f;
}
/* round down to nearest sampling resolution */
int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
/* for the rare case of hitting the bounds exactly
* the rounding logic wouldn't fit, so enforce it.
*/
/* limit to table bounds - required for maxima even when table spans full globe range */
if (lat <= SAMPLING_MIN_LAT) {
min_lat = SAMPLING_MIN_LAT;
}
if (lat >= SAMPLING_MAX_LAT) {
min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
}
if (lon <= SAMPLING_MIN_LON) {
min_lon = SAMPLING_MIN_LON;
}
if (lon >= SAMPLING_MAX_LON) {
min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
}
/* find index of nearest low sampling point */
unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES;
unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
/* perform bilinear interpolation on the four grid corners */
float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
}
float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
{
return declination_table[lat_index][lon_index];
}
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
bool map_projection_global_initialized()
{
return map_projection_initialized(&mp_ref);
}
bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
return ref->init_done;
}
uint64_t map_projection_global_timestamp()
{
return map_projection_timestamp(&mp_ref);
}
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
{
return ref->timestamp;
}
int map_projection_global_init(double lat_0, double lon_0,
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
}
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat_rad = lat_0 * M_DEG_TO_RAD;
ref->lon_rad = lon_0 * M_DEG_TO_RAD;
ref->sin_lat = sin(ref->lat_rad);
ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
return 0;
}
int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
{
return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
}
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
int map_projection_global_project(double lat, double lon, float *x, float *y)
{
return map_projection_project(&mp_ref, lat, lon, x, y);
}
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double lat_rad = lat * M_DEG_TO_RAD;
double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon;
if (arg > 1.0) {
arg = 1.0;
} else if (arg < -1.0) {
arg = -1.0;
}
double c = acos(arg);
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
int map_projection_global_reproject(float x, float y, double *lat, double *lon)
{
return map_projection_reproject(&mp_ref, x, y, lat, lon);
}
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_rad;
double lon_rad;
if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lat_rad = ref->lat_rad;
lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
return 0;
}
int map_projection_global_getref(double *lat_0, double *lon_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (lat_0 != NULL) {
*lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
}
if (lon_0 != NULL) {
*lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
}
return 0;
}
int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
{
gl_ref.alt = alt_0;
if (!map_projection_global_init(lat_0, lon_0, timestamp)) {
gl_ref.init_done = true;
return 0;
} else {
gl_ref.init_done = false;
return -1;
}
}
bool globallocalconverter_initialized()
{
return gl_ref.init_done && map_projection_global_initialized();
}
int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_project(lat, lon, x, y);
*z = gl_ref.alt - alt;
return 0;
}
int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_reproject(x, y, lat, lon);
*alt = gl_ref.alt - z;
return 0;
}
int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (map_projection_global_getref(lat_0, lon_0)) {
return -1;
}
if (alt_0 != NULL) {
*alt_0 = gl_ref.alt;
}
return 0;
}
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now / (double)180.0 * M_PI;
double lon_now_rad = lon_now / (double)180.0 * M_PI;
double lat_next_rad = lat_next / (double)180.0 * M_PI;
double lon_next_rad = lon_next / (double)180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon /
(double)2.0) * cos(lat_now_rad) * cos(lat_next_rad);
double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a));
return CONSTANTS_RADIUS_OF_EARTH * c;
}
void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target)
{
if (fabsf(dist) < FLT_EPSILON) {
*lat_target = lat_A;
*lon_target = lon_A;
} else if (dist >= FLT_EPSILON) {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
} else {
float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
heading = _wrap_2pi(heading + M_PI_F);
waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
}
}
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target)
{
bearing = _wrap_2pi(bearing);
double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
double lat_start_rad = lat_start * M_DEG_TO_RAD;
double lon_start_rad = lon_start * M_DEG_TO_RAD;
*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
double)bearing));
*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
*lat_target *= M_RAD_TO_DEG;
*lon_target *= M_RAD_TO_DEG;
}
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) ,
cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
theta = _wrap_pi(theta);
return theta;
}
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(
d_lon));
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
}
void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next,
float *v_n, float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
}
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
double *lon_res)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
// headed towards the end point.
float dist_to_end;
float bearing_end;
float bearing_track;
float bearing_diff;
int return_value = ERROR; // Set error flag, cleared when valid result calculated.
crosstrack_error->past_end = false;
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
// Return error if arguments are bad
if (dist_to_end < 0.1f) {
return ERROR;
}
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
bearing_diff = bearing_track - bearing_end;
bearing_diff = _wrap_pi(bearing_diff);
// Return past_end = true if past end point of line
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
crosstrack_error->past_end = true;
return_value = OK;
return return_value;
}
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
} else {
crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
}
return_value = OK;
return return_value;
}
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
// headed towards the end point.
// Determine if the current position is inside or outside the sector between the line from the center
// to the arc start and the line from the center to the arc end
float bearing_sector_start;
float bearing_sector_end;
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
bool in_sector;
int return_value = ERROR; // Set error flag, cleared when valid result calculated.
crosstrack_error->past_end = false;
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (radius < 0.1f) { return return_value; }
if (arc_sweep >= 0.0f) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
} else {
bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep;
if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
}
in_sector = false;
// Case where sector does not span zero
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start
&& bearing_now <= bearing_sector_end) { in_sector = true; }
// Case where sector does span zero
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start
|| bearing_now < bearing_sector_end)) { in_sector = true; }
// If in the sector then calculate distance and bearing to closest point
if (in_sector) {
crosstrack_error->past_end = false;
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
if (dist_to_center <= radius) {
crosstrack_error->distance = radius - dist_to_center;
crosstrack_error->bearing = bearing_now + M_PI_F;
} else {
crosstrack_error->distance = dist_to_center - radius;
crosstrack_error->bearing = bearing_now;
}
// If out of the sector then calculate dist and bearing to start or end point
} else {
// Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
// calculate the position of the start and end points. We should not be doing this often
// as this function generally will not be called repeatedly when we are out of the sector.
double start_disp_x = (double)radius * sin(arc_start_bearing);
double start_disp_y = (double)radius * cos(arc_start_bearing);
double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double lon_start = lon_now + start_disp_x / 111111.0;
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
double lon_end = lon_now + end_disp_x / 111111.0;
double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
if (dist_to_start < dist_to_end) {
crosstrack_error->distance = dist_to_start;
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
} else {
crosstrack_error->past_end = true;
crosstrack_error->distance = dist_to_end;
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
}
}
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
return_value = OK;
return return_value;
}
float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z)
{
double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI;
double x_rad = lat_now / 180.0 * M_PI;
double y_rad = lon_now / 180.0 * M_PI;
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
float dz = alt_now - alt_next;
*dist_xy = fabsf(dxy);
*dist_z = fabsf(dz);
return sqrtf(dxy * dxy + dz * dz);
}
float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z)
{
float dx = x_now - x_next;
float dy = y_now - y_next;
float dz = z_now - z_next;
*dist_xy = sqrtf(dx * dx + dy * dy);
*dist_z = fabsf(dz);
return sqrtf(dx * dx + dy * dy + dz * dz);
}
float _wrap_pi(float bearing)
{
/* value is inf or NaN */
if (!math::isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
float _wrap_2pi(float bearing)
{
/* value is inf or NaN */
if (!math::isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
float _wrap_180(float bearing)
{
/* value is inf or NaN */
if (!math::isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 180.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < -180.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
float _wrap_360(float bearing)
{
/* value is inf or NaN */
if (!math::isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing >= 360.0f) {
bearing -= 360.0f;
if (c++ > 3) {
return NAN;
}
}
c = 0;
while (bearing < 0.0f) {
bearing += 360.0f;
if (c++ > 3) {
return NAN;
}
}
return bearing;
}
#endif //POSIX_SHARED
EKF/geo.h
+310
View File
@@ -0,0 +1,310 @@
/****************************************************************************
*
* Copyright (C) 2012, 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 geo.h
*
* Definition of geo / math functions to perform geodesic calculations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
#ifndef GEO_H
#define GEO_H
#ifdef POSIX_SHARED
#include <stdbool.h>
#include "mathlib.h"
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */
#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
#define M_TWOPI_F 6.28318530717958647692f
#define M_PI_2_F 1.57079632679489661923f
#define M_RAD_TO_DEG 0.01745329251994329576f
#define M_DEG_TO_RAD 57.29577951308232087679f
#define OK 0
#define ERROR -1
// XXX remove
struct crosstrack_error_s {
bool past_end; // Flag indicating we are past the end of the line/arc segment
float distance; // Distance in meters to closest point on line/arc
float bearing; // Bearing in radians to closest point on line/arc
} ;
/* lat/lon are in radians */
struct map_projection_reference_s {
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
uint64_t timestamp;
};
struct globallocal_converter_reference_s {
float alt;
bool init_done;
};
/**
* Checks if global projection was initialized
* @return true if map was initialized before, false else
*/
bool map_projection_global_initialized(void);
/**
* Checks if projection given as argument was initialized
* @return true if map was initialized before, false else
*/
bool map_projection_initialized(const struct map_projection_reference_s *ref);
/**
* Get the timestamp of the global map projection
* @return the timestamp of the map_projection
*/
uint64_t map_projection_global_timestamp(void);
/**
* Get the timestamp of the map projection given by the argument
* @return the timestamp of the map_projection
*/
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
/**
* Writes the reference values of the global projection to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
/**
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad,
double *ref_lon_rad);
/**
* Initializes the map transformation given by the argument.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
int map_projection_init_timestamped(struct map_projection_reference_s *ref,
double lat_0, double lon_0, uint64_t timestamp);
/**
* Initializes the map transformation given by the argument and sets the timestamp to now.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the global projection
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_global_project(double lat, double lon, float *x, float *y);
/* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the projection given by the argument
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the global projection
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_global_reproject(float x, float y, double *lat, double *lon);
/**
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the projection given by the argument
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon);
/**
* Get reference position of the global map projection
*/
int map_projection_global_getref(double *lat_0, double *lon_0);
/**
* Initialize the global mapping between global position (spherical) and local position (NED).
*/
int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
/**
* Checks if globallocalconverter was initialized
* @return true if map was initialized before, false else
*/
bool globallocalconverter_initialized(void);
/**
* Convert from global position coordinates to local position coordinates using the global reference
*/
int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
/**
* Convert from local position coordinates to global position coordinates using the global reference
*/
int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
/**
* Get reference position of the global to local converter
*/
int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
/**
* Returns the distance to the next waypoint in meters.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
/**
* Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
* from waypoint A
*
* @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
* @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
* @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
* @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
* @param dist distance of target waypoint from waypoint A in meters (can be negative)
* @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
*/
void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
double *lat_target, double *lon_target);
/**
* Creates a waypoint from given waypoint, distance and bearing
* see http://www.movable-type.co.uk/scripts/latlong.html
*
* @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
* @param bearing in rad
* @param distance in meters
* @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
* @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
*/
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
double *lat_target, double *lon_target);
/**
* Returns the bearing to the next waypoint in radians.
*
* @param lat_now current position in degrees (47.1234567°, not 471234567°)
* @param lon_now current position in degrees (8.1234567°, not 81234567°)
* @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
* @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
*/
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n,
float *v_e);
void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next,
float *v_n, float *v_e);
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res,
double *lon_res);
int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_start, double lon_start, double lat_end, double lon_end);
int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep);
/*
* Calculate distance in global frame
*/
float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
double lat_next, double lon_next, float alt_next,
float *dist_xy, float *dist_z);
/*
* Calculate distance in local frame (NED)
*/
float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
float x_next, float y_next, float z_next,
float *dist_xy, float *dist_z);
float _wrap_180(float bearing);
float _wrap_360(float bearing);
float _wrap_pi(float bearing);
float _wrap_2pi(float bearing);
float get_mag_declination(float lat, float lon);
#else
#include <lib/geo/geo.h>
#endif //POSIX_SHARED
#endif //GEO_H
EKF/gps_checks.cpp
+5 -5
View File
@@ -40,8 +40,8 @@
*/
#include "ekf.h"
#include <mathlib/mathlib.h>
#include "mathlib.h"
#include "geo.h"
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_GDOP (1<<1)
@@ -63,7 +63,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
// Initialise projection
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init(&_pos_ref, lat, lon);
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
_NED_origin_initialised = true;
@@ -121,7 +121,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE);
} else {
map_projection_init(&_pos_ref, lat, lon);
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
_gps_alt_ref = gps->alt * 1e-3f;
}
@@ -150,7 +150,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
}
// Save current position as the reference for next time
map_projection_init(&_pos_ref, lat, lon);
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
_last_gps_origin_time_us = _time_last_imu;
// Calculate the vertical drift velocity and limit to 10x the threshold
EKF/mag_fusion.cpp
+280 -66
View File
@@ -40,7 +40,7 @@
*
*/
#include "ekf.h"
#include <mathlib/mathlib.h>
#include "mathlib.h"
void Ekf::fuseMag()
{
@@ -495,7 +495,7 @@ void Ekf::fuseHeading()
float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
R_YAW = R_YAW * R_YAW;
// calculate intermediate variables for observation jacobian
// calculate intermediate variables for observation jacobians
float t2 = q0 * q0;
float t3 = q1 * q1;
float t4 = q2 * q2;
@@ -509,7 +509,7 @@ void Ekf::fuseHeading()
if (fabsf(t6) > 1e-6f) {
t10 = 1.0f / (t6 * t6);
} else {
} else {
return;
}
@@ -527,36 +527,39 @@ void Ekf::fuseHeading()
float t15 = 1.0f / t6;
// calculate observation jacobian
float H_YAW[3] = {};
H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f));
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));
H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8)); // calculate observation jacobian
// calculate innovation
// rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement
// TODO - enable use of an off-board heading measurement
matrix::Dcm<float> R_to_earth(_state.quat_nominal);
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
// calculate intermediate expressions for Kalman gains
float t16 = q0 * q1 * 2.0f;
float t29 = q2 * q3 * 2.0f;
float t17 = t16 - t29;
float t18 = t15 * t17;
float t19 = q0 * q2 * 2.0f;
float t20 = q1 * q3 * 2.0f;
float t21 = t19 + t20;
float t22 = t9 * t10 * t21;
float t23 = t18 + t22;
float t40 = t14 * t23;
float t24 = t2 - t3 + t4 - t5;
float t25 = t15 * t24;
float t26 = t7 - t8;
float t27 = t9 * t10 * t26;
float t28 = t25 + t27;
float t41 = t14 * t28;
float t30 = P[1][1] * t40;
float t31 = P[1][2] * t40;
float t32 = P[2][2] * t41;
float t33 = t31 + t32;
float t34 = t41 * t33;
float t35 = P[2][1] * t41;
float t36 = t30 + t35;
float t37 = t40 * t36;
float t38 = R_YAW + t34 + t37; // Innovation variance
_heading_innov_var = t38;
// wrap the innovation to the interval between +-pi
innovation = matrix::wrap_pi(innovation);
_heading_innov = innovation;
// calculate innovation variance
float innovation_var = R_YAW;
_heading_innov_var = innovation_var;
float PH[3] = {};
for (unsigned row = 0; row < 3; row++) {
for (unsigned column = 0; column < 3; column++) {
PH[row] += P[row][column] * H_YAW[column];
}
innovation_var += H_YAW[row] * PH[row];
}
if (innovation_var >= R_YAW) {
if (t38 >= R_YAW) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_hdg = false;
@@ -569,44 +572,65 @@ void Ekf::fuseHeading()
return;
}
float innovation_var_inv = 1.0f / innovation_var;
float t39 = 1.0f / t38;
// calculate the kalman gains taking dvantage of the reduce size of H_YAW
float Kfusion[_k_num_states] = {};
// calculate Kalman gains
float Kfusion[24] = {};
Kfusion[0] = t39 * (P[0][1] * t40 + P[0][2] * t41);
Kfusion[1] = t39 * (t30 + P[1][2] * t41);
Kfusion[2] = t39 * (t32 + P[2][1] * t40);
Kfusion[3] = t39 * (P[3][1] * t40 + P[3][2] * t41);
Kfusion[4] = t39 * (P[4][1] * t40 + P[4][2] * t41);
Kfusion[5] = t39 * (P[5][1] * t40 + P[5][2] * t41);
Kfusion[6] = t39 * (P[6][1] * t40 + P[6][2] * t41);
Kfusion[7] = t39 * (P[7][1] * t40 + P[7][2] * t41);
Kfusion[8] = t39 * (P[8][1] * t40 + P[8][2] * t41);
Kfusion[9] = t39 * (P[9][1] * t40 + P[9][2] * t41);
Kfusion[10] = t39 * (P[10][1] * t40 + P[10][2] * t41);
Kfusion[11] = t39 * (P[11][1] * t40 + P[11][2] * t41);
Kfusion[12] = t39 * (P[12][1] * t40 + P[12][2] * t41);
Kfusion[13] = t39 * (P[13][1] * t40 + P[13][2] * t41);
Kfusion[14] = t39 * (P[14][1] * t40 + P[14][2] * t41);
Kfusion[15] = t39 * (P[15][1] * t40 + P[15][2] * t41);
// gains for states that are always used
for (unsigned row = 0; row <= 15; row++) {
for (unsigned column = 0; column < 3; column++) {
Kfusion[row] += P[row][column] * H_YAW[column];
}
/* we won't be using these states because we are doing heading fusion
Kfusion[16] = t39*(P[16][1]*t40+P[16][2]*t41);
Kfusion[17] = t39*(P[17][1]*t40+P[17][2]*t41);
Kfusion[18] = t39*(P[18][1]*t40+P[18][2]*t41);
Kfusion[19] = t39*(P[19][1]*t40+P[19][2]*t41);
Kfusion[20] = t39*(P[20][1]*t40+P[20][2]*t41);
Kfusion[21] = t39*(P[21][1]*t40+P[21][2]*t41);
*/
Kfusion[row] *= innovation_var_inv;
}
// only calculate gains for magnetic field states if they are being used
if (_control_status.flags.mag_3D) {
for (unsigned row = 16; row <= 21; row++) {
for (unsigned column = 0; column < 3; column++) {
Kfusion[row] += P[row][column] * H_YAW[column];
}
Kfusion[row] *= innovation_var_inv;
}
}
// only calculate gains for wind states if they are being used
// don't adjust these states if we are not using them
if (_control_status.flags.wind) {
for (unsigned row = 22; row <= 23; row++) {
for (unsigned column = 0; column < 3; column++) {
Kfusion[row] += P[row][column] * H_YAW[column];
}
Kfusion[row] *= innovation_var_inv;
}
Kfusion[22] = t39 * (P[22][1] * t40 + P[22][2] * t41);
Kfusion[23] = t39 * (P[23][1] * t40 + P[23][2] * t41);
}
// TODO - enable use of an off-board heading measurement
// rotate the magnetometer measurement into earth frame using an assumed zero yaw angle
matrix::Euler<float> euler(_state.quat_nominal);
float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation
euler(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler);
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// Use the difference between the horizontal projection and declination to give the measured heading
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
// wrap the heading to the interval between +-pi
measured_hdg = matrix::wrap_pi(measured_hdg);
// calculate the innovation
_heading_innov = predicted_hdg - measured_hdg;
// wrap the innovation to the interval between +-pi
_heading_innov = matrix::wrap_pi(_heading_innov);
// innovation test ratio
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
@@ -620,8 +644,8 @@ void Ekf::fuseHeading()
} else {
// constrain the innovation to the maximum set by the gate
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var));
innovation = math::constrain(innovation, -gate_limit, gate_limit);
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var));
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
}
} else {
@@ -630,7 +654,7 @@ void Ekf::fuseHeading()
// zero the attitude error states and use the kalman gain vector and innovation to update the states
_state.ang_error.setZero();
fuse(Kfusion, innovation);
fuse(Kfusion, _heading_innov);
// correct the nominal quaternion
Quaternion dq;
@@ -642,7 +666,7 @@ void Ekf::fuseHeading()
float HP[_k_num_states] = {};
for (unsigned column = 0; column < _k_num_states; column++) {
for (unsigned row = 0; row < 3; row++) {
for (unsigned row = 1; row <= 2; row++) {
HP[column] += H_YAW[row] * P[row][column];
}
}
@@ -776,8 +800,7 @@ void Ekf::fuseDeclination()
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[row][0] * P[0][column];
tmp += KH[row][16] * P[16][column];
float tmp = KH[row][16] * P[16][column];
tmp += KH[row][17] * P[17][column];
KHP[row][column] = tmp;
}
@@ -793,3 +816,194 @@ void Ekf::fuseDeclination()
makeSymmetrical();
limitCov();
}
void Ekf::fuseMag2D()
{
// assign intermediate state variables
float q0 = _state.quat_nominal(0);
float q1 = _state.quat_nominal(1);
float q2 = _state.quat_nominal(2);
float q3 = _state.quat_nominal(3);
float magX = _mag_sample_delayed.mag(0);
float magY = _mag_sample_delayed.mag(1);
float magZ = _mag_sample_delayed.mag(2);
float R_DECL = fmaxf(_params.mag_heading_noise, 1.0e-2f);
R_DECL = R_DECL * R_DECL;
// calculate intermediate variables for observation jacobian
float t2 = q0 * q0;
float t3 = q1 * q1;
float t4 = q2 * q2;
float t5 = q3 * q3;
float t6 = q0 * q3 * 2.0f;
float t8 = t2 - t3 + t4 - t5;
float t9 = q0 * q1 * 2.0f;
float t10 = q2 * q3 * 2.0f;
float t11 = t9 - t10;
float t14 = q1 * q2 * 2.0f;
float t21 = magY * t8;
float t22 = t6 + t14;
float t23 = magX * t22;
float t24 = magZ * t11;
float t7 = t21 + t23 - t24;
float t12 = t2 + t3 - t4 - t5;
float t13 = magX * t12;
float t15 = q0 * q2 * 2.0f;
float t16 = q1 * q3 * 2.0f;
float t17 = t15 + t16;
float t18 = magZ * t17;
float t19 = t6 - t14;
float t25 = magY * t19;
float t20 = t13 + t18 - t25;
if (fabsf(t20) < 1e-6f) {
return;
}
float t26 = 1.0f / (t20 * t20);
float t27 = t7 * t7;
float t28 = t26 * t27;
float t29 = t28 + 1.0f;
if (fabsf(t29) < 1e-12f) {
return;
}
float t30 = 1.0f / t29;
if (fabsf(t20) < 1e-12f) {
return;
}
float t31 = 1.0f / t20;
// calculate observation jacobian
float H_DECL[3] = {};
H_DECL[0] = -t30 * (t31 * (magZ * t8 + magY * t11) + t7 * t26 * (magY * t17 + magZ * t19));
H_DECL[1] = t30 * (t31 * (magX * t11 + magZ * t22) - t7 * t26 * (magZ * t12 - magX * t17));
H_DECL[2] = t30 * (t31 * (magX * t8 - magY * t22) + t7 * t26 * (magY * t12 + magX * t19));
// rotate the magnetometer measurement into earth frame
matrix::Dcm<float> R_to_earth(_state.quat_nominal);
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// check if there is enough magnetic field length to use and exit if too small
float magLength2 = sq(mag_earth_pred(0) + mag_earth_pred(1));
if (magLength2 < sq(_params.mag_noise)) {
return;
}
// Adjust the measurement variance upwards if thehorizontal strength to magnetometer noise ratio make the value unrealistic
R_DECL = fmaxf(R_DECL, sq(_params.mag_noise) / magLength2);
// Calculate the innovation, using the declination angle of the projection onto the horizontal as the measurement
_heading_innov = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
// wrap the innovation to the interval between +-pi
_heading_innov = matrix::wrap_pi(_heading_innov);
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
float PH[3];
_heading_innov_var = R_DECL;
for (unsigned row = 0; row <= 2; row++) {
PH[row] = 0.0f;
for (unsigned col = 0; col <= 2; col++) {
PH[row] += P[row][col] * H_DECL[col];
}
_heading_innov_var += H_DECL[row] * PH[row];
}
float varInnovInv;
if (_heading_innov_var >= R_DECL) {
// the innovation variance contribution from the state covariances is not negative, no fault
_fault_status.bad_mag_hdg = false;
} else {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.bad_mag_hdg = true;
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
// innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
_mag_healthy = false;
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (_control_status.flags.in_air) {
printf("return 5\n");
return;
} else {
// constrain the innovation to the maximum set by the gate
float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var));
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
}
} else {
_mag_healthy = true;
}
varInnovInv = 1.0f / _heading_innov_var;
// calculate the Kalman gains
float Kfusion[24] = {};
for (unsigned row = 0; row < 16; row++) {
Kfusion[row] = 0.0f;
for (unsigned col = 0; col <= 2; col++) {
Kfusion[row] += P[row][col] * H_DECL[col];
}
Kfusion[row] *= varInnovInv;
}
// by definition our error state is zero at the time of fusion
_state.ang_error.setZero();
// correct the states
fuse(Kfusion, _heading_innov);
// correct the quaternon using the attitude error estimate
Quaternion q_correction;
q_correction.from_axis_angle(_state.ang_error);
_state.quat_nominal = q_correction * _state.quat_nominal;
_state.quat_nominal.normalize();
_state.ang_error.setZero();
// correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
// and we only need the first 16 states
float HP[16];
for (uint8_t col = 0; col < 16; col++) {
HP[col] = 0.0f;
for (uint8_t row = 0; row <= 2; row++) {
HP[col] += H_DECL[row] * P[row][col];
}
}
for (uint8_t row = 0; row < 16; row++) {
for (uint8_t col = 0; col < 16; col++) {
P[row][col] -= Kfusion[row] * HP[col];
}
}
makeSymmetrical();
limitCov();
}
EKF/mathlib.cpp
+57
View File
@@ -0,0 +1,57 @@
/****************************************************************************
*
* Copyright (C) 2012, 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 mathlib.cpp
*
* Definition of math namespace function for POSIX SHARED
*
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
*/
#include "mathlib.h"
#ifdef POSIX_SHARED
float math::constrain(float &val, float min, float max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
float math::radians(float degrees)
{
return (degrees / 180.0f) * M_PI_F;
}
float math::degrees(float radians)
{
return (radians * 180.0f) / M_PI_F;
}
#endif
EKF/mathlib.h
+59
View File
@@ -0,0 +1,59 @@
/****************************************************************************
*
* Copyright (C) 2012, 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 mathlib.h
*
* Target specific math functions and definitions
*
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
*/
#ifndef MATHLIB_H
#define MATHLIB_H
#ifdef POSIX_SHARED
#include <Eigen/Dense>
#include <algorithm>
#define M_PI_F 3.14159265358979323846f
namespace math {
using namespace Eigen;
using namespace std;
float constrain(float &val, float min, float max);
float radians(float degrees);
float degrees(float radians);
}
#else
#include <mathlib/mathlib.h>
#endif //POSIX_SHARED
#endif //MATHLIB_H
README.md
+12 -1
View File
@@ -1,7 +1,18 @@
# ECL
Very lightweight Estimation & Control Library.
#### Very lightweight Estimation & Control Library.
[![Build Status](https://travis-ci.org/PX4/ecl.svg?branch=master)](https://travis-ci.org/PX4/ecl)
This library solves the estimation & control problems of a number of robots and drones. It accepts GPS, vision and inertial sensor inputs. It is extremely lightweight and efficient and yet has the rugged field-proven performance.
The library is currently BSD licensed, but might move to Apache 2.0.
## Building EKF Library
Prerequisite:
* Eigen3: http://eigen.tuxfamily.org/index.php installed
By following the steps mentioned below you can create a shared library which can be included in projects using `-l` flag of gcc:
* mkdir Build/
* cd Build/
* cmake ../EKF
* make
build.sh
Executable
+49
View File
@@ -0,0 +1,49 @@
############################################################################
#
# Copyright (c) 2015 ECL 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 ECL 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.
#
############################################################################
#download, build and install eigen
wget -O eigen.tar.bz2 http://bitbucket.org/eigen/eigen/get/3.2.8.tar.bz2
mkdir eigen
tar -xvjf eigen.tar.bz2 -C eigen --strip-components=1
mkdir eigen-build
cd eigen-build
cmake ../eigen
make
sudo make install
#build EKF shared library
cd ..
mkdir Build
cd Build
cmake ../EKF
make
matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt
+46 -21
View File
@@ -1,32 +1,57 @@
/*
Autocode for fusion of a yaw error measurement where the innovation is given by:
Observation jacobian for fusion of the horizontal components of magnetometer measurements.
innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
by rotating the measured magnetometer readings from body into earth axes.
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
declinationAngle is the estimated declination as that location
This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
Protection against /0 and covariance matrix errors will need to be added.
Divide by zero protection hs been added
*/
// intermediate variables
// calculate intermediate variables for observation jacobian
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
float t10 = 1.0f/(t6*t6);
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
float t15 = 1.0f/t6;
float t6 = q0*q3*2.0f;
float t8 = t2-t3+t4-t5;
float t9 = q0*q1*2.0f;
float t10 = q2*q3*2.0f;
float t11 = t9-t10;
float t14 = q1*q2*2.0f;
float t21 = magY*t8;
float t22 = t6+t14;
float t23 = magX*t22;
float t24 = magZ*t11;
float t7 = t21+t23-t24;
float t12 = t2+t3-t4-t5;
float t13 = magX*t12;
float t15 = q0*q2*2.0f;
float t16 = q1*q3*2.0f;
float t17 = t15+t16;
float t18 = magZ*t17;
float t19 = t6-t14;
float t25 = magY*t19;
float t20 = t13+t18-t25;
if (fabsf(t20) < 1e-6f) {
return;
}
float t26 = 1.0f/(t20*t20);
float t27 = t7*t7;
float t28 = t26*t27;
float t29 = t28+1.0;
if (fabsf(t29) < 1e-12f) {
return;
}
float t30 = 1.0f/t29;
if (fabsf(t20) < 1e-12f) {
return;
}
float t31 = 1.0f/t20;
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
float H_MAG[3];
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
// calculate observation jacobian
float H_DECL[3] = {};
H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
+27
View File
@@ -0,0 +1,27 @@
/*
Autocode for fusion of an Euler yaw measurement.
Protection against /0 and covariance matrix errors will need to be added.
*/
// intermediate variables
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
float t10 = 1.0f/(t6*t6);
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
float t15 = 1.0f/t6;
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
float H_MAG[3];
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
+34 -16
View File
@@ -1,6 +1,6 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% XYZ body fixed frame
% Sequential fusion of velocity and position measurements
% Fusion of true airspeed
@@ -15,8 +15,8 @@
% Based on use of a rotation vector for attitude estimation as described
% here:
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% pp. 855-860.
% State vector:
@@ -68,8 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
syms ptd real % location of terrain in D axis
syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
syms decl real; % earth magnetic field declination from true north
syms R_YAW real; % variance for magnetic deviation measurement
syms R_DECL real; % variance of supplied declination
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
syms rho real % air density (kg/m^3)
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
@@ -102,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat);
Tbn = Quat2Tbn(truthQuat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
@@ -189,10 +188,10 @@ F = jacobian(newStateVector, stateVector);
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[F,SF]=OptimiseAlgebra(F,'SF');
% define a symbolic covariance matrix using strings to represent
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
@@ -264,12 +263,33 @@ K_LOS = [K_LOSX,K_LOSY];
simplify(K_LOS);
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
% Use matlab c code converter for an alternate method of
% Use matlab c code converter for an alternate method of
ccode(H_LOS,'file','H_LOS.txt');
ccode(K_LOSX,'file','K_LOSX.txt');
ccode(K_LOSY,'file','K_LOSY.txt');
%% derive equations for fusion of direct yaw measurement
%% derive equations for simple fusion of 2-D magnetic heading measurements
% rotate magnetic field into earth axes
magMeasNED = Tbn*[magX;magY;magZ];
% the predicted measurement is the angle wrt true north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b];
Psimple = P(1:16,1:16);
H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian
%H_MAGS = H_MAGS(1:3);
H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
%H_MAGS = simplify(H_MAGS);
%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
ccode(H_MAGS,'file','calcH_MAGS.c');
% Calculate Kalman gain vector
K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL);
%K_MAGS = simplify(K_MAGS);
%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
ccode(K_MAGS,'file','calcK_MAGS.c');
%% derive equations for fusion of yaw measurements
% rotate X body axis into earth axes
yawVec = Tbn*[1;0;0];
@@ -298,7 +318,7 @@ H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_MAGD = simplify(H_MAGD);
%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
%ccode(H_MAGD,'file','calcH_MAGD.c');
ccode(H_MAGD,'file','calcH_MAGD.c');
% Calculate Kalman gain vector
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
@@ -319,7 +339,7 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
% Use a simple viscous drag model for the linear estimator equations
% Use the the derivative from speed to acceleration averaged across the
% Use the the derivative from speed to acceleration averaged across the
% speed range
% The nonlinear equation will be used to calculate the predicted
% measurement in implementation
@@ -339,14 +359,12 @@ H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
ccode([H_ACCY',K_ACCY],'file','calcACCY.c');
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
% generate c code for jacobians using the matlab code generator
ccode([H_ACCX;H_ACCY],'file','calcH_ACCXY.c');
%% Save output and convert to m and c code fragments
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
ConvertToC(nStates);
matrix
Submodule
+1
Submodule matrix added at 95e3d7d6ce
validation/data_validator.cpp
+12
View File
@@ -66,6 +66,18 @@ DataValidator::~DataValidator()
}
void
DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in)
{
float data[3];
data[0] = val;
data[1] = 0.0f;
data[2] = 0.0f;
put(timestamp, data, error_count_in, priority_in);
}
void
DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
{
validation/data_validator.h
+7
View File
@@ -54,6 +54,13 @@ public:
*
* @param val Item to put
*/
void put(uint64_t timestamp, float val, uint64_t error_count, int priority);
/**
* Put a 3D item into the validator.
*
* @param val Item to put
*/
void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority);
/**