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; } }