Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 33 Packages Projects Releases Wiki Activity

Adding comments to ekf.

Browse Source
This commit is contained in:
James Goppert
2013-01-15 11:00:52 -05:00
parent 5b0aa490d6
commit d02a24ec82
2 changed files with 102 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+21 -18
View File
@@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// miss counts
_missFast(0),
_missSlow(0),
// state
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
@@ -94,13 +95,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL")
_rAccel(this, "R_ACCEL"),
_magDip(this, "MAG_DIP"),
_magDec(this, "MAG_DEC")
{
using namespace math;
// initial state covariance matrix
P0 = Matrix::identity(9) * 1.0f;
P = P0;
P = P0;
// wait for gps lock
while (1) {
@@ -132,12 +135,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
printf("[kalman_demo] initializing EKF state with GPS\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
printf("[kalman_demo] initializing EKF state with GPS\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
@@ -342,10 +345,10 @@ void KalmanNav::predictFast(float dt)
vN * LDot + g;
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
double(vNDot), double(vEDot), double(vDDot));
printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
double(LDot), double(lDot), double(rotRate));
printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
double(vNDot), double(vEDot), double(vDDot));
printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
double(LDot), double(lDot), double(rotRate));
// rectangular integration
//printf("dt: %8.4f\n", double(dt));
@@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt)
// prepare for matrix
float R = R0 + float(alt);
float RSq = R*R;
float RSq = R * R;
// F Matrix
// Titterton pg. 291
@@ -472,8 +475,8 @@ void KalmanNav::correctAtt()
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level
static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
float bN = cosf(dip) * cosf(dec);
float bE = cosf(dip) * sinf(dec);
float bD = sinf(dip);
@@ -593,7 +596,7 @@ void KalmanNav::correctAtt()
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse()*y);
float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
@@ -671,7 +674,7 @@ void KalmanNav::correctPos()
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse()*y);
float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
apps/examples/kalman_demo/KalmanNav.hpp
+81 -36
View File
@@ -68,53 +68,98 @@
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
/**
* The main callback function for the class
*/
void update();
/**
* Fast prediction
* Contains: state integration
*/
virtual void updatePublications();
void predictFast(float dt);
/**
* Slow prediction
* Contains: covariance integration
*/
void predictSlow(float dt);
/**
* Attitude correction
*/
void correctAtt();
/**
* Position correction
*/
void correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
math::Matrix F;
math::Matrix G;
math::Matrix P;
math::Matrix P0;
math::Matrix V;
math::Matrix HAtt;
math::Matrix RAtt;
math::Matrix HPos;
math::Matrix RPos;
math::Dcm C_nb;
math::Quaternion q;
control::UOrbSubscription<sensor_combined_s> _sensors;
control::UOrbSubscription<vehicle_gps_position_s> _gps;
control::UOrbSubscription<parameter_update_s> _param_update;
control::UOrbPublication<vehicle_global_position_s> _pos;
control::UOrbPublication<vehicle_attitude_s> _att;
uint64_t _pubTimeStamp;
uint64_t _fastTimeStamp;
uint64_t _slowTimeStamp;
uint64_t _attTimeStamp;
uint64_t _outTimeStamp;
uint16_t _navFrames;
uint16_t _missFast;
uint16_t _missSlow;
float fN, fE, fD;
// kalman filter
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix G; /**< noise shaping matrix for gyro/accel */
math::Matrix P; /**< state covariance matrix */
math::Matrix P0; /**< initial state covariance matrix */
math::Matrix V; /**< gyro/ accel noise matrix */
math::Matrix HAtt; /**< attitude measurement matrix */
math::Matrix RAtt; /**< attitude measurement noise matrix */
math::Matrix HPos; /**< position measurement jacobian matrix */
math::Matrix RPos; /**< position measurement noise matrix */
// attitude
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _fastTimeStamp; /**< fast prediction time stamp */
uint64_t _slowTimeStamp; /**< slow prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _missFast; /**< number of times fast prediction loop missed */
uint16_t _missSlow; /**< number of times slow prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
float phi, theta, psi;
float vN, vE, vD;
double lat, lon, alt;
control::BlockParam<float> _vGyro;
control::BlockParam<float> _vAccel;
control::BlockParam<float> _rMag;
control::BlockParam<float> _rGpsVel;
control::BlockParam<float> _rGpsPos;
control::BlockParam<float> _rGpsAlt;
control::BlockParam<float> _rAccel;
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon, alt; /**< lat, lon, alt, radians */
// parameters
control::BlockParam<float> _vGyro; /**< gyro process noise */
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }