forked from Archive/PX4-Autopilot
Merge branch 'master' into mpc_local_pos
This commit is contained in:
commit
b98157c655
|
@ -2,85 +2,6 @@
|
|||
|
||||
#include <string.h>
|
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
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)
|
||||
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 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)
|
||||
uint8_t fusionModeGPS = 0; // 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 posNED[3]; // North, East Down position (m)
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse;
|
||||
float gpsVelD;
|
||||
float gpsLat;
|
||||
float gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised = false;
|
||||
|
||||
bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode = true; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed = true; ///< boolean true if airspeed data is being used
|
||||
bool useCompass = true; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection = true;
|
||||
|
||||
static unsigned storeIndex = 0;
|
||||
|
||||
float Vector3f::length(void) const
|
||||
{
|
||||
return sqrt(x*x + y*y + z*z);
|
||||
|
@ -185,7 +106,16 @@ void swap_var(float &d1, float &d2)
|
|||
d2 = tmp;
|
||||
}
|
||||
|
||||
void UpdateStrapdownEquationsNED()
|
||||
AttPosEKF::AttPosEKF()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
AttPosEKF::~AttPosEKF()
|
||||
{
|
||||
}
|
||||
|
||||
void AttPosEKF::UpdateStrapdownEquationsNED()
|
||||
{
|
||||
Vector3f delVelNav;
|
||||
float q00;
|
||||
|
@ -247,7 +177,7 @@ void UpdateStrapdownEquationsNED()
|
|||
qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
|
||||
|
||||
// Normalise the quaternions and update the quaternion states
|
||||
quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
|
||||
quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
|
||||
if (quatMag > 1e-16f)
|
||||
{
|
||||
float quatMagInv = 1.0f/quatMag;
|
||||
|
@ -312,7 +242,7 @@ void UpdateStrapdownEquationsNED()
|
|||
ConstrainStates();
|
||||
}
|
||||
|
||||
void CovariancePrediction(float dt)
|
||||
void AttPosEKF::CovariancePrediction(float dt)
|
||||
{
|
||||
// scalars
|
||||
float windVelSigma;
|
||||
|
@ -953,7 +883,7 @@ void CovariancePrediction(float dt)
|
|||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void FuseVelposNED()
|
||||
void AttPosEKF::FuseVelposNED()
|
||||
{
|
||||
|
||||
// declare variables used by fault isolation logic
|
||||
|
@ -999,8 +929,8 @@ void FuseVelposNED()
|
|||
observation[5] = -(hgtMea);
|
||||
|
||||
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring
|
||||
posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring
|
||||
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
|
||||
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
|
||||
R_OBS[0] = 0.04f + sq(velErr);
|
||||
R_OBS[1] = R_OBS[0];
|
||||
R_OBS[2] = 0.08f + sq(velErr);
|
||||
|
@ -1026,7 +956,7 @@ void FuseVelposNED()
|
|||
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
|
||||
}
|
||||
// apply a 5-sigma threshold
|
||||
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
|
||||
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
|
||||
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
|
||||
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
|
||||
{
|
||||
|
@ -1175,7 +1105,7 @@ void FuseVelposNED()
|
|||
//printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
|
||||
}
|
||||
|
||||
void FuseMagnetometer()
|
||||
void AttPosEKF::FuseMagnetometer()
|
||||
{
|
||||
uint8_t obsIndex;
|
||||
uint8_t indexLimit;
|
||||
|
@ -1482,7 +1412,7 @@ void FuseMagnetometer()
|
|||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void FuseAirspeed()
|
||||
void AttPosEKF::FuseAirspeed()
|
||||
{
|
||||
float vn;
|
||||
float ve;
|
||||
|
@ -1503,14 +1433,14 @@ void FuseAirspeed()
|
|||
|
||||
// Need to check that it is flying before fusing airspeed data
|
||||
// Calculate the predicted airspeed
|
||||
VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
|
||||
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
|
||||
// Perform fusion of True Airspeed measurement
|
||||
if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0))
|
||||
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
|
||||
{
|
||||
// Calculate observation jacobians
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
|
||||
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
|
||||
|
||||
float H_TAS[21];
|
||||
for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
|
||||
|
@ -1611,7 +1541,7 @@ void FuseAirspeed()
|
|||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
{
|
||||
uint8_t row;
|
||||
uint8_t col;
|
||||
|
@ -1624,7 +1554,7 @@ 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)
|
||||
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
{
|
||||
uint8_t row;
|
||||
uint8_t col;
|
||||
|
@ -1637,13 +1567,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
|||
}
|
||||
}
|
||||
|
||||
float sq(float valIn)
|
||||
float AttPosEKF::sq(float valIn)
|
||||
{
|
||||
return valIn*valIn;
|
||||
}
|
||||
|
||||
// Store states in a history array along with time stamp
|
||||
void StoreStates(uint64_t timestamp_ms)
|
||||
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
|
||||
{
|
||||
for (unsigned i=0; i<n_states; i++)
|
||||
storedStates[i][storeIndex] = states[i];
|
||||
|
@ -1653,7 +1583,7 @@ void StoreStates(uint64_t timestamp_ms)
|
|||
storeIndex = 0;
|
||||
}
|
||||
|
||||
void ResetStoredStates()
|
||||
void AttPosEKF::ResetStoredStates()
|
||||
{
|
||||
// reset all stored states
|
||||
memset(&storedStates[0][0], 0, sizeof(storedStates));
|
||||
|
@ -1674,7 +1604,7 @@ void ResetStoredStates()
|
|||
}
|
||||
|
||||
// Output the state vector stored at the time that best matches that specified by msec
|
||||
int RecallStates(float statesForFusion[n_states], uint64_t msec)
|
||||
int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
|
@ -1723,7 +1653,7 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
|
|||
return ret;
|
||||
}
|
||||
|
||||
void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
|
||||
void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
|
||||
{
|
||||
// Calculate the nav to body cosine matrix
|
||||
float q00 = sq(quat[0]);
|
||||
|
@ -1748,7 +1678,7 @@ void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
|
|||
Tnb.y.z = 2*(q23 + q01);
|
||||
}
|
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
|
||||
void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
|
||||
{
|
||||
// Calculate the body to nav cosine matrix
|
||||
float q00 = sq(quat[0]);
|
||||
|
@ -1773,7 +1703,7 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
|
|||
Tbn.z.y = 2*(q23 + q01);
|
||||
}
|
||||
|
||||
void eul2quat(float (&quat)[4], const float (&eul)[3])
|
||||
void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
|
||||
{
|
||||
float u1 = cos(0.5f*eul[0]);
|
||||
float u2 = cos(0.5f*eul[1]);
|
||||
|
@ -1787,35 +1717,35 @@ void eul2quat(float (&quat)[4], const float (&eul)[3])
|
|||
quat[3] = u1*u2*u6-u4*u5*u3;
|
||||
}
|
||||
|
||||
void quat2eul(float (&y)[3], const float (&u)[4])
|
||||
void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
|
||||
{
|
||||
y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
|
||||
y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2]));
|
||||
y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
|
||||
y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
|
||||
y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
|
||||
y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
|
||||
}
|
||||
|
||||
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
|
||||
void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
|
||||
{
|
||||
velNED[0] = gpsGndSpd*cos(gpsCourse);
|
||||
velNED[1] = gpsGndSpd*sin(gpsCourse);
|
||||
velNED[0] = gpsGndSpd*cosf(gpsCourse);
|
||||
velNED[1] = gpsGndSpd*sinf(gpsCourse);
|
||||
velNED[2] = gpsVelD;
|
||||
}
|
||||
|
||||
void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
|
||||
void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
|
||||
{
|
||||
posNED[0] = earthRadius * (lat - latRef);
|
||||
posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
|
||||
posNED[2] = -(hgt - hgtRef);
|
||||
}
|
||||
|
||||
void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
|
||||
void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
|
||||
{
|
||||
lat = latRef + posNED[0] * earthRadiusInv;
|
||||
lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
|
||||
hgt = hgtRef - posNED[2];
|
||||
}
|
||||
|
||||
void OnGroundCheck()
|
||||
void AttPosEKF::OnGroundCheck()
|
||||
{
|
||||
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
|
||||
if (staticMode) {
|
||||
|
@ -1823,7 +1753,7 @@ void OnGroundCheck()
|
|||
}
|
||||
}
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude)
|
||||
void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
|
||||
{
|
||||
//Define Earth rotation vector in the NED navigation frame
|
||||
omega.x = earthRate*cosf(latitude);
|
||||
|
@ -1831,13 +1761,13 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
|
|||
omega.z = -earthRate*sinf(latitude);
|
||||
}
|
||||
|
||||
void CovarianceInit()
|
||||
void AttPosEKF::CovarianceInit()
|
||||
{
|
||||
// Calculate the initial covariance matrix P
|
||||
P[0][0] = 0.25f*sq(1.0f*deg2rad);
|
||||
P[1][1] = 0.25f*sq(1.0f*deg2rad);
|
||||
P[2][2] = 0.25f*sq(1.0f*deg2rad);
|
||||
P[3][3] = 0.25f*sq(10.0f*deg2rad);
|
||||
P[0][0] = 0.25f * sq(1.0f*deg2rad);
|
||||
P[1][1] = 0.25f * sq(1.0f*deg2rad);
|
||||
P[2][2] = 0.25f * sq(1.0f*deg2rad);
|
||||
P[3][3] = 0.25f * sq(10.0f*deg2rad);
|
||||
P[4][4] = sq(0.7);
|
||||
P[5][5] = P[4][4];
|
||||
P[6][6] = sq(0.7);
|
||||
|
@ -1857,12 +1787,12 @@ void CovarianceInit()
|
|||
P[20][20] = P[18][18];
|
||||
}
|
||||
|
||||
float ConstrainFloat(float val, float min, float max)
|
||||
float AttPosEKF::ConstrainFloat(float val, float min, float max)
|
||||
{
|
||||
return (val > max) ? max : ((val < min) ? min : val);
|
||||
}
|
||||
|
||||
void ConstrainVariances()
|
||||
void AttPosEKF::ConstrainVariances()
|
||||
{
|
||||
if (!numericalProtection) {
|
||||
return;
|
||||
|
@ -1914,7 +1844,7 @@ void ConstrainVariances()
|
|||
|
||||
}
|
||||
|
||||
void ConstrainStates()
|
||||
void AttPosEKF::ConstrainStates()
|
||||
{
|
||||
if (!numericalProtection) {
|
||||
return;
|
||||
|
@ -1971,7 +1901,7 @@ void ConstrainStates()
|
|||
|
||||
}
|
||||
|
||||
void ForceSymmetry()
|
||||
void AttPosEKF::ForceSymmetry()
|
||||
{
|
||||
if (!numericalProtection) {
|
||||
return;
|
||||
|
@ -1989,7 +1919,7 @@ void ForceSymmetry()
|
|||
}
|
||||
}
|
||||
|
||||
bool FilterHealthy()
|
||||
bool AttPosEKF::FilterHealthy()
|
||||
{
|
||||
if (!statesInitialised) {
|
||||
return false;
|
||||
|
@ -2012,7 +1942,7 @@ bool FilterHealthy()
|
|||
* This resets the position to the last GPS measurement
|
||||
* or to zero in case of static position.
|
||||
*/
|
||||
void ResetPosition(void)
|
||||
void AttPosEKF::ResetPosition(void)
|
||||
{
|
||||
if (staticMode) {
|
||||
states[7] = 0;
|
||||
|
@ -2030,7 +1960,7 @@ void ResetPosition(void)
|
|||
*
|
||||
* This resets the height state with the last altitude measurements
|
||||
*/
|
||||
void ResetHeight(void)
|
||||
void AttPosEKF::ResetHeight(void)
|
||||
{
|
||||
// write to the state vector
|
||||
states[9] = -hgtMea;
|
||||
|
@ -2039,7 +1969,7 @@ void ResetHeight(void)
|
|||
/**
|
||||
* Reset the velocity state.
|
||||
*/
|
||||
void ResetVelocity(void)
|
||||
void AttPosEKF::ResetVelocity(void)
|
||||
{
|
||||
if (staticMode) {
|
||||
states[4] = 0.0f;
|
||||
|
@ -2054,7 +1984,7 @@ void ResetVelocity(void)
|
|||
}
|
||||
|
||||
|
||||
void FillErrorReport(struct ekf_status_report *err)
|
||||
void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
|
||||
{
|
||||
for (int i = 0; i < n_states; i++)
|
||||
{
|
||||
|
@ -2069,7 +1999,7 @@ void FillErrorReport(struct ekf_status_report *err)
|
|||
err->hgtTimeout = current_ekf_state.hgtTimeout;
|
||||
}
|
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report) {
|
||||
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
|
||||
bool err = false;
|
||||
|
||||
// check all states and covariance matrices
|
||||
|
@ -2122,7 +2052,7 @@ bool StatesNaN(struct ekf_status_report *err_report) {
|
|||
* updated, but before any of the fusion steps are
|
||||
* executed.
|
||||
*/
|
||||
int CheckAndBound()
|
||||
int AttPosEKF::CheckAndBound()
|
||||
{
|
||||
|
||||
// Store the old filter state
|
||||
|
@ -2164,7 +2094,7 @@ int CheckAndBound()
|
|||
return 0;
|
||||
}
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
|
||||
void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
|
||||
{
|
||||
float initialRoll, initialPitch;
|
||||
float cosRoll, sinRoll, cosPitch, sinPitch;
|
||||
|
@ -2200,7 +2130,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
|
|||
initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
|
||||
}
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3])
|
||||
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
|
||||
{
|
||||
|
||||
// Clear the init flag
|
||||
|
@ -2254,7 +2184,7 @@ void InitializeDynamic(float (&initvelNED)[3])
|
|||
summedDelVel.z = 0.0f;
|
||||
}
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3])
|
||||
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
|
||||
{
|
||||
//store initial lat,long and height
|
||||
latRef = gpsLat;
|
||||
|
@ -2266,7 +2196,7 @@ void InitialiseFilter(float (&initvelNED)[3])
|
|||
InitializeDynamic(initvelNED);
|
||||
}
|
||||
|
||||
void ZeroVariables()
|
||||
void AttPosEKF::ZeroVariables()
|
||||
{
|
||||
// Do the data structure init
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
|
@ -2292,12 +2222,12 @@ void ZeroVariables()
|
|||
memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
|
||||
}
|
||||
|
||||
void GetFilterState(struct ekf_status_report *state)
|
||||
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
|
||||
{
|
||||
memcpy(state, ¤t_ekf_state, sizeof(state));
|
||||
}
|
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error)
|
||||
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
|
||||
{
|
||||
memcpy(last_error, &last_ekf_error, sizeof(last_error));
|
||||
}
|
||||
|
|
|
@ -48,85 +48,10 @@ 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;
|
||||
// extern bool staticMode;
|
||||
|
||||
enum GPS_FIX {
|
||||
GPS_FIX_NOFIX = 0,
|
||||
|
@ -150,6 +75,93 @@ struct ekf_status_report {
|
|||
bool kalmanGainsNaN;
|
||||
};
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
||||
AttPosEKF();
|
||||
~AttPosEKF();
|
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective 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)
|
||||
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 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)
|
||||
uint8_t fusionModeGPS = 0; // 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 posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse;
|
||||
float gpsVelD;
|
||||
float gpsLat;
|
||||
float gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised = false;
|
||||
|
||||
bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData = false; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData = false; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode = true; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed = true; ///< boolean true if airspeed data is being used
|
||||
bool useCompass = true; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection = true;
|
||||
|
||||
unsigned storeIndex = 0;
|
||||
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
void CovariancePrediction(float dt);
|
||||
|
@ -164,8 +176,6 @@ 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
|
||||
|
@ -190,15 +200,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
|
|||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude);
|
||||
|
||||
void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
|
||||
void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
|
||||
void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
static 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);
|
||||
static 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);
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
|
@ -231,5 +245,15 @@ void FillErrorReport(struct ekf_status_report *err);
|
|||
|
||||
void InitializeDynamic(float (&initvelNED)[3]);
|
||||
|
||||
protected:
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
void ResetHeight(void);
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
|
||||
|
||||
};
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
|
|
|
@ -124,6 +124,16 @@ public:
|
|||
*/
|
||||
int start();
|
||||
|
||||
/**
|
||||
* Print the current status.
|
||||
*/
|
||||
void print_status();
|
||||
|
||||
/**
|
||||
* Trip the filter by feeding it NaN values.
|
||||
*/
|
||||
int trip_nan();
|
||||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
|
@ -201,6 +211,7 @@ private:
|
|||
param_t tas_delay_ms;
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
AttPosEKF *_ekf;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
|
@ -282,7 +293,8 @@ FixedwingEstimator::FixedwingEstimator() :
|
|||
/* states */
|
||||
_initialized(false),
|
||||
_gps_initialized(false),
|
||||
_mavlink_fd(-1)
|
||||
_mavlink_fd(-1),
|
||||
_ekf(nullptr)
|
||||
{
|
||||
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
@ -386,6 +398,12 @@ void
|
|||
FixedwingEstimator::task_main()
|
||||
{
|
||||
|
||||
_ekf = new AttPosEKF();
|
||||
|
||||
if (!_ekf) {
|
||||
errx(1, "failed allocating EKF filter - out of RAM!");
|
||||
}
|
||||
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
|
@ -416,23 +434,23 @@ FixedwingEstimator::task_main()
|
|||
parameters_update();
|
||||
|
||||
/* set initial filter state */
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
fuseHgtData = false;
|
||||
fuseMagData = false;
|
||||
fuseVtasData = false;
|
||||
statesInitialised = false;
|
||||
_ekf->fuseVelData = false;
|
||||
_ekf->fusePosData = false;
|
||||
_ekf->fuseHgtData = false;
|
||||
_ekf->fuseMagData = false;
|
||||
_ekf->fuseVtasData = false;
|
||||
_ekf->statesInitialised = false;
|
||||
|
||||
/* initialize measurement data */
|
||||
VtasMeas = 0.0f;
|
||||
_ekf->VtasMeas = 0.0f;
|
||||
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
|
||||
Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
|
||||
dVelIMU.x = 0.0f;
|
||||
dVelIMU.y = 0.0f;
|
||||
dVelIMU.z = 0.0f;
|
||||
dAngIMU.x = 0.0f;
|
||||
dAngIMU.y = 0.0f;
|
||||
dAngIMU.z = 0.0f;
|
||||
_ekf->dVelIMU.x = 0.0f;
|
||||
_ekf->dVelIMU.y = 0.0f;
|
||||
_ekf->dVelIMU.z = 0.0f;
|
||||
_ekf->dAngIMU.x = 0.0f;
|
||||
_ekf->dAngIMU.y = 0.0f;
|
||||
_ekf->dAngIMU.z = 0.0f;
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[2];
|
||||
|
@ -511,7 +529,7 @@ FixedwingEstimator::task_main()
|
|||
}
|
||||
|
||||
last_sensor_timestamp = _gyro.timestamp;
|
||||
IMUmsec = _gyro.timestamp / 1e3f;
|
||||
_ekf.IMUmsec = _gyro.timestamp / 1e3f;
|
||||
|
||||
float deltaT = (_gyro.timestamp - last_run) / 1e6f;
|
||||
last_run = _gyro.timestamp;
|
||||
|
@ -523,20 +541,20 @@ FixedwingEstimator::task_main()
|
|||
|
||||
// Always store data, independent of init status
|
||||
/* fill in last data set */
|
||||
dtIMU = deltaT;
|
||||
_ekf->dtIMU = deltaT;
|
||||
|
||||
angRate.x = _gyro.x;
|
||||
angRate.y = _gyro.y;
|
||||
angRate.z = _gyro.z;
|
||||
_ekf->angRate.x = _gyro.x;
|
||||
_ekf->angRate.y = _gyro.y;
|
||||
_ekf->angRate.z = _gyro.z;
|
||||
|
||||
accel.x = _accel.x;
|
||||
accel.y = _accel.y;
|
||||
accel.z = _accel.z;
|
||||
_ekf->accel.x = _accel.x;
|
||||
_ekf->accel.y = _accel.y;
|
||||
_ekf->accel.z = _accel.z;
|
||||
|
||||
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
|
||||
lastAngRate = angRate;
|
||||
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
|
||||
lastAccel = accel;
|
||||
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
|
||||
_ekf->lastAngRate = angRate;
|
||||
_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
|
||||
_ekf->lastAccel = accel;
|
||||
|
||||
|
||||
#else
|
||||
|
@ -565,20 +583,20 @@ FixedwingEstimator::task_main()
|
|||
|
||||
// Always store data, independent of init status
|
||||
/* fill in last data set */
|
||||
dtIMU = deltaT;
|
||||
_ekf->dtIMU = deltaT;
|
||||
|
||||
angRate.x = _sensor_combined.gyro_rad_s[0];
|
||||
angRate.y = _sensor_combined.gyro_rad_s[1];
|
||||
angRate.z = _sensor_combined.gyro_rad_s[2];
|
||||
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
|
||||
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
|
||||
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
|
||||
|
||||
accel.x = _sensor_combined.accelerometer_m_s2[0];
|
||||
accel.y = _sensor_combined.accelerometer_m_s2[1];
|
||||
accel.z = _sensor_combined.accelerometer_m_s2[2];
|
||||
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
|
||||
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
|
||||
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
|
||||
|
||||
dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
|
||||
lastAngRate = angRate;
|
||||
dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
|
||||
lastAccel = accel;
|
||||
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
|
||||
lastAngRate = _ekf->angRate;
|
||||
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
|
||||
lastAccel = _ekf->accel;
|
||||
|
||||
if (last_mag != _sensor_combined.magnetometer_timestamp) {
|
||||
mag_updated = true;
|
||||
|
@ -599,7 +617,7 @@ FixedwingEstimator::task_main()
|
|||
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
|
||||
perf_count(_perf_airspeed);
|
||||
|
||||
VtasMeas = _airspeed.true_airspeed_m_s;
|
||||
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
|
||||
newAdsData = true;
|
||||
|
||||
} else {
|
||||
|
@ -624,24 +642,24 @@ FixedwingEstimator::task_main()
|
|||
|
||||
/* check if we had a GPS outage for a long time */
|
||||
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
|
||||
ResetPosition();
|
||||
ResetVelocity();
|
||||
ResetStoredStates();
|
||||
_ekf->ResetPosition();
|
||||
_ekf->ResetVelocity();
|
||||
_ekf->ResetStoredStates();
|
||||
}
|
||||
|
||||
/* fuse GPS updates */
|
||||
|
||||
//_gps.timestamp / 1e3;
|
||||
GPSstatus = _gps.fix_type;
|
||||
velNED[0] = _gps.vel_n_m_s;
|
||||
velNED[1] = _gps.vel_e_m_s;
|
||||
velNED[2] = _gps.vel_d_m_s;
|
||||
_ekf->GPSstatus = _gps.fix_type;
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
|
||||
|
||||
gpsLat = math::radians(_gps.lat / (double)1e7);
|
||||
gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
|
||||
gpsHgt = _gps.alt / 1e3f;
|
||||
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
|
||||
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
|
||||
_ekf->gpsHgt = _gps.alt / 1e3f;
|
||||
newDataGps = true;
|
||||
|
||||
}
|
||||
|
@ -654,10 +672,10 @@ FixedwingEstimator::task_main()
|
|||
if (baro_updated) {
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
|
||||
|
||||
baroHgt = _baro.altitude - _baro_ref;
|
||||
_ekf->baroHgt = _baro.altitude - _baro_ref;
|
||||
|
||||
// Could use a blend of GPS and baro alt data if desired
|
||||
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
|
||||
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
|
||||
}
|
||||
|
||||
#ifndef SENSOR_COMBINED_SUB
|
||||
|
@ -673,27 +691,27 @@ FixedwingEstimator::task_main()
|
|||
|
||||
// XXX we compensate the offsets upfront - should be close to zero.
|
||||
// 0.001f
|
||||
magData.x = _mag.x;
|
||||
magBias.x = 0.000001f; // _mag_offsets.x_offset
|
||||
_ekf->magData.x = _mag.x;
|
||||
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
|
||||
|
||||
magData.y = _mag.y;
|
||||
magBias.y = 0.000001f; // _mag_offsets.y_offset
|
||||
_ekf->magData.y = _mag.y;
|
||||
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
|
||||
|
||||
magData.z = _mag.z;
|
||||
magBias.z = 0.000001f; // _mag_offsets.y_offset
|
||||
_ekf->magData.z = _mag.z;
|
||||
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
|
||||
|
||||
#else
|
||||
|
||||
// XXX we compensate the offsets upfront - should be close to zero.
|
||||
// 0.001f
|
||||
magData.x = _sensor_combined.magnetometer_ga[0];
|
||||
magBias.x = 0.000001f; // _mag_offsets.x_offset
|
||||
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
|
||||
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
|
||||
|
||||
magData.y = _sensor_combined.magnetometer_ga[1];
|
||||
magBias.y = 0.000001f; // _mag_offsets.y_offset
|
||||
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
|
||||
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
|
||||
|
||||
magData.z = _sensor_combined.magnetometer_ga[2];
|
||||
magBias.z = 0.000001f; // _mag_offsets.y_offset
|
||||
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
|
||||
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
|
||||
|
||||
#endif
|
||||
|
||||
|
@ -707,7 +725,7 @@ FixedwingEstimator::task_main()
|
|||
/**
|
||||
* CHECK IF THE INPUT DATA IS SANE
|
||||
*/
|
||||
int check = CheckAndBound();
|
||||
int check = _ekf->CheckAndBound();
|
||||
|
||||
switch (check) {
|
||||
case 0:
|
||||
|
@ -741,7 +759,7 @@ FixedwingEstimator::task_main()
|
|||
|
||||
struct ekf_status_report ekf_report;
|
||||
|
||||
GetLastErrorState(&ekf_report);
|
||||
_ekf->GetLastErrorState(&ekf_report);
|
||||
|
||||
struct estimator_status_report rep;
|
||||
memset(&rep, 0, sizeof(rep));
|
||||
|
@ -781,16 +799,16 @@ FixedwingEstimator::task_main()
|
|||
|
||||
if (hrt_elapsed_time(&start_time) > 100000) {
|
||||
|
||||
if (!_gps_initialized && (GPSstatus == 3)) {
|
||||
velNED[0] = _gps.vel_n_m_s;
|
||||
velNED[1] = _gps.vel_e_m_s;
|
||||
velNED[2] = _gps.vel_d_m_s;
|
||||
if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
double lat = _gps.lat * 1e-7;
|
||||
double lon = _gps.lon * 1e-7;
|
||||
float alt = _gps.alt * 1e-3;
|
||||
|
||||
InitialiseFilter(velNED);
|
||||
_ekf->InitialiseFilter(_ekf->velNED);
|
||||
|
||||
// Initialize projection
|
||||
_local_pos.ref_lat = _gps.lat;
|
||||
|
@ -801,7 +819,7 @@ FixedwingEstimator::task_main()
|
|||
// Store
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
|
||||
_baro_ref = _baro.altitude;
|
||||
baroHgt = _baro.altitude - _baro_ref;
|
||||
_ekf->baroHgt = _baro.altitude - _baro_ref;
|
||||
_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
|
||||
|
||||
// XXX this is not multithreading safe
|
||||
|
@ -810,24 +828,24 @@ FixedwingEstimator::task_main()
|
|||
|
||||
_gps_initialized = true;
|
||||
|
||||
} else if (!statesInitialised) {
|
||||
velNED[0] = 0.0f;
|
||||
velNED[1] = 0.0f;
|
||||
velNED[2] = 0.0f;
|
||||
posNED[0] = 0.0f;
|
||||
posNED[1] = 0.0f;
|
||||
posNED[2] = 0.0f;
|
||||
} else if (!_ekf->statesInitialised) {
|
||||
_ekf->velNED[0] = 0.0f;
|
||||
_ekf->velNED[1] = 0.0f;
|
||||
_ekf->velNED[2] = 0.0f;
|
||||
_ekf->posNED[0] = 0.0f;
|
||||
_ekf->posNED[1] = 0.0f;
|
||||
_ekf->posNED[2] = 0.0f;
|
||||
|
||||
posNE[0] = posNED[0];
|
||||
posNE[1] = posNED[1];
|
||||
InitialiseFilter(velNED);
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
_ekf->InitialiseFilter(_ekf->velNED);
|
||||
}
|
||||
}
|
||||
|
||||
// If valid IMU data and states initialised, predict states and covariances
|
||||
if (statesInitialised) {
|
||||
if (_ekf->statesInitialised) {
|
||||
// Run the strapdown INS equations every IMU update
|
||||
UpdateStrapdownEquationsNED();
|
||||
_ekf->UpdateStrapdownEquationsNED();
|
||||
#if 0
|
||||
// debug code - could be tunred into a filter mnitoring/watchdog function
|
||||
float tempQuat[4];
|
||||
|
@ -844,20 +862,20 @@ FixedwingEstimator::task_main()
|
|||
|
||||
#endif
|
||||
// store the predicted states for subsequent use by measurement fusion
|
||||
StoreStates(IMUmsec);
|
||||
_ekf->StoreStates(IMUmsec);
|
||||
// Check if on ground - status is used by covariance prediction
|
||||
OnGroundCheck();
|
||||
_ekf->OnGroundCheck();
|
||||
// sum delta angles and time used by covariance prediction
|
||||
summedDelAng = summedDelAng + correctedDelAng;
|
||||
summedDelVel = summedDelVel + dVelIMU;
|
||||
dt += dtIMU;
|
||||
_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
|
||||
_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
|
||||
dt += _ekf->dtIMU;
|
||||
|
||||
// perform a covariance prediction if the total delta angle has exceeded the limit
|
||||
// or the time limit will be exceeded at the next IMU update
|
||||
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
|
||||
CovariancePrediction(dt);
|
||||
summedDelAng = summedDelAng.zero();
|
||||
summedDelVel = summedDelVel.zero();
|
||||
if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
|
||||
_ekf->CovariancePrediction(dt);
|
||||
_ekf->summedDelAng = _ekf->summedDelAng.zero();
|
||||
_ekf->summedDelVel = _ekf->summedDelVel.zero();
|
||||
dt = 0.0f;
|
||||
}
|
||||
|
||||
|
@ -867,79 +885,79 @@ FixedwingEstimator::task_main()
|
|||
// Fuse GPS Measurements
|
||||
if (newDataGps && _gps_initialized) {
|
||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||
velNED[0] = _gps.vel_n_m_s;
|
||||
velNED[1] = _gps.vel_e_m_s;
|
||||
velNED[2] = _gps.vel_d_m_s;
|
||||
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
|
||||
|
||||
posNE[0] = posNED[0];
|
||||
posNE[1] = posNED[1];
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
// set fusion flags
|
||||
fuseVelData = true;
|
||||
fusePosData = true;
|
||||
_ekf->fuseVelData = true;
|
||||
_ekf->fusePosData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
// run the fusion step
|
||||
FuseVelposNED();
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
} else if (statesInitialised) {
|
||||
} else if (_ekf->statesInitialised) {
|
||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||
velNED[0] = 0.0f;
|
||||
velNED[1] = 0.0f;
|
||||
velNED[2] = 0.0f;
|
||||
posNED[0] = 0.0f;
|
||||
posNED[1] = 0.0f;
|
||||
posNED[2] = 0.0f;
|
||||
_ekf->velNED[0] = 0.0f;
|
||||
_ekf->velNED[1] = 0.0f;
|
||||
_ekf->velNED[2] = 0.0f;
|
||||
_ekf->posNED[0] = 0.0f;
|
||||
_ekf->posNED[1] = 0.0f;
|
||||
_ekf->posNED[2] = 0.0f;
|
||||
|
||||
posNE[0] = posNED[0];
|
||||
posNE[1] = posNED[1];
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
// set fusion flags
|
||||
fuseVelData = true;
|
||||
fusePosData = true;
|
||||
_ekf->fuseVelData = true;
|
||||
_ekf->fusePosData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
// run the fusion step
|
||||
FuseVelposNED();
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
} else {
|
||||
fuseVelData = false;
|
||||
fusePosData = false;
|
||||
_ekf->fuseVelData = false;
|
||||
_ekf->fusePosData = false;
|
||||
}
|
||||
|
||||
if (newAdsData && statesInitialised) {
|
||||
if (newAdsData && _ekf->statesInitialised) {
|
||||
// Could use a blend of GPS and baro alt data if desired
|
||||
hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt;
|
||||
fuseHgtData = true;
|
||||
_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
|
||||
_ekf->fuseHgtData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
|
||||
// run the fusion step
|
||||
FuseVelposNED();
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
} else {
|
||||
fuseHgtData = false;
|
||||
_ekf->fuseHgtData = false;
|
||||
}
|
||||
|
||||
// Fuse Magnetometer Measurements
|
||||
if (newDataMag && statesInitialised) {
|
||||
fuseMagData = true;
|
||||
RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
if (newDataMag && _ekf->statesInitialised) {
|
||||
_ekf->fuseMagData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
|
||||
} else {
|
||||
fuseMagData = false;
|
||||
_ekf->fuseMagData = false;
|
||||
}
|
||||
|
||||
if (statesInitialised) FuseMagnetometer();
|
||||
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
|
||||
|
||||
// Fuse Airspeed Measurements
|
||||
if (newAdsData && statesInitialised && VtasMeas > 8.0f) {
|
||||
fuseVtasData = true;
|
||||
RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
FuseAirspeed();
|
||||
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
|
||||
_ekf->fuseVtasData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
_ekf->FuseAirspeed();
|
||||
|
||||
} else {
|
||||
fuseVtasData = false;
|
||||
_ekf->fuseVtasData = false;
|
||||
}
|
||||
|
||||
// Publish results
|
||||
|
@ -956,7 +974,7 @@ FixedwingEstimator::task_main()
|
|||
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
|
||||
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
|
||||
|
||||
math::Quaternion q(states[0], states[1], states[2], states[3]);
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
math::Matrix<3, 3> R = q.to_dcm();
|
||||
math::Vector<3> euler = R.to_euler();
|
||||
|
||||
|
@ -964,10 +982,10 @@ FixedwingEstimator::task_main()
|
|||
_att.R[i][j] = R(i, j);
|
||||
|
||||
_att.timestamp = last_sensor_timestamp;
|
||||
_att.q[0] = states[0];
|
||||
_att.q[1] = states[1];
|
||||
_att.q[2] = states[2];
|
||||
_att.q[3] = states[3];
|
||||
_att.q[0] = _ekf->states[0];
|
||||
_att.q[1] = _ekf->states[1];
|
||||
_att.q[2] = _ekf->states[2];
|
||||
_att.q[3] = _ekf->states[3];
|
||||
_att.q_valid = true;
|
||||
_att.R_valid = true;
|
||||
|
||||
|
@ -976,13 +994,13 @@ FixedwingEstimator::task_main()
|
|||
_att.pitch = euler(1);
|
||||
_att.yaw = euler(2);
|
||||
|
||||
_att.rollspeed = angRate.x - states[10];
|
||||
_att.pitchspeed = angRate.y - states[11];
|
||||
_att.yawspeed = angRate.z - states[12];
|
||||
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
|
||||
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
|
||||
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
|
||||
// gyro offsets
|
||||
_att.rate_offsets[0] = states[10];
|
||||
_att.rate_offsets[1] = states[11];
|
||||
_att.rate_offsets[2] = states[12];
|
||||
_att.rate_offsets[0] = _ekf->states[10];
|
||||
_att.rate_offsets[1] = _ekf->states[11];
|
||||
_att.rate_offsets[2] = _ekf->states[12];
|
||||
|
||||
/* lazily publish the attitude only once available */
|
||||
if (_att_pub > 0) {
|
||||
|
@ -995,20 +1013,15 @@ FixedwingEstimator::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
if (!isfinite(states[0])) {
|
||||
print_status();
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
if (_gps_initialized) {
|
||||
_local_pos.timestamp = last_sensor_timestamp;
|
||||
_local_pos.x = states[7];
|
||||
_local_pos.y = states[8];
|
||||
_local_pos.z = states[9];
|
||||
_local_pos.x = _ekf->states[7];
|
||||
_local_pos.y = _ekf->states[8];
|
||||
_local_pos.z = _ekf->states[9];
|
||||
|
||||
_local_pos.vx = states[4];
|
||||
_local_pos.vy = states[5];
|
||||
_local_pos.vz = states[6];
|
||||
_local_pos.vx = _ekf->states[4];
|
||||
_local_pos.vy = _ekf->states[5];
|
||||
_local_pos.vz = _ekf->states[6];
|
||||
|
||||
_local_pos.xy_valid = _gps_initialized;
|
||||
_local_pos.z_valid = true;
|
||||
|
@ -1104,9 +1117,10 @@ FixedwingEstimator::start()
|
|||
return OK;
|
||||
}
|
||||
|
||||
void print_status()
|
||||
void
|
||||
FixedwingEstimator::print_status()
|
||||
{
|
||||
math::Quaternion q(states[0], states[1], states[2], states[3]);
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
math::Matrix<3, 3> R = q.to_dcm();
|
||||
math::Vector<3> euler = R.to_euler();
|
||||
|
||||
|
@ -1122,30 +1136,30 @@ void print_status()
|
|||
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
|
||||
|
||||
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec);
|
||||
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z);
|
||||
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
|
||||
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]);
|
||||
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]);
|
||||
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]);
|
||||
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]);
|
||||
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]);
|
||||
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]);
|
||||
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]);
|
||||
printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
|
||||
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
|
||||
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
|
||||
printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
|
||||
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
|
||||
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
|
||||
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
|
||||
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
|
||||
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
|
||||
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
|
||||
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
|
||||
(statesInitialised) ? "INITIALIZED" : "NON_INIT",
|
||||
(onGround) ? "ON_GROUND" : "AIRBORNE",
|
||||
(fuseVelData) ? "FUSE_VEL" : "INH_VEL",
|
||||
(fusePosData) ? "FUSE_POS" : "INH_POS",
|
||||
(fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
|
||||
(fuseMagData) ? "FUSE_MAG" : "INH_MAG",
|
||||
(fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
|
||||
(useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
|
||||
(useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
|
||||
(staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
|
||||
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
|
||||
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
|
||||
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
|
||||
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
|
||||
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
|
||||
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
|
||||
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
|
||||
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
|
||||
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
|
||||
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
|
||||
}
|
||||
|
||||
int trip_nan() {
|
||||
int FixedwingEstimator::trip_nan() {
|
||||
|
||||
int ret = 0;
|
||||
|
||||
|
@ -1163,7 +1177,7 @@ int trip_nan() {
|
|||
float nan_val = 0.0f / 0.0f;
|
||||
|
||||
warnx("system not armed, tripping state vector with NaN values");
|
||||
states[5] = nan_val;
|
||||
_ekf->states[5] = nan_val;
|
||||
usleep(100000);
|
||||
|
||||
// warnx("tripping covariance #1 with NaN values");
|
||||
|
@ -1175,15 +1189,15 @@ int trip_nan() {
|
|||
// usleep(100000);
|
||||
|
||||
warnx("tripping covariance #3 with NaN values");
|
||||
P[3][3] = nan_val; // covariance matrix
|
||||
_ekf->P[3][3] = nan_val; // covariance matrix
|
||||
usleep(100000);
|
||||
|
||||
warnx("tripping Kalman gains with NaN values");
|
||||
Kfusion[0] = nan_val; // Kalman gains
|
||||
_ekf->Kfusion[0] = nan_val; // Kalman gains
|
||||
usleep(100000);
|
||||
|
||||
warnx("tripping stored states[0] with NaN values");
|
||||
storedStates[0][0] = nan_val;
|
||||
_ekf->storedStates[0][0] = nan_val;
|
||||
usleep(100000);
|
||||
|
||||
warnx("\nDONE - FILTER STATE:");
|
||||
|
@ -1231,7 +1245,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
|
|||
if (estimator::g_estimator) {
|
||||
warnx("running");
|
||||
|
||||
print_status();
|
||||
estimator::g_estimator->print_status();
|
||||
|
||||
exit(0);
|
||||
|
||||
|
@ -1242,7 +1256,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
|
|||
|
||||
if (!strcmp(argv[1], "trip")) {
|
||||
if (estimator::g_estimator) {
|
||||
int ret = trip_nan();
|
||||
int ret = estimator::g_estimator->trip_nan();
|
||||
|
||||
exit(ret);
|
||||
|
||||
|
|
|
@ -302,12 +302,19 @@ struct log_PARM_s {
|
|||
/* --- ESTM - ESTIMATOR STATUS --- */
|
||||
#define LOG_ESTM_MSG 132
|
||||
struct log_ESTM_s {
|
||||
float s[32];
|
||||
float s[10];
|
||||
uint8_t n_states;
|
||||
uint8_t states_nan;
|
||||
uint8_t covariance_nan;
|
||||
uint8_t kalman_gain_nan;
|
||||
};
|
||||
// struct log_ESTM_s {
|
||||
// float s[32];
|
||||
// uint8_t n_states;
|
||||
// uint8_t states_nan;
|
||||
// uint8_t covariance_nan;
|
||||
// uint8_t kalman_gain_nan;
|
||||
// };
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
|
@ -341,7 +348,8 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(TIME, "Q", "StartTime"),
|
||||
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
|
||||
LOG_FORMAT(PARM, "Nf", "Name,Value"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
//LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
};
|
||||
|
||||
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
|
||||
|
|
Loading…
Reference in New Issue