void BlockLocalPositionEstimator::gpsInit()
{
// measure
Vector<double, n_y_gps> y;
if (gpsMeasure(y) != OK) {
_gpsStats.reset();
return;
}
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
double gpsLatHome = _gpsStats.getMean()(0);
double gpsLonHome = _gpsStats.getMean()(1);
if (!_receivedGps) {
_receivedGps = true;
map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
}
_gpsAltHome = _gpsStats.getMean()(2);
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLatHome,
gpsLonHome,
double(_gpsAltHome));
_gpsInitialized = true;
_gpsFault = FAULT_NONE;
_gpsStats.reset();
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _gpsAltHome;
}
}
}
void BlockLocalPositionEstimator::gpsInit()
{
// check for good gps signal
uint8_t nSat = _sub_gps.get().satellites_used;
float eph = _sub_gps.get().eph;
float epv = _sub_gps.get().epv;
uint8_t fix_type = _sub_gps.get().fix_type;
if (
nSat < 6 ||
eph > _param_lpe_eph_max.get() ||
epv > _param_lpe_epv_max.get() ||
fix_type < 3
) {
_gpsStats.reset();
return;
}
// measure
Vector<double, n_y_gps> y;
if (gpsMeasure(y) != OK) {
_gpsStats.reset();
return;
}
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
// get mean gps values
double gpsLat = _gpsStats.getMean()(0);
double gpsLon = _gpsStats.getMean()(1);
float gpsAlt = _gpsStats.getMean()(2);
_sensorTimeout &= ~SENSOR_GPS;
_sensorFault &= ~SENSOR_GPS;
_gpsStats.reset();
if (!_receivedGps) {
// this is the first time we have received gps
_receivedGps = true;
// note we subtract X_z which is in down directon so it is
// an addition
_gpsAltOrigin = gpsAlt + _x(X_z);
// find lat, lon of current origin by subtracting x and y
// if not using vision position since vision will
// have it's own origin, not necessarily where vehicle starts
if (!_map_ref.init_done && !(_param_lpe_fusion.get() & FUSE_VIS_POS)) {
double gpsLatOrigin = 0;
double gpsLonOrigin = 0;
// reproject at current coordinates
map_projection_init(&_map_ref, gpsLat, gpsLon);
// find origin
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
// reinit origin
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
// always override alt origin on first GPS to fix
// possible baro offset in global altitude at init
_altOrigin = _gpsAltOrigin;
_altOriginInitialized = true;
_altOriginGlobal = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
}
PX4_INFO("[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLat,
gpsLon,
double(gpsAlt));
}
}
}
void BlockLocalPositionEstimator::gpsCorrect()
{
// measure
Vector<double, n_y_gps> y_global;
if (gpsMeasure(y_global) != OK) { return; }
// gps measurement in local frame
double lat = y_global(Y_gps_x);
double lon = y_global(Y_gps_y);
float alt = y_global(Y_gps_z);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, n_y_gps> y;
y.setZero();
y(Y_gps_x) = px;
y(Y_gps_y) = py;
y(Y_gps_z) = pz;
y(Y_gps_vx) = y_global(Y_gps_vx);
y(Y_gps_vy) = y_global(Y_gps_vy);
y(Y_gps_vz) = y_global(Y_gps_vz);
// gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C;
C.setZero();
C(Y_gps_x, X_x) = 1;
C(Y_gps_y, X_y) = 1;
C(Y_gps_z, X_z) = 1;
C(Y_gps_vx, X_vx) = 1;
C(Y_gps_vy, X_vy) = 1;
C(Y_gps_vz, X_vz) = 1;
// gps covariance matrix
SquareMatrix<float, n_y_gps> R;
R.setZero();
// default to parameter, use gps cov if provided
float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get();
// if field is not below minimum, set it to the value provided
if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > _param_lpe_gps_z.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
if (gps_s_stddev > _param_lpe_gps_vxy.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
if (gps_s_stddev > _param_lpe_gps_vz.get()) {
var_vz = gps_s_stddev * gps_s_stddev;
}
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
R(3, 3) = var_vxy;
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x
uint8_t i_hist = 0;
if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
// residual
Vector<float, n_y_gps> r = y - C * x0;
// residual covariance
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 6; i++) {
_pub_innov.get().vel_pos_innov[i] = r(i);
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}
// residual covariance, (inverse)
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
// artifically increase beta threshhold to prevent fault during landing
float beta_thresh = 1e2f;
if (beta / BETA_TABLE[n_y_gps] > beta_thresh) {
if (!(_sensorFault & SENSOR_GPS)) {
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)),
double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5)));
_sensorFault |= SENSOR_GPS;
}
} else if (_sensorFault & SENSOR_GPS) {
_sensorFault &= ~SENSOR_GPS;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction always for GPS
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
void BlockLocalPositionEstimator::gpsCorrect()
{
// measure
Vector<double, n_y_gps> y_global;
if (gpsMeasure(y_global) != OK) { return; }
// gps measurement in local frame
double lat = y_global(0);
double lon = y_global(1);
float alt = y_global(2);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltHome);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, 6> y;
y.setZero();
y(0) = px;
y(1) = py;
y(2) = pz;
y(3) = y_global(3);
y(4) = y_global(4);
y(5) = y_global(5);
// gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C;
C.setZero();
C(Y_gps_x, X_x) = 1;
C(Y_gps_y, X_y) = 1;
C(Y_gps_z, X_z) = 1;
C(Y_gps_vx, X_vx) = 1;
C(Y_gps_vy, X_vy) = 1;
C(Y_gps_vz, X_vz) = 1;
// gps covariance matrix
Matrix<float, n_y_gps, n_y_gps> R;
R.setZero();
// default to parameter, use gps cov if provided
float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get();
float var_z = _gps_z_stddev.get() * _gps_z_stddev.get();
float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
// if field is not zero, set it to the value provided
if (_sub_gps.get().eph > 1e-3f) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > 1e-3f) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
R(3, 3) = var_vxy;
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x and P
float t_delay = 0;
int i = 0;
for (i = 1; i < HIST_LEN; i++) {
t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0));
if (t_delay > _gps_delay.get()) {
break;
}
}
// if you are 3 steps past the delay you wanted, this
// data is probably too old to use
if (t_delay > GPS_DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay));
return;
}
Vector<float, n_x> x0 = _xDelay.get(i);
// residual
Vector<float, n_y_gps> r = y - C * x0;
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_gps]) {
if (_gpsFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
//double(r(0)), double(r(1)), double(r(2)),
//double(r(3)), double(r(4)), double(r(5)));
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
//double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
//double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
_gpsFault = FAULT_MINOR;
}
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction if no hard fault
if (_gpsFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
_x += K * r;
_P -= K * C * _P;
}
}
