Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Move old estimators to examples

Browse Source
This commit is contained in:
Lorenz Meier
2016-08-06 11:56:58 +02:00
parent cff9e90bec
commit ee5cdab963
18 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/attitude_estimator_ekf/AttitudeEKF.m
+298
View File
@@ -0,0 +1,298 @@
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
%LQG Position Estimator and Controller
% Observer:
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
% x[n+1|n] = Ax[n|n] + Bu[n]
%
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $
%
%
% Arguments:
% approx_prediction: if 1 then the exponential map is approximated with a
% first order taylor approximation. has at the moment not a big influence
% (just 1st or 2nd order approximation) we should change it to rodriquez
% approximation.
% use_inertia_matrix: set to true if you have the inertia matrix J for your
% quadrotor
% xa_apo_k: old state vectotr
% zFlag: if sensor measurement is available [gyro, acc, mag]
% dt: dt in s
% z: measurements [gyro, acc, mag]
% q_rotSpeed: process noise gyro
% q_rotAcc: process noise gyro acceleration
% q_acc: process noise acceleration
% q_mag: process noise magnetometer
% r_gyro: measurement noise gyro
% r_accel: measurement noise accel
% r_mag: measurement noise mag
% J: moment of inertia matrix
% Output:
% xa_apo: updated state vectotr
% Pa_apo: updated state covariance matrix
% Rot_matrix: rotation matrix
% eulerAngles: euler angles
% debugOutput: not used
%% model specific parameters
% compute once the inverse of the Inertia
persistent Ji;
if isempty(Ji)
Ji=single(inv(J));
end
%% init
persistent x_apo
if(isempty(x_apo))
gyro_init=single([0;0;0]);
gyro_acc_init=single([0;0;0]);
acc_init=single([0;0;-9.81]);
mag_init=single([1;0;0]);
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
end
persistent P_apo
if(isempty(P_apo))
% P_apo = single(eye(NSTATES) * 1000);
P_apo = single(200*ones(12));
end
debugOutput = single(zeros(4,1));
%% copy the states
wx= x_apo(1); % x body angular rate
wy= x_apo(2); % y body angular rate
wz= x_apo(3); % z body angular rate
wax= x_apo(4); % x body angular acceleration
way= x_apo(5); % y body angular acceleration
waz= x_apo(6); % z body angular acceleration
zex= x_apo(7); % x component gravity vector
zey= x_apo(8); % y component gravity vector
zez= x_apo(9); % z component gravity vector
mux= x_apo(10); % x component magnetic field vector
muy= x_apo(11); % y component magnetic field vector
muz= x_apo(12); % z component magnetic field vector
%% prediction section
% compute the apriori state estimate from the previous aposteriori estimate
%body angular accelerations
if (use_inertia_matrix==1)
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
else
wak =[wax;way;waz];
end
%body angular rates
wk =[wx; wy; wz] + dt*wak;
%derivative of the prediction rotation matrix
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
%prediction of the earth z vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
zek =(O*dt+single(eye(3)))*[zex;zey;zez];
else
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
%precision
end
%prediction of the magnetic vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
muk =(O*dt+single(eye(3)))*[mux;muy;muz];
else
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
%precision
end
x_apr=[wk;wak;zek;muk];
% compute the apriori error covariance estimate from the previous
%aposteriori estimate
EZ=[0,zez,-zey;
-zez,0,zex;
zey,-zex,0]';
MA=[0,muz,-muy;
-muz,0,mux;
muy,-mux,0]';
E=single(eye(3));
Z=single(zeros(3));
A_lin=[ Z, E, Z, Z
Z, Z, Z, Z
EZ, Z, O, Z
MA, Z, Z, O];
A_lin=eye(12)+A_lin*dt;
%process covariance matrix
persistent Q
if (isempty(Q))
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
q_rotAcc,q_rotAcc,q_rotAcc,...
q_acc,q_acc,q_acc,...
q_mag,q_mag,q_mag]);
end
P_apr=A_lin*P_apo*A_lin'+Q;
%% update
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0,0,0,0;
% 0,r_gyro,0,0,0,0,0,0,0;
% 0,0,r_gyro,0,0,0,0,0,0;
% 0,0,0,r_accel,0,0,0,0,0;
% 0,0,0,0,r_accel,0,0,0,0;
% 0,0,0,0,0,r_accel,0,0,0;
% 0,0,0,0,0,0,r_mag,0,0;
% 0,0,0,0,0,0,0,r_mag,0;
% 0,0,0,0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
%observation matrix
%[zw;ze;zmk];
H_k=[ E, Z, Z, Z;
Z, Z, E, Z;
Z, Z, Z, E];
y_k=z(1:9)-H_k*x_apr;
%S_k=H_k*P_apr*H_k'+R;
S_k=H_k*P_apr*H_k';
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
K_k=(P_apr*H_k'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k)*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
R=[r_gyro,0,0;
0,r_gyro,0;
0,0,r_gyro];
R_v=[r_gyro,r_gyro,r_gyro];
%observation matrix
H_k=[ E, Z, Z, Z];
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_accel,0,0;
% 0,0,0,0,r_accel,0;
% 0,0,0,0,0,r_accel];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, E, Z];
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_mag,0,0;
% 0,0,0,0,r_mag,0;
% 0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, Z, E];
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
x_apo=x_apr;
P_apo=P_apr;
end
end
end
end
%% euler anglels extraction
z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
m_n_b = x_apo(10:12)./norm(x_apo(10:12));
y_n_b=cross(z_n_b,m_n_b);
y_n_b=y_n_b./norm(y_n_b);
x_n_b=(cross(y_n_b,z_n_b));
x_n_b=x_n_b./norm(x_n_b);
xa_apo=x_apo;
Pa_apo=P_apo;
% rotation matrix from earth to body system
Rot_matrix=[x_n_b,y_n_b,z_n_b];
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
theta=-asin(Rot_matrix(1,3));
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
eulerAngles=[phi;theta;psi];
src/examples/attitude_estimator_ekf/CMakeLists.txt
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_module(
MODULE modules__attitude_estimator_ekf
MAIN attitude_estimator_ekf
STACK_MAIN 1200
COMPILE_FLAGS
-Wno-float-equal
SRCS
attitude_estimator_ekf_main.cpp
codegen/AttitudeEKF.c
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj
+502
View File
File diff suppressed because it is too large Load Diff
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
Executable
+657
View File
File diff suppressed because it is too large Load Diff
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c
Executable
+130
View File
@@ -0,0 +1,130 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 attitude_estimator_ekf_params.c
*
* Parameters for EKF filter
*/
#include "attitude_estimator_ekf_params.h"
#include <math.h>
#include <px4_defines.h>
/* Extended Kalman Filter covariances */
/**
* Body angular rate process noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
/**
* Body angular acceleration process noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
/**
* Acceleration process noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
/**
* Magnet field vector process noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
/**
* Gyro measurement noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
/**
* Accel measurement noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
/**
* Mag measurement noise
*
* @group Attitude EKF estimator
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/**
* Moment of inertia matrix diagonal entry (1, 1)
*
* @group Attitude EKF estimator
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
/**
* Moment of inertia matrix diagonal entry (2, 2)
*
* @group Attitude EKF estimator
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
/**
* Moment of inertia matrix diagonal entry (3, 3)
*
* @group Attitude EKF estimator
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
/**
* Moment of inertia enabled in estimator
*
* If set to != 0 the moment of inertia will be used in the estimator
*
* @group Attitude EKF estimator
* @boolean
*/
PARAM_DEFINE_INT32(ATT_J_EN, 0);
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h
Executable
+75
View File
@@ -0,0 +1,75 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 attitude_estimator_ekf_params.h
*
* Parameters for EKF filter
*/
#include <systemlib/param/param.h>
struct attitude_estimator_ekf_params {
float r[3];
float q[4];
float moment_inertia_J[9];
int32_t use_moment_inertia;
float roll_off;
float pitch_off;
float yaw_off;
float mag_decl;
int acc_comp;
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2;
param_t q0, q1, q2, q3;
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
param_t use_moment_inertia;
param_t mag_decl;
param_t acc_comp;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_ekf_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c
+1456
View File
File diff suppressed because it is too large Load Diff
src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h
+26
View File
@@ -0,0 +1,26 @@
/*
* AttitudeEKF.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_H__
#define __ATTITUDEEKF_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "AttitudeEKF_types.h"
/* Function Declarations */
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
extern void AttitudeEKF_initialize(void);
extern void AttitudeEKF_terminate(void);
#endif
/* End of code generation (AttitudeEKF.h) */
src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
+17
View File
@@ -0,0 +1,17 @@
/*
* AttitudeEKF_types.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_TYPES_H__
#define __ATTITUDEEKF_TYPES_H__
/* Include files */
#include "rtwtypes.h"
#endif
/* End of code generation (AttitudeEKF_types.h) */
src/examples/attitude_estimator_ekf/codegen/rtwtypes.h
+160
View File
@@ -0,0 +1,160 @@
/*
* rtwtypes.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __RTWTYPES_H__
#define __RTWTYPES_H__
#ifndef TRUE
# define TRUE (1U)
#endif
#ifndef FALSE
# define FALSE (0U)
#endif
#ifndef __TMWTYPES__
#define __TMWTYPES__
#include <limits.h>
/*=======================================================================*
* Target hardware information
* Device type: ARM Compatible->ARM Cortex
* Number of bits: char: 8 short: 16 int: 32
* long: 32
* native word size: 32
* Byte ordering: LittleEndian
* Signed integer division rounds to: Undefined
* Shift right on a signed integer as arithmetic shift: on
*=======================================================================*/
/*=======================================================================*
* Fixed width word size data types: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
* real32_T, real64_T - 32 and 64 bit floating point numbers *
*=======================================================================*/
typedef signed char int8_T;
typedef unsigned char uint8_T;
typedef short int16_T;
typedef unsigned short uint16_T;
typedef int int32_T;
typedef unsigned int uint32_T;
typedef float real32_T;
typedef double real64_T;
/*===========================================================================*
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
* ulong_T, char_T and byte_T. *
*===========================================================================*/
typedef double real_T;
typedef double time_T;
typedef unsigned char boolean_T;
typedef int int_T;
typedef unsigned int uint_T;
typedef unsigned long ulong_T;
typedef char char_T;
typedef char_T byte_T;
/*===========================================================================*
* Complex number type definitions *
*===========================================================================*/
#define CREAL_T
typedef struct {
real32_T re;
real32_T im;
} creal32_T;
typedef struct {
real64_T re;
real64_T im;
} creal64_T;
typedef struct {
real_T re;
real_T im;
} creal_T;
typedef struct {
int8_T re;
int8_T im;
} cint8_T;
typedef struct {
uint8_T re;
uint8_T im;
} cuint8_T;
typedef struct {
int16_T re;
int16_T im;
} cint16_T;
typedef struct {
uint16_T re;
uint16_T im;
} cuint16_T;
typedef struct {
int32_T re;
int32_T im;
} cint32_T;
typedef struct {
uint32_T re;
uint32_T im;
} cuint32_T;
/*=======================================================================*
* Min and Max: *
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
*=======================================================================*/
#define MAX_int8_T ((int8_T)(127))
#define MIN_int8_T ((int8_T)(-128))
#define MAX_uint8_T ((uint8_T)(255))
#define MIN_uint8_T ((uint8_T)(0))
#define MAX_int16_T ((int16_T)(32767))
#define MIN_int16_T ((int16_T)(-32768))
#define MAX_uint16_T ((uint16_T)(65535))
#define MIN_uint16_T ((uint16_T)(0))
#define MAX_int32_T ((int32_T)(2147483647))
#define MIN_int32_T ((int32_T)(-2147483647-1))
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
#define MIN_uint32_T ((uint32_T)(0))
/* Logical type definitions */
#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
# ifndef false
# define false (0U)
# endif
# ifndef true
# define true (1U)
# endif
#endif
/*
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
* for signed integer values.
*/
#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
#error "This code must be compiled using a 2's complement representation for signed integer values"
#endif
/*
* Maximum length of a MATLAB identifier (function/variable)
* including the null-termination character. Referenced by
* rt_logging.c and rt_matrx.c.
*/
#define TMW_NAME_LENGTH_MAX 64
#endif
#endif
/* End of code generation (rtwtypes.h) */
src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+379
View File
@@ -0,0 +1,379 @@
/****************************************************************************
*
* Copyright (c) 2013-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 AttitudePositionEstimatorEKF.h
* Implementation of the attitude and position estimator. This is a PX4 wrapper around
* the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp)
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <geo/geo.h>
#include <terrain_estimation/terrain_estimator.h>
#include <systemlib/perf_counter.h>
#include "estimator_22states.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
//Forward declaration
class AttPosEKF;
class AttitudePositionEstimatorEKF : public control::SuperBlock
{
public:
/**
* Constructor
*/
AttitudePositionEstimatorEKF();
/* we do not want people ever copying this class */
AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete;
AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete;
/**
* Destructor, also kills the sensors task.
*/
~AttitudePositionEstimatorEKF();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
/**
* Task status
*
* @return true if the mainloop is running
*/
bool task_running() { return _task_running; }
/**
* Print the current status.
*/
void print_status();
/**
* Trip the filter by feeding it NaN values.
*/
int trip_nan();
/**
* Enable logging.
*
* @param enable Set to true to enable logging, false to disable
*/
int enable_logging(bool enable);
/**
* Set debug level.
*
* @param debug Desired debug level - 0 to disable.
*/
int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
int _estimator_task; /**< task handle for sensor task */
int _sensor_combined_sub;
int _distance_sub; /**< distance measurement */
int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */
int _vehicle_status_sub;
int _vehicle_land_detected_sub;
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
int _landDetectorSub;
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
struct airspeed_s _airspeed; /**< airspeed */
struct baro_report _baro; /**< baro readings */
struct vehicle_status_s _vehicle_status;
struct vehicle_land_detected_s _vehicle_land_detected;
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct actuator_armed_s _armed;
hrt_abstime _last_accel;
hrt_abstime _last_mag;
unsigned _prediction_steps;
uint64_t _prediction_last;
struct sensor_combined_s _sensor_combined;
struct map_projection_reference_s _pos_ref;
float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
hrt_abstime _last_debug_print = 0;
perf_counter_t _loop_perf; ///< loop performance counter
perf_counter_t _loop_intvl; ///< loop rate counter
perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
perf_counter_t _perf_mag; ///<local performance counter for mag updates
perf_counter_t _perf_gps; ///<local performance counter for gps updates
perf_counter_t _perf_baro; ///<local performance counter for baro updates
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
perf_counter_t _perf_reset; ///<local performance counter for filter resets
float _gps_alt_filt;
float _baro_alt_filt;
float _covariancePredictionDt; ///< time lapsed since last covariance prediction
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
hrt_abstime _distance_last_valid;
bool _data_good; ///< all required filter data is ok
bool _ekf_logging; ///< log EKF state
unsigned _debug; ///< debug level - default 0
bool _was_landed; ///< indicates if was landed in previous iteration
bool _newHgtData;
bool _newAdsData;
bool _newDataMag;
bool _newRangeData;
orb_advert_t _mavlink_log_pub;
control::BlockParamFloat _mag_offset_x;
control::BlockParamFloat _mag_offset_y;
control::BlockParamFloat _mag_offset_z;
struct {
int32_t vel_delay_ms;
int32_t pos_delay_ms;
int32_t height_delay_ms;
int32_t mag_delay_ms;
int32_t tas_delay_ms;
float velne_noise;
float veld_noise;
float posne_noise;
float posd_noise;
float mag_noise;
float gyro_pnoise;
float acc_pnoise;
float gbias_pnoise;
float abias_pnoise;
float mage_pnoise;
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
int32_t airspeed_mode;
} _parameters; /**< local copies of interesting parameters */
struct {
param_t vel_delay_ms;
param_t pos_delay_ms;
param_t height_delay_ms;
param_t mag_delay_ms;
param_t tas_delay_ms;
param_t velne_noise;
param_t veld_noise;
param_t posne_noise;
param_t posd_noise;
param_t mag_noise;
param_t gyro_pnoise;
param_t acc_pnoise;
param_t gbias_pnoise;
param_t abias_pnoise;
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
param_t airspeed_mode;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
TerrainEstimator *_terrain_estimator;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _LP_att_P;
math::LowPassFilter2p _LP_att_Q;
math::LowPassFilter2p _LP_att_R;
private:
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*
*/
void control_update();
/**
* Check for changes in land detected.
*/
void vehicle_status_poll();
/**
* Check for changes in land detected.
*/
void vehicle_land_detected_poll();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main filter task.
*/
void task_main();
/**
* Check filter sanity state
*
* @return zero if ok, non-zero for a filter error condition.
*/
int check_filter_state();
/**
* @brief
* Publish the euler and quaternions for attitude estimation
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
**/
void publishLocalPosition();
/**
* @brief
* Publish global position estimation (WSG84 and AMSL).
* A global position estimate is only valid if we have a good GPS fix
**/
void publishGlobalPosition();
/**
* @brief
* Publish wind estimates for north and east in m/s
**/
void publishWindEstimate();
/**
* @brief
* Runs the sensor fusion step of the filter. The parameters determine which of the sensors
* are fused with each other
**/
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
const bool fuseBaro, const bool fuseAirSpeed);
/**
* @brief
* Initialize first time good GPS fix so we can get a reference point to calculate local position from
* Should only be required to call once
**/
void initializeGPS();
/**
* Initialize the reference position for the local coordinate frame
*/
void initReferencePosition(hrt_abstime timestamp, bool gps_valid,
double lat, double lon, float gps_alt, float baro_alt);
/**
* @brief
* Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
* flags to true (e.g newDataGps)
**/
void pollData();
};
src/examples/ekf_att_pos_estimator/CMakeLists.txt
+47
View File
@@ -0,0 +1,47 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_module(
MODULE modules__ekf_att_pos_estimator
MAIN ekf_att_pos_estimator
COMPILE_FLAGS -Os
STACK_MAIN 3000
STACK_MAX 3400
SRCS
ekf_att_pos_estimator_main.cpp
estimator_22states.cpp
estimator_utilities.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+1747
View File
File diff suppressed because it is too large Load Diff
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+307
View File
@@ -0,0 +1,307 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ekf_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lorenz@px4.io>
*/
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
/**
* Airspeed measurement noise.
*
* Increasing this value will make the filter trust this sensor
* less and trust other sensors more.
*
* @min 0.5
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
/**
* Velocity measurement noise in north-east (horizontal) direction.
*
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7
*
* @min 0.2
* @max 3.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f);
/**
* Position noise in north-east (horizontal) direction
*
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.5
* @max 3.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.01
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
/**
* Gyro process noise
*
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
* This noise controls how much the filter trusts the gyro measurements.
* Increasing it makes the filter trust the gyro less and other sensors more.
*
* @min 0.001
* @max 0.05
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
/**
* Accelerometer process noise
*
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
* Increasing this value makes the filter trust the accelerometer less
* and other sensors more.
*
* @min 0.05
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.125f);
/**
* Gyro bias estimate process noise
*
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
* Increasing this value will make the gyro bias converge faster but noisier.
*
* @min 0.00000005
* @max 0.00001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
/**
* Accelerometer bias estimate process noise
*
* Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f.
* Increasing this value makes the bias estimation faster and noisier.
*
* @min 0.00001
* @max 0.001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f);
/**
* Magnetometer earth frame offsets process noise
*
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
* Increasing this value makes the magnetometer earth bias estimate converge
* faster but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
/**
* Magnetometer body frame offsets process noise
*
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
* Increasing this value makes the magnetometer body bias estimate converge faster
* but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Magnetometer X bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
/**
* Magnetometer Y bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
/**
* Magnetometer Z bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
/**
* Threshold for filter initialization.
*
* If the standard deviation of the GPS position estimate is below this threshold
* in meters, the filter will initialize.
*
* @min 0.3
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
src/examples/ekf_att_pos_estimator/estimator_22states.cpp
+3346
View File
File diff suppressed because one or more lines are too long
src/examples/ekf_att_pos_estimator/estimator_22states.h
+424
View File
@@ -0,0 +1,424 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 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.
*
* Neither the name of the {organization} 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 HOLDER 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 estimator_22states.h
*
* Definition of the attitude and position estimator.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include "estimator_utilities.h"
#include <cstddef>
constexpr size_t EKF_STATE_ESTIMATES = 22;
constexpr size_t EKF_DATA_BUFFER_SIZE = 50;
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters()
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
struct mag_state_struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
};
struct mag_state_struct magstate;
struct mag_state_struct resetMagState;
// Global variables
float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix
float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains
float states[EKF_STATE_ESTIMATES]; // state matrix
float resetStates[EKF_STATE_ESTIMATES];
float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored
// Times
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time
float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f prevDelAng; ///< previous delta angle, used for coning correction
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f lastGyroOffset; // Last gyro offset
Vector3f delAngTotal;
Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
float dtIMUfilt; // average time between IMU measurements (sec)
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
float dtVelPosFilt; // average time between position / velocity fusion steps
float dtHgtFilt; // average time between height measurement updates
float dtGpsFilt; // average time between gps measurement updates
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
float varInnovVtas; // innovation variance output
float varInnovRng; // range finder innovation variance
float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
// GPS input data variables
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool fuseOptFlowData; // true when optical flow data is fused
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
bool staticMode; ///< boolean true if no position feedback is fused
bool useGPS; // boolean true if GPS data is being used
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
bool useOpticalFlow; // true when optical flow data is being used
bool ekfDiverged;
uint64_t lastReset;
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
// Optical Flow error estimation
float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
// Two state EKF used to estimate focal length scale factor and terrain position
float Popt[2][2]; // state covariance matrix
float flowStates[2]; // flow states [scale factor, terrain position]
float prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
float flowInnovGate; // number of standard deviations used for the innovation consistency check
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
float rngInnovGate; // number of standard deviations used for the innovation consistency check
float minFlowRng; // minimum range over which to fuse optical flow measurements
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
void updateDtGpsFilt(float dt);
void updateDtHgtFilt(float dt);
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void FuseVelposNED();
void FuseMagnetometer();
void FuseAirspeed();
void FuseOptFlow();
/**
* @brief
* Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
* This fiter requires optical flow rates that are not motion compensated
* Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
**/
void OpticalFlowEKF();
void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
void RecallOmega(float *omegaForFusion, uint64_t msec);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
//static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static inline float sq(float valIn) {return valIn * valIn;}
/**
* @brief
* Tell the EKF if the vehicle has landed
**/
void setOnGround(const bool isLanded);
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float maxf);
void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
/**
* @brief
* Check the filter inputs and bound its operational state
*
* This check will reset the filter states if required
* due to a failure of consistency or timeout checks.
* it should be run after the measurement data has been
* updated, but before any of the fusion steps are
* executed.
*/
int CheckAndBound(struct ekf_status_report *last_error);
/**
* @brief
* Reset the filter position states
*
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition();
/**
* @brief
* Reset the velocity state.
*/
void ResetVelocity();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN();
void InitializeDynamic(float (&initvelNED)[3], float declination);
/**
* @brief
* Tells the EKF wheter the vehicle is a fixed wing frame or not.
* This changes the way the EKF fuses position or attitude calulations
* by making some assumptions on movement.
* @param fixedWing
* true if the vehicle moves like a Fixed Wing, false otherwise
**/
void setIsFixedWing(const bool fixedWing);
/**
* @brief
* Reset internal filter states and clear all variables to zero value
*/
void ZeroVariables();
void get_covariance(float c[28]);
float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
protected:
/**
* @brief
* Initializes algorithm parameters to safe default values
**/
void InitialiseParameters();
void updateDtVelPosFilt(float dt);
bool FilterHealthy();
bool GyroOffsetsDiverged();
bool VelNEDDiverged();
/**
* @brief
* Reset the height state.
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight();
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
void ResetStoredStates();
private:
bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type
bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
float _accNavMagHorizontal; ///< First-order low-pass filtered rate of change maneuver velocity
};
uint32_t millis();
uint64_t getMicros();
src/examples/ekf_att_pos_estimator/estimator_utilities.cpp
+234
View File
@@ -0,0 +1,234 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 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.
*
* Neither the name of the {organization} 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 HOLDER 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 estimator_utilities.cpp
*
* Estimator support utilities.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "estimator_utilities.h"
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
// optimized out by the compiler.
//#define EKF_DEBUG
#ifdef EKF_DEBUG
#include <stdio.h>
#include <stdarg.h>
static void
ekf_debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[ekf]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
void
ekf_debug(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
ekf_debug_print(fmt, args);
}
#else
void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
/* we don't want to pull in the standard lib just to swap two floats */
void swap_var(float &d1, float &d2);
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
}
void Vector3f::zero(void)
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Mat3f::Mat3f() :
x{1.0f, 0.0f, 0.0f},
y{0.0f, 1.0f, 0.0f},
z{0.0f, 0.0f, 1.0f}
{
}
void Mat3f::identity() {
x.x = 1.0f;
x.y = 0.0f;
x.z = 0.0f;
y.x = 0.0f;
y.y = 1.0f;
y.z = 0.0f;
z.x = 0.0f;
z.y = 0.0f;
z.z = 1.0f;
}
Mat3f Mat3f::transpose() const
{
Mat3f ret = *this;
swap_var(ret.x.y, ret.y.x);
swap_var(ret.x.z, ret.z.x);
swap_var(ret.y.z, ret.z.y);
return ret;
}
void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
{
velNEDr[0] = gpsGndSpd*cosf(gpsCourse);
velNEDr[1] = gpsGndSpd*sinf(gpsCourse);
velNEDr[2] = gpsVelD;
}
void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
{
posNEDr[0] = earthRadius * (lat - latReference);
posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference);
posNEDr[2] = -(hgt - hgtReference);
}
void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + (double)posNEDi[0] * earthRadiusInv;
lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNEDi[2];
}
// overload + operator to provide a vector addition
Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x + vecIn2.x;
vecOut.y = vecIn1.y + vecIn2.y;
vecOut.z = vecIn1.z + vecIn2.z;
return vecOut;
}
// overload - operator to provide a vector subtraction
Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x - vecIn2.x;
vecOut.y = vecIn1.y - vecIn2.y;
vecOut.z = vecIn1.z - vecIn2.z;
return vecOut;
}
// overload * operator to provide a matrix vector product
Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn)
{
Vector3f vecOut;
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
vecOut.z = matIn.z.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
return vecOut;
}
// overload * operator to provide a matrix product
Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2)
{
Mat3f matOut;
matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
return matOut;
}
// overload % operator to provide a vector cross product
Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(const Vector3f &vecIn1, const float sclIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(float sclIn1, const Vector3f &vecIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
// overload / operator to provide a vector scalar division
Vector3f operator/(const Vector3f &vec, const float scalar)
{
Vector3f vecOut;
vecOut.x = vec.x / scalar;
vecOut.y = vec.y / scalar;
vecOut.z = vec.z / scalar;
return vecOut;
}
void swap_var(float &d1, float &d2)
{
float tmp = d1;
d1 = d2;
d2 = tmp;
}
src/examples/ekf_att_pos_estimator/estimator_utilities.h
+135
View File
@@ -0,0 +1,135 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 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.
*
* Neither the name of the {organization} 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 HOLDER 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 estimator_utilities.h
*
* Estimator support utilities.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <math.h>
#include <stdint.h>
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f
#define earthRate 0.000072921f
#define earthRadius 6378145.0
#define earthRadiusInv 1.5678540e-7
class Vector3f
{
public:
float x;
float y;
float z;
Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) :
x(a),
y(b),
z(c)
{}
float length() const;
void zero();
};
class Mat3f
{
private:
public:
Vector3f x;
Vector3f y;
Vector3f z;
Mat3f();
void identity();
Mat3f transpose() const;
};
Vector3f operator*(const float sclIn1, const Vector3f &vecIn1);
Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2);
Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2);
Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn);
Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2);
Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2);
Vector3f operator*(const Vector3f &vecIn1, const float sclIn1);
Vector3f operator/(const Vector3f &vec, const float scalar);
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
struct ekf_status_report {
bool error;
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
bool imuTimeout;
bool onGround;
bool staticMode;
bool useCompass;
bool useAirspeed;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
float states[32];
unsigned n_states;
bool angNaN;
bool summedDelVelNaN;
bool KHNaN;
bool KHPNaN;
bool PNaN;
bool covarianceNaN;
bool kalmanGainsNaN;
bool statesNaN;
bool gyroOffsetsExcessive;
bool covariancesExcessive;
bool velOffsetExcessive;
};
void ekf_debug(const char *fmt, ...);
void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference);
void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);