fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Better / cleaner initialization of the attitude estimator

Browse Source
This commit is contained in:
Lorenz Meier 2014-04-22 01:18:30 +02:00
parent 0a89364e3c
commit 706d08055d
3 changed files with 38 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -4,7 +4,7 @@
// 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
#define EKF_DEBUG
#ifdef EKF_DEBUG
#include <stdio.h>
@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
if (!isfinite(KH[i][j])) {
err_report->covarianceNaN = true;
ekf_debug("KH NaN");
err = true;
} // intermediate result used for covariance updates
if (err) {
ekf_debug("KH NaN");
}
if (!isfinite(KHP[i][j])) {
err_report->covarianceNaN = true;
ekf_debug("KHP NaN");
err = true;
} // intermediate result used for covariance updates
if (err) {
ekf_debug("KHP NaN");
}
if (!isfinite(P[i][j])) {
err_report->covarianceNaN = true;
ekf_debug("P NaN");
err = true;
} // covariance matrix
if (err) {
ekf_debug("P NaN");
}
}
if (!isfinite(Kfusion[i])) {
@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
summedDelVel.z = 0.0f;
}
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt)
{
//store initial lat,long and height
latRef = gpsLat;
lonRef = gpsLon;
hgtRef = gpsHgt;
latRef = referenceLat;
lonRef = referenceLon;
hgtRef = referenceHgt;
memset(&last_ekf_error, 0, sizeof(last_ekf_error));

2
src/modules/ekf_att_pos_estimator/estimator.h
View File

@ -308,7 +308,7 @@ void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3]);
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt);
float ConstrainFloat(float val, float min, float max);

34
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
View File

@ -606,7 +606,6 @@ FixedwingEstimator::task_main()
/* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
warnx("TS fail");
}
@ -650,14 +649,14 @@ FixedwingEstimator::task_main()
IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
last_run = _sensor_combined.timestamp;
/* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
warnx("TS fail");
}
last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
@ -823,6 +822,8 @@ FixedwingEstimator::task_main()
*/
int check = _ekf->CheckAndBound();
const char* ekfname = "[ekf] ";
switch (check) {
case 0:
/* all ok */
@ -830,27 +831,34 @@ FixedwingEstimator::task_main()
case 1:
{
const char* str = "NaN in states, resetting";
warnx(str);
mavlink_log_critical(_mavlink_fd, str);
warnx("%s%s", ekfname, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 2:
{
const char* str = "stale IMU data, resetting";
warnx(str);
mavlink_log_critical(_mavlink_fd, str);
warnx("%s%s", ekfname, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 3:
{
const char* str = "switching dynamic / static state";
warnx(str);
mavlink_log_critical(_mavlink_fd, str);
const char* str = "switching to dynamic state";
warnx("%s%s", ekfname, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
default:
{
const char* str = "unknown reset condition";
warnx("%s%s", ekfname, str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
}
}
// If non-zero, we got a problem
// If non-zero, we got a filter reset
if (check) {
struct ekf_status_report ekf_report;
@ -912,7 +920,7 @@ FixedwingEstimator::task_main()
double lon = home.lon;
float alt = home.alt;
_ekf->InitialiseFilter(_ekf->velNED);
_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt);
// Initialize projection
_local_pos.ref_lat = home.lat * 1e7;
@ -942,7 +950,7 @@ FixedwingEstimator::task_main()
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->InitialiseFilter(_ekf->velNED);
_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f);
}
}