void print_parameter_settings ()
{
fprintf (stderr, "\tImage size = %i %i\n", cvts.xmax, cvts.ymax);
fprintf (stderr, "\tLowest scale = %i pixels\t\t(-low <integer>)\n", scale_low);
fprintf (stderr, "\tHighest scale = %i pixels\t\t(-high <integer>)\n", scale_high);
fprintf (stderr, "\tNumber of scales = %i\t\t\t(-num <integer>)\n", range);
fprintf (stderr, "\tScale spacing = %f\n", S_I(1) / S_I(0));
fprintf (stderr, "\tKey value = %f\t\t(-key <float>)\n", key);
fprintf (stderr, "\tWhite value = %f\t\t(-white <float>)\n", white);
fprintf (stderr, "\tPhi = %f\t\t(-phi <float>)\n", phi);
fprintf (stderr, "\tThreshold = %f\t\t(-threshold <float>)\n", threshold);
dynamic_range ();
}
void build_gaussian_fft ()
{
int i;
double length = cvts.xmax * cvts.ymax;
double fft_scale = 1. / sqrt (length);
filter_fft = (zomplex**) calloc (range, sizeof (zomplex*));
for (scale = 0; scale < range; scale++)
{
fprintf (stderr, "Computing FFT of Gaussian at scale %i (size %i x %i)%c",
scale, cvts.xmax, cvts.ymax, (char)13);
filter_fft[scale] = (zomplex*) calloc (length, sizeof (zomplex));
gaussian_filter (filter_fft[scale], S_I(scale), k);
compute_fft (filter_fft[scale], cvts.xmax, cvts.ymax);
for (i = 0; i < length; i++)
{
filter_fft[scale][i].re *= fft_scale;
filter_fft[scale][i].im *= fft_scale;
}
}
fprintf (stderr, "\n");
}
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 TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
const struct distance_sensor_s *distance,
const struct vehicle_attitude_s *attitude)
{
// terrain estimate is invalid if we have range sensor timeout
if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {
_terrain_valid = false;
}
if (distance->timestamp > _time_last_distance) {
float d = distance->current_distance;
matrix::Matrix<float, 1, n_x> C;
C(0, 0) = -1; // measured altitude,
float R = 0.009f;
matrix::Vector<float, 1> y;
y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch);
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
S_I(0, 0) += R;
S_I = matrix::inv<float, 1> (S_I);
matrix::Vector<float, 1> r = y - C * _x;
matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
// some sort of outlayer rejection
if (fabsf(distance->current_distance - _distance_last) < 1.0f) {
_x += K * r;
_P -= K * C * _P;
}
// if the current and the last range measurement are bad then we consider the terrain
// estimate to be invalid
if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) {
_terrain_valid = false;
} else {
_terrain_valid = true;
}
_time_last_distance = distance->timestamp;
_distance_last = distance->current_distance;
}
if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) {
matrix::Matrix<float, 1, n_x> C;
C(0, 1) = 1;
float R = 0.056f;
matrix::Vector<float, 1> y;
y(0) = gps->vel_d_m_s;
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
S_I(0, 0) += R;
S_I = matrix::inv<float, 1>(S_I);
matrix::Vector<float, 1> r = y - C * _x;
matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
_x += K * r;
_P -= K * C * _P;
_time_last_gps = gps->timestamp_position;
}
// reinitialise filter if we find bad data
bool reinit = false;
for (int i = 0; i < n_x; i++) {
if (!PX4_ISFINITE(_x(i))) {
reinit = true;
}
}
for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_x; j++) {
if (!PX4_ISFINITE(_P(i, j))) {
reinit = true;
}
}
}
if (reinit) {
memset(&_x._data[0], 0, sizeof(_x._data));
_P.setZero();
_P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f;
}
}
