Anton Babushkin
11 years ago
46 changed files with 4353 additions and 244 deletions
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,235 @@
@@ -0,0 +1,235 @@
|
||||
#include <math.h> |
||||
#include <stdint.h> |
||||
|
||||
#pragma once |
||||
|
||||
#define GRAVITY_MSS 9.80665f |
||||
#define deg2rad 0.017453292f |
||||
#define rad2deg 57.295780f |
||||
#define pi 3.141592657f |
||||
#define earthRate 0.000072921f |
||||
#define earthRadius 6378145.0f |
||||
#define earthRadiusInv 1.5678540e-7f |
||||
|
||||
class Vector3f |
||||
{ |
||||
private: |
||||
public: |
||||
float x; |
||||
float y; |
||||
float z; |
||||
|
||||
float length(void) const; |
||||
Vector3f zero(void) const; |
||||
}; |
||||
|
||||
class Mat3f |
||||
{ |
||||
private: |
||||
public: |
||||
Vector3f x; |
||||
Vector3f y; |
||||
Vector3f z; |
||||
|
||||
Mat3f(); |
||||
|
||||
Mat3f transpose(void) const; |
||||
}; |
||||
|
||||
Vector3f operator*(float sclIn1, Vector3f vecIn1); |
||||
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); |
||||
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); |
||||
Vector3f operator*( Mat3f matIn, Vector3f vecIn); |
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); |
||||
Vector3f operator*(Vector3f vecIn1, float sclIn1); |
||||
|
||||
void swap_var(float &d1, float &d2); |
||||
|
||||
const unsigned int n_states = 21; |
||||
const unsigned int data_buffer_size = 50; |
||||
|
||||
extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
extern Vector3f dVelIMU; |
||||
extern Vector3f dAngIMU; |
||||
|
||||
extern float P[n_states][n_states]; // covariance matrix
|
||||
extern float Kfusion[n_states]; // Kalman gains
|
||||
extern float states[n_states]; // state matrix
|
||||
extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
|
||||
extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
|
||||
extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
|
||||
extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
|
||||
extern bool useAirspeed; // boolean true if airspeed data is being used
|
||||
extern bool useCompass; // boolean true if magnetometer data is being used
|
||||
extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
extern float innovVelPos[6]; // innovation output
|
||||
extern float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
extern bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
|
||||
extern float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
extern float posNE[2]; // North, East position obs (m)
|
||||
extern float hgtMea; // measured height (m)
|
||||
extern float posNED[3]; // North, East Down position (m)
|
||||
|
||||
extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
|
||||
extern float innovMag[3]; // innovation output
|
||||
extern float varInnovMag[3]; // innovation variance output
|
||||
extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
extern float innovVtas; // innovation output
|
||||
extern float varInnovVtas; // innovation variance output
|
||||
extern bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
extern float VtasMeas; // true airspeed measurement (m/s)
|
||||
extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
extern float latRef; // WGS-84 latitude of reference point (rad)
|
||||
extern float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
extern float hgtRef; // WGS-84 height of reference point (m)
|
||||
extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
extern float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
// GPS input data variables
|
||||
extern float gpsCourse; |
||||
extern float gpsVelD; |
||||
extern float gpsLat; |
||||
extern float gpsLon; |
||||
extern float gpsHgt; |
||||
extern uint8_t GPSstatus; |
||||
|
||||
// Baro input
|
||||
extern float baroHgt; |
||||
|
||||
extern bool statesInitialised; |
||||
extern bool numericalProtection; |
||||
|
||||
const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
|
||||
const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
|
||||
extern bool staticMode; |
||||
|
||||
enum GPS_FIX { |
||||
GPS_FIX_NOFIX = 0, |
||||
GPS_FIX_2D = 2, |
||||
GPS_FIX_3D = 3 |
||||
}; |
||||
|
||||
struct ekf_status_report { |
||||
bool velHealth; |
||||
bool posHealth; |
||||
bool hgtHealth; |
||||
bool velTimeout; |
||||
bool posTimeout; |
||||
bool hgtTimeout; |
||||
uint32_t velFailTime; |
||||
uint32_t posFailTime; |
||||
uint32_t hgtFailTime; |
||||
float states[n_states]; |
||||
bool statesNaN; |
||||
bool covarianceNaN; |
||||
bool kalmanGainsNaN; |
||||
}; |
||||
|
||||
void UpdateStrapdownEquationsNED(); |
||||
|
||||
void CovariancePrediction(float dt); |
||||
|
||||
void FuseVelposNED(); |
||||
|
||||
void FuseMagnetometer(); |
||||
|
||||
void FuseAirspeed(); |
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); |
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); |
||||
|
||||
float sq(float valIn); |
||||
|
||||
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[n_states], uint64_t msec); |
||||
|
||||
void ResetStoredStates(); |
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); |
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude); |
||||
|
||||
void eul2quat(float (&quat)[4], const float (&eul)[3]); |
||||
|
||||
void quat2eul(float (&eul)[3], const float (&quat)[4]); |
||||
|
||||
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); |
||||
|
||||
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); |
||||
|
||||
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); |
||||
|
||||
void OnGroundCheck(); |
||||
|
||||
void CovarianceInit(); |
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3]); |
||||
|
||||
float ConstrainFloat(float val, float min, float max); |
||||
|
||||
void ConstrainVariances(); |
||||
|
||||
void ConstrainStates(); |
||||
|
||||
void ForceSymmetry(); |
||||
|
||||
int CheckAndBound(); |
||||
|
||||
void ResetPosition(); |
||||
|
||||
void ResetVelocity(); |
||||
|
||||
void ZeroVariables(); |
||||
|
||||
void GetFilterState(struct ekf_status_report *state); |
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error); |
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report); |
||||
void FillErrorReport(struct ekf_status_report *err); |
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3]); |
||||
|
||||
uint32_t millis(); |
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,117 @@
@@ -0,0 +1,117 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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 fw_att_pos_estimator_params.c |
||||
* |
||||
* Parameters defined by the attitude and position estimator task |
||||
* |
||||
* @author Lorenz Meier <lm@inf.ethz.ch> |
||||
*/ |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <systemlib/param/param.h> |
||||
|
||||
/*
|
||||
* Estimator parameters, accessible via MAVLink |
||||
* |
||||
*/ |
||||
|
||||
/**
|
||||
* Velocity estimate delay |
||||
* |
||||
* The delay in milliseconds of the velocity estimate from GPS. |
||||
* |
||||
* @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. |
||||
* |
||||
* @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. |
||||
* |
||||
* @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. |
||||
* |
||||
* @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. |
||||
* |
||||
* @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); |
||||
|
||||
@ -0,0 +1,42 @@
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Main Attitude and Position Estimator for Fixed Wing Aircraft
|
||||
#
|
||||
|
||||
MODULE_COMMAND = fw_att_pos_estimator
|
||||
|
||||
SRCS = fw_att_pos_estimator_main.cpp \
|
||||
fw_att_pos_estimator_params.c \
|
||||
estimator.cpp
|
||||
@ -0,0 +1,80 @@
@@ -0,0 +1,80 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 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 estimator_status.h |
||||
* Definition of the estimator_status_report uORB topic. |
||||
* |
||||
* @author Lorenz Meier <lm@inf.ethz.ch> |
||||
*/ |
||||
|
||||
#ifndef ESTIMATOR_STATUS_H_ |
||||
#define ESTIMATOR_STATUS_H_ |
||||
|
||||
#include <stdint.h> |
||||
#include <stdbool.h> |
||||
#include "../uORB.h" |
||||
|
||||
/**
|
||||
* @addtogroup topics |
||||
* @{ |
||||
*/ |
||||
|
||||
/**
|
||||
* Estimator status report. |
||||
* |
||||
* This is a generic status report struct which allows any of the onboard estimators |
||||
* to write the internal state to the system log. |
||||
* |
||||
*/ |
||||
struct estimator_status_report { |
||||
|
||||
/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only */ |
||||
|
||||
uint64_t timestamp; /**< Timestamp in microseconds since boot */ |
||||
float states[32]; /**< Internal filter states */ |
||||
float n_states; /**< Number of states effectively used */ |
||||
bool states_nan; /**< If set to true, one of the states is NaN */ |
||||
bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ |
||||
bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ |
||||
|
||||
}; |
||||
|
||||
/**
|
||||
* @} |
||||
*/ |
||||
|
||||
/* register this as object request broker structure */ |
||||
ORB_DECLARE(estimator_status); |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue