int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y) { // measure float d = _sub_lidar->get().current_distance + _lidar_z_offset.get(); warnx("d %10.2g, lidar z offset %10.2g\n", double(d), double(_lidar_z_offset.get())); float eps = 0.01f; float min_dist = _sub_lidar->get().min_distance + eps; float max_dist = _sub_lidar->get().max_distance - eps; // check for bad data if (d > max_dist || d < min_dist) { return -1; } // update stats _lidarStats.update(Scalarf(d)); _time_last_lidar = _timeStamp; y.setZero(); y(0) = d; return OK; }
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y) { // check for agl if (agl() < flow_min_agl) { return -1; } // check quality float qual = _sub_flow.get().quality; if (qual < _flow_min_q.get()) { return -1; } // calculate range to center of image for flow if (!_validTZ) { return -1; } float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); // check for global accuracy if (_gpsInitialized) { double lat = _sub_gps.get().lat * 1.0e-7; double lon = _sub_gps.get().lon * 1.0e-7; float px = 0; float py = 0; map_projection_project(&_map_ref, lat, lon, &px, &py); Vector2f delta(px - _flowX, py - _flowY); if (delta.norm() > 3) { mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow too far from GPS, resetting position"); _flowX = px; _flowY = py; return -1; } } // optical flow in x, y axis float flow_x_rad = _sub_flow.get().pixel_flow_x_integral; float flow_y_rad = _sub_flow.get().pixel_flow_y_integral; // angular rotation in x, y axis float gyro_x_rad = _flow_gyro_x_high_pass.update( _sub_flow.get().gyro_x_rate_integral); float gyro_y_rad = _flow_gyro_y_high_pass.update( _sub_flow.get().gyro_y_rate_integral); //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", //double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d)); // compute velocities in camera frame using ground distance // assume camera frame is body frame Vector3f delta_b( -(flow_x_rad - gyro_x_rad)*d, -(flow_y_rad - gyro_y_rad)*d, 0); // rotation of flow from body to nav frame Matrix3f R_nb(_sub_att.get().R); Vector3f delta_n = R_nb * delta_b; // flow integration _flowX += delta_n(0); _flowY += delta_n(1); // measurement y(Y_flow_x) = _flowX; y(Y_flow_y) = _flowY; _flowQStats.update(Scalarf(_sub_flow.get().quality)); // imporant to timestamp flow even if distance is bad _time_last_flow = _timeStamp; return OK; }
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y) { matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); // check for sane pitch/roll if (euler.phi() > 0.5f || euler.theta() > 0.5f) { return -1; } // check for agl if (agl() < flow_min_agl) { return -1; } // check quality float qual = _sub_flow.get().quality; if (qual < _flow_min_q.get()) { return -1; } // calculate range to center of image for flow if (!(_estimatorInitialized & EST_TZ)) { return -1; } float d = agl() * cosf(euler.phi()) * cosf(euler.theta()); // optical flow in x, y axis // TODO consider making flow scale a states of the kalman filter float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get(); float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get(); float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f; if (dt_flow > 0.5f || dt_flow < 1.0e-6f) { return -1; } // angular rotation in x, y axis float gyro_x_rad = 0; float gyro_y_rad = 0; if (_fusion.get() & FUSE_FLOW_GYRO_COMP) { gyro_x_rad = _flow_gyro_x_high_pass.update( _sub_flow.get().gyro_x_rate_integral); gyro_y_rad = _flow_gyro_y_high_pass.update( _sub_flow.get().gyro_y_rate_integral); } //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", //double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d)); // compute velocities in body frame using ground distance // note that the integral rates in the optical_flow uORB topic are RH rotations about body axes Vector3f delta_b( +(flow_y_rad - gyro_y_rad) * d, -(flow_x_rad - gyro_x_rad) * d, 0); // rotation of flow from body to nav frame Vector3f delta_n = _R_att * delta_b; // imporant to timestamp flow even if distance is bad _time_last_flow = _timeStamp; // measurement y(Y_flow_vx) = delta_n(0) / dt_flow; y(Y_flow_vy) = delta_n(1) / dt_flow; _flowQStats.update(Scalarf(_sub_flow.get().quality)); return OK; }