PX4 Firmware
PX4 Autopilot Software http://px4.io
ekf2_main.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file ekf2_main.cpp
36  * Implementation of the attitude and position estimator.
37  *
38  * @author Roman Bapst
39  */
40 
41 #include <float.h>
42 
43 #include <drivers/drv_hrt.h>
44 #include <lib/ecl/EKF/ekf.h>
45 #include <lib/mathlib/mathlib.h>
46 #include <lib/perf/perf_counter.h>
47 #include <px4_platform_common/defines.h>
48 #include <px4_platform_common/module.h>
49 #include <px4_platform_common/module_params.h>
50 #include <px4_platform_common/posix.h>
51 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
52 #include <px4_platform_common/time.h>
53 #include <uORB/Publication.hpp>
54 #include <uORB/PublicationMulti.hpp>
55 #include <uORB/Subscription.hpp>
56 #include <uORB/SubscriptionCallback.hpp>
57 #include <uORB/topics/airspeed.h>
58 #include <uORB/topics/distance_sensor.h>
59 #include <uORB/topics/ekf2_innovations.h>
60 #include <uORB/topics/ekf2_timestamps.h>
61 #include <uORB/topics/ekf_gps_position.h>
62 #include <uORB/topics/estimator_status.h>
63 #include <uORB/topics/ekf_gps_drift.h>
64 #include <uORB/topics/landing_target_pose.h>
65 #include <uORB/topics/optical_flow.h>
66 #include <uORB/topics/parameter_update.h>
67 #include <uORB/topics/sensor_bias.h>
68 #include <uORB/topics/sensor_combined.h>
69 #include <uORB/topics/sensor_selection.h>
70 #include <uORB/topics/vehicle_air_data.h>
71 #include <uORB/topics/vehicle_attitude.h>
72 #include <uORB/topics/vehicle_global_position.h>
73 #include <uORB/topics/vehicle_gps_position.h>
74 #include <uORB/topics/vehicle_land_detected.h>
75 #include <uORB/topics/vehicle_local_position.h>
76 #include <uORB/topics/vehicle_magnetometer.h>
77 #include <uORB/topics/vehicle_odometry.h>
78 #include <uORB/topics/vehicle_status.h>
79 #include <uORB/topics/wind_estimate.h>
80 
81 #include "Utility/PreFlightChecker.hpp"
82 
83 // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
84 #define BLEND_MASK_USE_SPD_ACC 1
85 #define BLEND_MASK_USE_HPOS_ACC 2
86 #define BLEND_MASK_USE_VPOS_ACC 4
87 
88 // define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
89 #define GPS_MAX_RECEIVERS 2
90 #define GPS_BLENDED_INSTANCE 2
91 
92 using math::constrain;
93 using namespace time_literals;
94 
95 class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::WorkItem
96 {
97 public:
98  explicit Ekf2(bool replay_mode = false);
99  ~Ekf2() override;
100 
101  /** @see ModuleBase */
102  static int task_spawn(int argc, char *argv[]);
103 
104  /** @see ModuleBase */
105  static int custom_command(int argc, char *argv[]);
106 
107  /** @see ModuleBase */
108  static int print_usage(const char *reason = nullptr);
109 
110  void Run() override;
111 
112  bool init();
113 
114  int print_status() override;
115 
116 private:
117  int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor
118  void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data);
119 
120  PreFlightChecker _preflt_checker;
121  void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
122  const vehicle_status_s &vehicle_status,
123  const ekf2_innovations_s &innov);
124  void resetPreFlightChecks();
125 
126  template<typename Param>
127  void update_mag_bias(Param &mag_bias_param, int axis_index);
128 
129  template<typename Param>
130  bool update_mag_decl(Param &mag_decl_param);
131 
132  bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now);
133  bool publish_wind_estimate(const hrt_abstime &timestamp);
134 
135  const Vector3f get_vel_body_wind();
136 
137  /*
138  * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
139  * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
140  * have significant position differences, variation in receiver estimated accuracy will cause undesirable
141  * variation in the position solution.
142  */
143  bool blend_gps_data();
144 
145  /*
146  * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
147  * by calc_blend_weights()
148  * States are written to _gps_state and _gps_blended_state class variables
149  */
150  void update_gps_blend_states();
151 
152  /*
153  * The location in _gps_blended_state will move around as the relative accuracy changes.
154  * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
155  * calculated.
156  */
157  void update_gps_offsets();
158 
159  /*
160  * Apply the steady state physical receiver offsets calculated by update_gps_offsets().
161  */
162  void apply_gps_offsets();
163 
164  /*
165  Calculate GPS output that is a blend of the offset corrected physical receiver data
166  */
167  void calc_gps_blend_output();
168 
169  /*
170  * Calculate filtered WGS84 height from estimated AMSL height
171  */
172  float filter_altitude_ellipsoid(float amsl_hgt);
173 
174  const bool _replay_mode; ///< true when we use replay data from a log
175 
176  // time slip monitoring
177  uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec)
178  uint64_t _start_time_us = 0; ///< system time at EKF start (uSec)
179  int64_t _last_time_slip_us = 0; ///< Last time slip (uSec)
180 
181  perf_counter_t _ekf_update_perf;
182 
183  // Initialise time stamps used to send sensor data to the EKF and for logging
184  uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected
185 
186  // Used to down sample magnetometer data
187  float _mag_data_sum[3] = {}; ///< summed magnetometer readings (Gauss)
188  uint64_t _mag_time_sum_ms = 0; ///< summed magnetoemter time stamps (mSec)
189  uint8_t _mag_sample_count = 0; ///< number of magnetometer measurements summed during downsampling
190  int32_t _mag_time_ms_last_used = 0; ///< time stamp of the last averaged magnetometer measurement sent to the EKF (mSec)
191 
192  // Used to down sample barometer data
193  float _balt_data_sum = 0.0f; ///< summed pressure altitude readings (m)
194  uint64_t _balt_time_sum_ms = 0; ///< summed pressure altitude time stamps (mSec)
195  uint8_t _balt_sample_count = 0; ///< number of barometric altitude measurements summed
196  uint32_t _balt_time_ms_last_used =
197  0; ///< time stamp of the last averaged barometric altitude measurement sent to the EKF (mSec)
198 
199  // Used to check, save and use learned magnetometer biases
200  hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
201  hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save
202 
203  float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (mGauss)
204  bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
205  float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2)
206 
207  // Used to control saving of mag declination to be used on next startup
208  bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
209 
210  // set pose/velocity as invalid if standard deviation is bigger than max_std_dev
211  // TODO: the user should be allowed to set these values by a parameter
212  static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position
213  static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
214  //static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity
215 
216  // GPS blending and switching
217  gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
218  gps_message _gps_blended_state{}; ///< internal state data for the blended GPS
219  gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
220  Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
221  float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
222  Vector3f _blended_antenna_offset = {}; ///< blended antenna offset
223  float _blend_weights[GPS_MAX_RECEIVERS] = {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
224  uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {}; ///< the previous value of time_us for that GPS instance - used to detect new data.
225  uint8_t _gps_best_index = 0; ///< index of the physical receiver with the lowest reported error
226  uint8_t _gps_select_index = 0; ///< 0 = GPS1, 1 = GPS2, 2 = blended
227  uint8_t _gps_time_ref_index =
228  0; ///< index of the receiver that is used as the timing reference for the blending update
229  uint8_t _gps_oldest_index = 0; ///< index of the physical receiver with the oldest data
230  uint8_t _gps_newest_index = 0; ///< index of the physical receiver with the newest data
231  uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate
232  float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds.
233  bool _gps_new_output_data = false; ///< true if there is new output data for the EKF
234  bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate
235 
236  int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
237  uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
238  float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
239 
240  bool _imu_bias_reset_request{false};
241 
242  // republished aligned external visual odometry
243  bool new_ev_data_received = false;
244  vehicle_odometry_s _ev_odom{};
245 
246  uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
247  uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
248  uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
249  uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
250  uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
251  uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
252  uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
253  uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
254  uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
255  uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
256 
257  uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
258 
259  // because we can have several distance sensor instances with different orientations
260  uORB::Subscription _range_finder_subs[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}};
261  int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
262 
263  // because we can have multiple GPS instances
264  uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}};
265 
266  sensor_selection_s _sensor_selection{};
267  vehicle_land_detected_s _vehicle_land_detected{};
268  vehicle_status_s _vehicle_status{};
269 
270  uORB::Publication<ekf2_innovations_s> _estimator_innovations_pub{ORB_ID(ekf2_innovations)};
271  uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
272  uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
273  uORB::Publication<ekf_gps_position_s> _blended_gps_pub{ORB_ID(ekf_gps_position)};
274  uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
275  uORB::Publication<sensor_bias_s> _sensor_bias_pub{ORB_ID(sensor_bias)};
276  uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
277  uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
278  uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
279  uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
280  uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
281  uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
282 
283  Ekf _ekf;
284 
285  parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
286 
287  DEFINE_PARAMETERS(
288  (ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
289  _param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
290  (ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
291  _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec)
292  (ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
293  _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
294  (ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
295  _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
296  (ParamExtFloat<px4::params::EKF2_OF_DELAY>)
297  _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
298  (ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
299  _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
300  (ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
301  _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
302  (ParamExtFloat<px4::params::EKF2_EV_DELAY>)
303  _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
304  (ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
305  _param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec)
306 
307  (ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
308  _param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
309  (ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
310  _param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
311 
312  // process noise
313  (ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
314  _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
315  (ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
316  _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
317  (ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>)
318  _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
319  (ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
320  _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
321  (ParamExtFloat<px4::params::EKF2_WIND_NOISE>)
322  _param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2)
323  (ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
324  (ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
325  _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
326 
327  (ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
328  _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
329  (ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
330  _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m)
331  (ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
332  _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m)
333  (ParamExtFloat<px4::params::EKF2_BARO_NOISE>)
334  _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m)
335  (ParamExtFloat<px4::params::EKF2_BARO_GATE>)
336  _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD)
337  (ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>)
338  _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m)
339  (ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>)
340  _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
341  (ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
342  _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD)
343  (ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
344  _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD)
345  (ParamExtFloat<px4::params::EKF2_TAS_GATE>)
346  _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
347 
348  // control of magnetometer fusion
349  (ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
350  _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad)
351  (ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
352  _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
353  (ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
354  _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
355  (ParamExtFloat<px4::params::EKF2_BETA_GATE>)
356  _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
357  (ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
358  (ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
359  (ParamExtFloat<px4::params::EKF2_HDG_GATE>)
360  _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
361  (ParamExtFloat<px4::params::EKF2_MAG_GATE>)
362  _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD)
363  (ParamExtInt<px4::params::EKF2_DECL_TYPE>)
364  _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data
365  (ParamExtInt<px4::params::EKF2_MAG_TYPE>)
366  _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used
367  (ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>)
368  _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used
369  (ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
370  _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec)
371 
372  (ParamExtInt<px4::params::EKF2_GPS_CHECK>)
373  _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used
374  (ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m)
375  (ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m)
376  (ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s)
377  (ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats, ///< minimum acceptable satellite count
378  (ParamExtFloat<px4::params::EKF2_REQ_PDOP>)
379  _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision
380  (ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
381  _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s)
382  (ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s)
383 
384  // measurement source control
385  (ParamExtInt<px4::params::EKF2_AID_MASK>)
386  _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
387  (ParamExtInt<px4::params::EKF2_HGT_MODE>) _param_ekf2_hgt_mode, ///< selects the primary source for height data
388  (ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
389  _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
390 
391  // range finder fusion
392  (ParamExtFloat<px4::params::EKF2_RNG_NOISE>)
393  _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m)
394  (ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m)
395  (ParamExtFloat<px4::params::EKF2_RNG_GATE>)
396  _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD)
397  (ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m)
398  (ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad)
399  (ParamExtInt<px4::params::EKF2_RNG_AID>)
400  _param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder
401  (ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>)
402  _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s)
403  (ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
404  _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m)
405  (ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>)
406  _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
407 
408  // vision estimate fusion
409  (ParamInt<px4::params::EKF2_EV_NOISE_MD>)
410  _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
411  (ParamFloat<px4::params::EKF2_EVP_NOISE>)
412  _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
413  (ParamFloat<px4::params::EKF2_EVV_NOISE>)
414  _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
415  (ParamFloat<px4::params::EKF2_EVA_NOISE>)
416  _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
417  (ParamExtFloat<px4::params::EKF2_EVV_GATE>)
418  _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
419  (ParamExtFloat<px4::params::EKF2_EVP_GATE>)
420  _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
421 
422  // optical flow fusion
423  (ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
424  _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
425  (ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
426  _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
427  (ParamExtInt<px4::params::EKF2_OF_QMIN>)
428  _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
429  (ParamExtFloat<px4::params::EKF2_OF_GATE>)
430  _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
431 
432  // sensor positions in body frame
433  (ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
434  (ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
435  (ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
436  (ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
437  (ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
438  (ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
439  (ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
440  (ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
441  (ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
442  (ParamExtFloat<px4::params::EKF2_OF_POS_X>)
443  _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
444  (ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
445  _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
446  (ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
447  _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
448  (ParamExtFloat<px4::params::EKF2_EV_POS_X>)
449  _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
450  (ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
451  _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
452  (ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
453  _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
454 
455  // control of airspeed and sideslip fusion
456  (ParamFloat<px4::params::EKF2_ARSP_THR>)
457  _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
458  (ParamInt<px4::params::EKF2_FUSE_BETA>)
459  _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
460 
461  // output predictor filter time constants
462  (ParamExtFloat<px4::params::EKF2_TAU_VEL>)
463  _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec)
464  (ParamExtFloat<px4::params::EKF2_TAU_POS>)
465  _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec)
466 
467  // IMU switch on bias parameters
468  (ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
469  _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
470  (ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
471  _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
472  (ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
473  _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
474 
475  // EKF saved XYZ magnetometer bias values
476  (ParamFloat<px4::params::EKF2_MAGBIAS_X>) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss)
477  (ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss)
478  (ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss)
479  (ParamInt<px4::params::EKF2_MAGBIAS_ID>)
480  _param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values
481  (ParamFloat<px4::params::EKF2_MAGB_VREF>)
482  _param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
483  (ParamFloat<px4::params::EKF2_MAGB_K>)
484  _param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm
485 
486  // EKF accel bias learning control
487  (ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2)
488  (ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
489  _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
490  (ParamExtFloat<px4::params::EKF2_ABL_GYRLIM>)
491  _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
492  (ParamExtFloat<px4::params::EKF2_ABL_TAU>)
493  _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
494 
495  // Multi-rotor drag specific force fusion
496  (ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
497  _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
498  (ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
499  (ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
500 
501  // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
502  // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
503  (ParamFloat<px4::params::EKF2_ASPD_MAX>)
504  _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2)
505  (ParamFloat<px4::params::EKF2_PCOEF_XP>)
506  _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis
507  (ParamFloat<px4::params::EKF2_PCOEF_XN>)
508  _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis
509  (ParamFloat<px4::params::EKF2_PCOEF_YP>)
510  _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis
511  (ParamFloat<px4::params::EKF2_PCOEF_YN>)
512  _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis
513  (ParamFloat<px4::params::EKF2_PCOEF_Z>)
514  _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
515 
516  // GPS blending
517  (ParamInt<px4::params::EKF2_GPS_MASK>)
518  _param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
519  (ParamFloat<px4::params::EKF2_GPS_TAU>)
520  _param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
521 
522  // Test used to determine if the vehicle is static or moving
523  (ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
524  _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
525 
526  (ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
527  (ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check ///< Mag field strength check
528 
529  )
530 
531 };
532 
533 Ekf2::Ekf2(bool replay_mode):
534  ModuleParams(nullptr),
535  WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
536  _replay_mode(replay_mode),
537  _ekf_update_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": update")),
538  _params(_ekf.getParamHandle()),
539  _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
540  _param_ekf2_mag_delay(_params->mag_delay_ms),
541  _param_ekf2_baro_delay(_params->baro_delay_ms),
542  _param_ekf2_gps_delay(_params->gps_delay_ms),
543  _param_ekf2_of_delay(_params->flow_delay_ms),
544  _param_ekf2_rng_delay(_params->range_delay_ms),
545  _param_ekf2_asp_delay(_params->airspeed_delay_ms),
546  _param_ekf2_ev_delay(_params->ev_delay_ms),
547  _param_ekf2_avel_delay(_params->auxvel_delay_ms),
548  _param_ekf2_gyr_noise(_params->gyro_noise),
549  _param_ekf2_acc_noise(_params->accel_noise),
550  _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
551  _param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
552  _param_ekf2_mag_e_noise(_params->mage_p_noise),
553  _param_ekf2_mag_b_noise(_params->magb_p_noise),
554  _param_ekf2_wind_noise(_params->wind_vel_p_noise),
555  _param_ekf2_terr_noise(_params->terrain_p_noise),
556  _param_ekf2_terr_grad(_params->terrain_gradient),
557  _param_ekf2_gps_v_noise(_params->gps_vel_noise),
558  _param_ekf2_gps_p_noise(_params->gps_pos_noise),
559  _param_ekf2_noaid_noise(_params->pos_noaid_noise),
560  _param_ekf2_baro_noise(_params->baro_noise),
561  _param_ekf2_baro_gate(_params->baro_innov_gate),
562  _param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
563  _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
564  _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
565  _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
566  _param_ekf2_tas_gate(_params->tas_innov_gate),
567  _param_ekf2_head_noise(_params->mag_heading_noise),
568  _param_ekf2_mag_noise(_params->mag_noise),
569  _param_ekf2_eas_noise(_params->eas_noise),
570  _param_ekf2_beta_gate(_params->beta_innov_gate),
571  _param_ekf2_beta_noise(_params->beta_noise),
572  _param_ekf2_mag_decl(_params->mag_declination_deg),
573  _param_ekf2_hdg_gate(_params->heading_innov_gate),
574  _param_ekf2_mag_gate(_params->mag_innov_gate),
575  _param_ekf2_decl_type(_params->mag_declination_source),
576  _param_ekf2_mag_type(_params->mag_fusion_type),
577  _param_ekf2_mag_acclim(_params->mag_acc_gate),
578  _param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
579  _param_ekf2_gps_check(_params->gps_check_mask),
580  _param_ekf2_req_eph(_params->req_hacc),
581  _param_ekf2_req_epv(_params->req_vacc),
582  _param_ekf2_req_sacc(_params->req_sacc),
583  _param_ekf2_req_nsats(_params->req_nsats),
584  _param_ekf2_req_pdop(_params->req_pdop),
585  _param_ekf2_req_hdrift(_params->req_hdrift),
586  _param_ekf2_req_vdrift(_params->req_vdrift),
587  _param_ekf2_aid_mask(_params->fusion_mode),
588  _param_ekf2_hgt_mode(_params->vdist_sensor_type),
589  _param_ekf2_noaid_tout(_params->valid_timeout_max),
590  _param_ekf2_rng_noise(_params->range_noise),
591  _param_ekf2_rng_sfe(_params->range_noise_scaler),
592  _param_ekf2_rng_gate(_params->range_innov_gate),
593  _param_ekf2_min_rng(_params->rng_gnd_clearance),
594  _param_ekf2_rng_pitch(_params->rng_sens_pitch),
595  _param_ekf2_rng_aid(_params->range_aid),
596  _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
597  _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
598  _param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
599  _param_ekf2_evv_gate(_params->ev_vel_innov_gate),
600  _param_ekf2_evp_gate(_params->ev_pos_innov_gate),
601  _param_ekf2_of_n_min(_params->flow_noise),
602  _param_ekf2_of_n_max(_params->flow_noise_qual_min),
603  _param_ekf2_of_qmin(_params->flow_qual_min),
604  _param_ekf2_of_gate(_params->flow_innov_gate),
605  _param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
606  _param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
607  _param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
608  _param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
609  _param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
610  _param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
611  _param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
612  _param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
613  _param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
614  _param_ekf2_of_pos_x(_params->flow_pos_body(0)),
615  _param_ekf2_of_pos_y(_params->flow_pos_body(1)),
616  _param_ekf2_of_pos_z(_params->flow_pos_body(2)),
617  _param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
618  _param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
619  _param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
620  _param_ekf2_tau_vel(_params->vel_Tau),
621  _param_ekf2_tau_pos(_params->pos_Tau),
622  _param_ekf2_gbias_init(_params->switch_on_gyro_bias),
623  _param_ekf2_abias_init(_params->switch_on_accel_bias),
624  _param_ekf2_angerr_init(_params->initial_tilt_err),
625  _param_ekf2_abl_lim(_params->acc_bias_lim),
626  _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
627  _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
628  _param_ekf2_abl_tau(_params->acc_bias_learn_tc),
629  _param_ekf2_drag_noise(_params->drag_noise),
630  _param_ekf2_bcoef_x(_params->bcoef_x),
631  _param_ekf2_bcoef_y(_params->bcoef_y),
632  _param_ekf2_move_test(_params->is_moving_scaler),
633  _param_ekf2_mag_check(_params->check_mag_strength)
634 {
635  // initialise parameter cache
636  updateParams();
637 
638  _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
639 }
640 
641 Ekf2::~Ekf2()
642 {
643  perf_free(_ekf_update_perf);
644 }
645 
646 bool
647 Ekf2::init()
648 {
649  if (!_sensors_sub.registerCallback()) {
650  PX4_ERR("sensor combined callback registration failed!");
651  return false;
652  }
653 
654  return true;
655 }
656 
657 int Ekf2::print_status()
658 {
659  PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid");
660  PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid");
661 
662  PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us);
663 
664  perf_print_counter(_ekf_update_perf);
665 
666  return 0;
667 }
668 
669 template<typename Param>
670 void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index)
671 {
672  if (_valid_cal_available[axis_index]) {
673 
674  // calculate weighting using ratio of variances and update stored bias values
675  const float weighting = constrain(_param_ekf2_magb_vref.get() / (_param_ekf2_magb_vref.get() +
676  _last_valid_variance[axis_index]), 0.0f, _param_ekf2_magb_k.get());
677  const float mag_bias_saved = mag_bias_param.get();
678 
679  _last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved;
680 
681  mag_bias_param.set(_last_valid_mag_cal[axis_index]);
682  mag_bias_param.commit_no_notification();
683 
684  _valid_cal_available[axis_index] = false;
685  }
686 }
687 
688 template<typename Param>
689 bool Ekf2::update_mag_decl(Param &mag_decl_param)
690 {
691  // update stored declination value
692  float declination_deg;
693 
694  if (_ekf.get_mag_decl_deg(&declination_deg)) {
695  mag_decl_param.set(declination_deg);
696  mag_decl_param.commit_no_notification();
697  return true;
698  }
699 
700  return false;
701 }
702 
703 void Ekf2::Run()
704 {
705  if (should_exit()) {
706  _sensors_sub.unregisterCallback();
707  exit_and_cleanup();
708  return;
709  }
710 
711  sensor_combined_s sensors;
712 
713  if (_sensors_sub.update(&sensors)) {
714 
715  // check for parameter updates
716  if (_parameter_update_sub.updated()) {
717  // clear update
718  parameter_update_s pupdate;
719  _parameter_update_sub.copy(&pupdate);
720 
721  // update parameters from storage
722  updateParams();
723  }
724 
725  // ekf2_timestamps (using 0.1 ms relative timestamps)
726  ekf2_timestamps_s ekf2_timestamps{};
727  ekf2_timestamps.timestamp = sensors.timestamp;
728 
729  ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
730  ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
731  ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
732  ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
733  ekf2_timestamps.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
734  ekf2_timestamps.vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
735  ekf2_timestamps.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
736 
737  // update all other topics if they have new data
738  if (_status_sub.update(&_vehicle_status)) {
739 
740  const bool is_fixed_wing = (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
741 
742  // only fuse synthetic sideslip measurements if conditions are met
743  _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
744 
745  // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
746  _ekf.set_is_fixed_wing(is_fixed_wing);
747  }
748 
749  // Always update sensor selction first time through if time stamp is non zero
750  if (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0)) {
751  const sensor_selection_s sensor_selection_prev = _sensor_selection;
752 
753  if (_sensor_selection_sub.copy(&_sensor_selection)) {
754  if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
755  if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
756  PX4_WARN("accel id changed, resetting IMU bias");
757  _imu_bias_reset_request = true;
758  }
759 
760  if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
761  PX4_WARN("gyro id changed, resetting IMU bias");
762  _imu_bias_reset_request = true;
763  }
764  }
765  }
766  }
767 
768  // attempt reset until successful
769  if (_imu_bias_reset_request) {
770  _imu_bias_reset_request = !_ekf.reset_imu_bias();
771  }
772 
773  const hrt_abstime now = sensors.timestamp;
774 
775  // push imu data into estimator
776  imuSample imu_sample_new;
777  imu_sample_new.time_us = now;
778  imu_sample_new.delta_ang_dt = sensors.gyro_integral_dt * 1.e-6f;
779  imu_sample_new.delta_ang = Vector3f{sensors.gyro_rad} * imu_sample_new.delta_ang_dt;
780  imu_sample_new.delta_vel_dt = sensors.accelerometer_integral_dt * 1.e-6f;
781  imu_sample_new.delta_vel = Vector3f{sensors.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
782 
783  _ekf.setIMUData(imu_sample_new);
784 
785  // publish attitude immediately (uses quaternion from output predictor)
786  publish_attitude(sensors, now);
787 
788  // read mag data
789  if (_magnetometer_sub.updated()) {
790  vehicle_magnetometer_s magnetometer;
791 
792  if (_magnetometer_sub.copy(&magnetometer)) {
793  // Reset learned bias parameters if there has been a persistant change in magnetometer ID
794  // Do not reset parmameters when armed to prevent potential time slips casued by parameter set
795  // and notification events
796  // Check if there has been a persistant change in magnetometer ID
797  if (_sensor_selection.mag_device_id != 0
798  && (_sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
799 
800  if (_invalid_mag_id_count < 200) {
801  _invalid_mag_id_count++;
802  }
803 
804  } else {
805  if (_invalid_mag_id_count > 0) {
806  _invalid_mag_id_count--;
807  }
808  }
809 
810  if ((_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) {
811  // the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID
812  // this means we need to reset the learned bias values to zero
813  _param_ekf2_magbias_x.set(0.f);
814  _param_ekf2_magbias_x.commit_no_notification();
815  _param_ekf2_magbias_y.set(0.f);
816  _param_ekf2_magbias_y.commit_no_notification();
817  _param_ekf2_magbias_z.set(0.f);
818  _param_ekf2_magbias_z.commit_no_notification();
819  _param_ekf2_magbias_id.set(_sensor_selection.mag_device_id);
820  _param_ekf2_magbias_id.commit();
821 
822  _invalid_mag_id_count = 0;
823 
824  PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get());
825  }
826 
827  // If the time last used by the EKF is less than specified, then accumulate the
828  // data and push the average when the specified interval is reached.
829  _mag_time_sum_ms += magnetometer.timestamp / 1000;
830  _mag_sample_count++;
831  _mag_data_sum[0] += magnetometer.magnetometer_ga[0];
832  _mag_data_sum[1] += magnetometer.magnetometer_ga[1];
833  _mag_data_sum[2] += magnetometer.magnetometer_ga[2];
834  int32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count;
835 
836  if ((mag_time_ms - _mag_time_ms_last_used) > _params->sensor_interval_min_ms) {
837  const float mag_sample_count_inv = 1.0f / _mag_sample_count;
838  // calculate mean of measurements and correct for learned bias offsets
839  float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv - _param_ekf2_magbias_x.get(),
840  _mag_data_sum[1] *mag_sample_count_inv - _param_ekf2_magbias_y.get(),
841  _mag_data_sum[2] *mag_sample_count_inv - _param_ekf2_magbias_z.get()
842  };
843 
844  _ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);
845 
846  _mag_time_ms_last_used = mag_time_ms;
847  _mag_time_sum_ms = 0;
848  _mag_sample_count = 0;
849  _mag_data_sum[0] = 0.0f;
850  _mag_data_sum[1] = 0.0f;
851  _mag_data_sum[2] = 0.0f;
852  }
853 
854  ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
855  (int64_t)ekf2_timestamps.timestamp / 100);
856  }
857  }
858 
859  // read baro data
860  if (_airdata_sub.updated()) {
861  vehicle_air_data_s airdata;
862 
863  if (_airdata_sub.copy(&airdata)) {
864  // If the time last used by the EKF is less than specified, then accumulate the
865  // data and push the average when the specified interval is reached.
866  _balt_time_sum_ms += airdata.timestamp / 1000;
867  _balt_sample_count++;
868  _balt_data_sum += airdata.baro_alt_meter;
869  uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;
870 
871  if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) {
872  // take mean across sample period
873  float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
874 
875  _ekf.set_air_density(airdata.rho);
876 
877  // calculate static pressure error = Pmeas - Ptruth
878  // model position error sensitivity as a body fixed ellipse with a different scale in the positive and
879  // negative X and Y directions
880  const Vector3f vel_body_wind = get_vel_body_wind();
881 
882  float K_pstatic_coef_x;
883 
884  if (vel_body_wind(0) >= 0.0f) {
885  K_pstatic_coef_x = _param_ekf2_pcoef_xp.get();
886 
887  } else {
888  K_pstatic_coef_x = _param_ekf2_pcoef_xn.get();
889  }
890 
891  float K_pstatic_coef_y;
892 
893  if (vel_body_wind(1) >= 0.0f) {
894  K_pstatic_coef_y = _param_ekf2_pcoef_yp.get();
895 
896  } else {
897  K_pstatic_coef_y = _param_ekf2_pcoef_yn.get();
898  }
899 
900  const float max_airspeed_sq = _param_ekf2_aspd_max.get() * _param_ekf2_aspd_max.get();
901  const float x_v2 = fminf(vel_body_wind(0) * vel_body_wind(0), max_airspeed_sq);
902  const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq);
903  const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq);
904 
905  const float pstatic_err = 0.5f * airdata.rho * (
906  K_pstatic_coef_x * x_v2 + K_pstatic_coef_y * y_v2 + _param_ekf2_pcoef_z.get() * z_v2);
907 
908  // correct baro measurement using pressure error estimate and assuming sea level gravity
909  balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);
910 
911  // push to estimator
912  _ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
913 
914  _balt_time_ms_last_used = balt_time_ms;
915  _balt_time_sum_ms = 0;
916  _balt_sample_count = 0;
917  _balt_data_sum = 0.0f;
918  }
919 
920  ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
921  (int64_t)ekf2_timestamps.timestamp / 100);
922  }
923  }
924 
925  // read gps1 data if available
926  bool gps1_updated = _gps_subs[0].updated();
927 
928  if (gps1_updated) {
929  vehicle_gps_position_s gps;
930 
931  if (_gps_subs[0].copy(&gps)) {
932  fillGpsMsgWithVehicleGpsPosData(_gps_state[0], gps);
933  _gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid;
934 
935  ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
936  }
937  }
938 
939  // check for second GPS receiver data
940  bool gps2_updated = _gps_subs[1].updated();
941 
942  if (gps2_updated) {
943  vehicle_gps_position_s gps;
944 
945  if (_gps_subs[1].copy(&gps)) {
946  fillGpsMsgWithVehicleGpsPosData(_gps_state[1], gps);
947  _gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid;
948  }
949  }
950 
951  if ((_param_ekf2_gps_mask.get() == 0) && gps1_updated) {
952  // When GPS blending is disabled we always use the first receiver instance
953  _ekf.setGpsData(_gps_state[0].time_usec, _gps_state[0]);
954 
955  } else if ((_param_ekf2_gps_mask.get() > 0) && (gps1_updated || gps2_updated)) {
956  // blend dual receivers if available
957 
958  // calculate blending weights
959  if (!blend_gps_data()) {
960  // handle case where the blended states cannot be updated
961  if (_gps_state[0].fix_type > _gps_state[1].fix_type) {
962  // GPS 1 has the best fix status so use that
963  _gps_select_index = 0;
964 
965  } else if (_gps_state[1].fix_type > _gps_state[0].fix_type) {
966  // GPS 2 has the best fix status so use that
967  _gps_select_index = 1;
968 
969  } else if (_gps_select_index == 2) {
970  // use last receiver we received data from
971  if (gps1_updated) {
972  _gps_select_index = 0;
973 
974  } else if (gps2_updated) {
975  _gps_select_index = 1;
976  }
977  }
978 
979  // Only use selected receiver data if it has been updated
980  _gps_new_output_data = (gps1_updated && _gps_select_index == 0) ||
981  (gps2_updated && _gps_select_index == 1);
982  }
983 
984  if (_gps_new_output_data) {
985  // correct the _gps_state data for steady state offsets and write to _gps_output
986  apply_gps_offsets();
987 
988  // calculate a blended output from the offset corrected receiver data
989  if (_gps_select_index == 2) {
990  calc_gps_blend_output();
991  }
992 
993  // write selected GPS to EKF
994  _ekf.setGpsData(_gps_output[_gps_select_index].time_usec, _gps_output[_gps_select_index]);
995 
996  // log blended solution as a third GPS instance
997  ekf_gps_position_s gps;
998  gps.timestamp = _gps_output[_gps_select_index].time_usec;
999  gps.lat = _gps_output[_gps_select_index].lat;
1000  gps.lon = _gps_output[_gps_select_index].lon;
1001  gps.alt = _gps_output[_gps_select_index].alt;
1002  gps.fix_type = _gps_output[_gps_select_index].fix_type;
1003  gps.eph = _gps_output[_gps_select_index].eph;
1004  gps.epv = _gps_output[_gps_select_index].epv;
1005  gps.s_variance_m_s = _gps_output[_gps_select_index].sacc;
1006  gps.vel_m_s = _gps_output[_gps_select_index].vel_m_s;
1007  gps.vel_n_m_s = _gps_output[_gps_select_index].vel_ned[0];
1008  gps.vel_e_m_s = _gps_output[_gps_select_index].vel_ned[1];
1009  gps.vel_d_m_s = _gps_output[_gps_select_index].vel_ned[2];
1010  gps.vel_ned_valid = _gps_output[_gps_select_index].vel_ned_valid;
1011  gps.satellites_used = _gps_output[_gps_select_index].nsats;
1012  gps.heading = _gps_output[_gps_select_index].yaw;
1013  gps.heading_offset = _gps_output[_gps_select_index].yaw_offset;
1014  gps.selected = _gps_select_index;
1015 
1016  // Publish to the EKF blended GPS topic
1017  _blended_gps_pub.publish(gps);
1018 
1019  // clear flag to avoid re-use of the same data
1020  _gps_new_output_data = false;
1021  }
1022  }
1023 
1024  if (_airspeed_sub.updated()) {
1025  airspeed_s airspeed;
1026 
1027  if (_airspeed_sub.copy(&airspeed)) {
1028  // only set airspeed data if condition for airspeed fusion are met
1029  if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
1030 
1031  const float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
1032  _ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
1033  }
1034 
1035  ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
1036  (int64_t)ekf2_timestamps.timestamp / 100);
1037  }
1038  }
1039 
1040  if (_optical_flow_sub.updated()) {
1041  optical_flow_s optical_flow;
1042 
1043  if (_optical_flow_sub.copy(&optical_flow)) {
1044  flow_message flow;
1045  flow.flowdata(0) = optical_flow.pixel_flow_x_integral;
1046  flow.flowdata(1) = optical_flow.pixel_flow_y_integral;
1047  flow.quality = optical_flow.quality;
1048  flow.gyrodata(0) = optical_flow.gyro_x_rate_integral;
1049  flow.gyrodata(1) = optical_flow.gyro_y_rate_integral;
1050  flow.gyrodata(2) = optical_flow.gyro_z_rate_integral;
1051  flow.dt = optical_flow.integration_timespan;
1052 
1053  if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
1054  PX4_ISFINITE(optical_flow.pixel_flow_x_integral)) {
1055 
1056  _ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
1057  }
1058 
1059  // Save sensor limits reported by the optical flow sensor
1060  _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
1061  optical_flow.max_ground_distance);
1062 
1063  ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
1064  (int64_t)ekf2_timestamps.timestamp / 100);
1065  }
1066  }
1067 
1068  if (_range_finder_sub_index >= 0) {
1069 
1070  if (_range_finder_subs[_range_finder_sub_index].updated()) {
1071  distance_sensor_s range_finder;
1072 
1073  if (_range_finder_subs[_range_finder_sub_index].copy(&range_finder)) {
1074  _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance, range_finder.signal_quality);
1075 
1076  // Save sensor limits reported by the rangefinder
1077  _ekf.set_rangefinder_limits(range_finder.min_distance, range_finder.max_distance);
1078 
1079  ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
1080  (int64_t)ekf2_timestamps.timestamp / 100);
1081  }
1082  }
1083 
1084  } else {
1085  _range_finder_sub_index = getRangeSubIndex();
1086  }
1087 
1088  // get external vision data
1089  // if error estimates are unavailable, use parameter defined defaults
1090  new_ev_data_received = false;
1091 
1092  if (_ev_odom_sub.updated()) {
1093  new_ev_data_received = true;
1094 
1095  // copy both attitude & position, we need both to fill a single ext_vision_message
1096  _ev_odom_sub.copy(&_ev_odom);
1097 
1098  ext_vision_message ev_data;
1099 
1100  // check for valid velocity data
1101  if (PX4_ISFINITE(_ev_odom.vx) && PX4_ISFINITE(_ev_odom.vy) && PX4_ISFINITE(_ev_odom.vz)) {
1102  ev_data.vel(0) = _ev_odom.vx;
1103  ev_data.vel(1) = _ev_odom.vy;
1104  ev_data.vel(2) = _ev_odom.vz;
1105 
1106  // velocity measurement error from ev_data or parameters
1107  if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])) {
1108  ev_data.velErr = fmaxf(_param_ekf2_evv_noise.get(),
1109  sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE],
1110  fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE],
1111  _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]))));
1112 
1113  } else {
1114  ev_data.velErr = _param_ekf2_evv_noise.get();
1115  }
1116  }
1117 
1118  // check for valid position data
1119  if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) {
1120  ev_data.pos(0) = _ev_odom.x;
1121  ev_data.pos(1) = _ev_odom.y;
1122  ev_data.pos(2) = _ev_odom.z;
1123 
1124  // position measurement error from ev_data or parameters
1125  if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {
1126  ev_data.posErr = fmaxf(_param_ekf2_evp_noise.get(),
1127  sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
1128  _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
1129  ev_data.hgtErr = fmaxf(_param_ekf2_evp_noise.get(),
1130  sqrtf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]));
1131 
1132  } else {
1133  ev_data.posErr = _param_ekf2_evp_noise.get();
1134  ev_data.hgtErr = _param_ekf2_evp_noise.get();
1135  }
1136  }
1137 
1138  // check for valid orientation data
1139  if (PX4_ISFINITE(_ev_odom.q[0])) {
1140  ev_data.quat = matrix::Quatf(_ev_odom.q);
1141 
1142  // orientation measurement error from ev_data or parameters
1143  if (!_param_ekf2_ev_noise_md.get()
1144  && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
1145  ev_data.angErr = fmaxf(_param_ekf2_eva_noise.get(),
1146  sqrtf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]));
1147 
1148  } else {
1149  ev_data.angErr = _param_ekf2_eva_noise.get();
1150  }
1151  }
1152 
1153  // only set data if all positions and orientation are valid
1154  if (ev_data.posErr < ep_max_std_dev && ev_data.angErr < eo_max_std_dev) {
1155  // use timestamp from external computer, clocks are synchronized when using MAVROS
1156  _ekf.setExtVisionData(_ev_odom.timestamp, &ev_data);
1157  }
1158 
1159  ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)_ev_odom.timestamp / 100 -
1160  (int64_t)ekf2_timestamps.timestamp / 100);
1161  }
1162 
1163  bool vehicle_land_detected_updated = _vehicle_land_detected_sub.updated();
1164 
1165  if (vehicle_land_detected_updated) {
1166  if (_vehicle_land_detected_sub.copy(&_vehicle_land_detected)) {
1167  _ekf.set_in_air_status(!_vehicle_land_detected.landed);
1168  }
1169  }
1170 
1171  // use the landing target pose estimate as another source of velocity data
1172  if (_landing_target_pose_sub.updated()) {
1173  landing_target_pose_s landing_target_pose;
1174 
1175  if (_landing_target_pose_sub.copy(&landing_target_pose)) {
1176  // we can only use the landing target if it has a fixed position and a valid velocity estimate
1177  if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
1178  // velocity of vehicle relative to target has opposite sign to target relative to vehicle
1179  float velocity[2] = { -landing_target_pose.vx_rel, -landing_target_pose.vy_rel};
1180  float variance[2] = {landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel};
1181  _ekf.setAuxVelData(landing_target_pose.timestamp, velocity, variance);
1182  }
1183  }
1184  }
1185 
1186  // run the EKF update and output
1187  perf_begin(_ekf_update_perf);
1188  const bool updated = _ekf.update();
1189  perf_end(_ekf_update_perf);
1190 
1191  // integrate time to monitor time slippage
1192  if (_start_time_us == 0) {
1193  _start_time_us = now;
1194  _last_time_slip_us = 0;
1195 
1196  } else if (_start_time_us > 0) {
1197  _integrated_time_us += sensors.gyro_integral_dt;
1198  _last_time_slip_us = (now - _start_time_us) - _integrated_time_us;
1199  }
1200 
1201  if (updated) {
1202 
1203  filter_control_status_u control_status;
1204  _ekf.get_control_mode(&control_status.value);
1205 
1206  // only publish position after successful alignment
1207  if (control_status.flags.tilt_align) {
1208  // generate vehicle local position data
1209  vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
1210 
1211  // generate vehicle odometry data
1212  vehicle_odometry_s odom{};
1213 
1214  lpos.timestamp = now;
1215  odom.timestamp = lpos.timestamp;
1216 
1217  odom.local_frame = odom.LOCAL_FRAME_NED;
1218 
1219  // Position of body origin in local NED frame
1220  float position[3];
1221  _ekf.get_position(position);
1222  const float lpos_x_prev = lpos.x;
1223  const float lpos_y_prev = lpos.y;
1224  lpos.x = (_ekf.local_position_is_valid()) ? position[0] : 0.0f;
1225  lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f;
1226  lpos.z = position[2];
1227 
1228  // Vehicle odometry position
1229  odom.x = lpos.x;
1230  odom.y = lpos.y;
1231  odom.z = lpos.z;
1232 
1233  // Velocity of body origin in local NED frame (m/s)
1234  float velocity[3];
1235  _ekf.get_velocity(velocity);
1236  lpos.vx = velocity[0];
1237  lpos.vy = velocity[1];
1238  lpos.vz = velocity[2];
1239 
1240  // Vehicle odometry linear velocity
1241  odom.vx = lpos.vx;
1242  odom.vy = lpos.vy;
1243  odom.vz = lpos.vz;
1244 
1245  // vertical position time derivative (m/s)
1246  _ekf.get_pos_d_deriv(&lpos.z_deriv);
1247 
1248  // Acceleration of body origin in local NED frame
1249  float vel_deriv[3];
1250  _ekf.get_vel_deriv_ned(vel_deriv);
1251  lpos.ax = vel_deriv[0];
1252  lpos.ay = vel_deriv[1];
1253  lpos.az = vel_deriv[2];
1254 
1255  // TODO: better status reporting
1256  lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
1257  lpos.z_valid = !_preflt_checker.hasVertFailed();
1258  lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
1259  lpos.v_z_valid = !_preflt_checker.hasVertFailed();
1260 
1261  // Position of local NED origin in GPS / WGS84 frame
1262  map_projection_reference_s ekf_origin;
1263  uint64_t origin_time;
1264 
1265  // true if position (x,y,z) has a valid WGS-84 global reference (ref_lat, ref_lon, alt)
1266  const bool ekf_origin_valid = _ekf.get_ekf_origin(&origin_time, &ekf_origin, &lpos.ref_alt);
1267  lpos.xy_global = ekf_origin_valid;
1268  lpos.z_global = ekf_origin_valid;
1269 
1270  if (ekf_origin_valid && (origin_time > lpos.ref_timestamp)) {
1271  lpos.ref_timestamp = origin_time;
1272  lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
1273  lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
1274  }
1275 
1276  // The rotation of the tangent plane vs. geographical north
1277  matrix::Quatf q = _ekf.get_quaternion();
1278 
1279  lpos.yaw = matrix::Eulerf(q).psi();
1280 
1281  // Vehicle odometry quaternion
1282  q.copyTo(odom.q);
1283 
1284  // Vehicle odometry angular rates
1285  float gyro_bias[3];
1286  _ekf.get_gyro_bias(gyro_bias);
1287  odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
1288  odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
1289  odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
1290 
1291  lpos.dist_bottom_valid = _ekf.get_terrain_valid();
1292 
1293  float terrain_vpos;
1294  _ekf.get_terrain_vert_pos(&terrain_vpos);
1295  lpos.dist_bottom = terrain_vpos - lpos.z; // Distance to bottom surface (ground) in meters
1296 
1297  // constrain the distance to ground to _rng_gnd_clearance
1298  if (lpos.dist_bottom < _param_ekf2_min_rng.get()) {
1299  lpos.dist_bottom = _param_ekf2_min_rng.get();
1300  }
1301 
1302  if (!_had_valid_terrain) {
1303  _had_valid_terrain = lpos.dist_bottom_valid;
1304  }
1305 
1306  // only consider ground effect if compensation is configured and the vehicle is armed (props spinning)
1307  if (_param_ekf2_gnd_eff_dz.get() > 0.0f && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
1308  // set ground effect flag if vehicle is closer than a specified distance to the ground
1309  if (lpos.dist_bottom_valid) {
1310  _ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get());
1311 
1312  // if we have no valid terrain estimate and never had one then use ground effect flag from land detector
1313  // _had_valid_terrain is used to make sure that we don't fall back to using this option
1314  // if we temporarily lose terrain data due to the distance sensor getting out of range
1315 
1316  } else if (vehicle_land_detected_updated && !_had_valid_terrain) {
1317  // update ground effect flag based on land detector state
1318  _ekf.set_gnd_effect_flag(_vehicle_land_detected.in_ground_effect);
1319  }
1320 
1321  } else {
1322  _ekf.set_gnd_effect_flag(false);
1323  }
1324 
1325  lpos.dist_bottom_rate = -lpos.vz; // Distance to bottom surface (ground) change rate
1326 
1327  _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
1328  _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
1329 
1330  // get state reset information of position and velocity
1331  _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
1332  _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
1333  _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
1334  _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
1335 
1336  // get control limit information
1337  _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
1338 
1339  // convert NaN to INFINITY
1340  if (!PX4_ISFINITE(lpos.vxy_max)) {
1341  lpos.vxy_max = INFINITY;
1342  }
1343 
1344  if (!PX4_ISFINITE(lpos.vz_max)) {
1345  lpos.vz_max = INFINITY;
1346  }
1347 
1348  if (!PX4_ISFINITE(lpos.hagl_min)) {
1349  lpos.hagl_min = INFINITY;
1350  }
1351 
1352  if (!PX4_ISFINITE(lpos.hagl_max)) {
1353  lpos.hagl_max = INFINITY;
1354  }
1355 
1356  // Get covariances to vehicle odometry
1357  float covariances[24];
1358  _ekf.covariances_diagonal().copyTo(covariances);
1359 
1360  // get the covariance matrix size
1361  const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
1362  const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
1363 
1364  // initially set pose covariances to 0
1365  for (size_t i = 0; i < POS_URT_SIZE; i++) {
1366  odom.pose_covariance[i] = 0.0;
1367  }
1368 
1369  // set the position variances
1370  odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
1371  odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
1372  odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
1373 
1374  // TODO: implement propagation from quaternion covariance to Euler angle covariance
1375  // by employing the covariance law
1376 
1377  // initially set velocity covariances to 0
1378  for (size_t i = 0; i < VEL_URT_SIZE; i++) {
1379  odom.velocity_covariance[i] = 0.0;
1380  }
1381 
1382  // set the linear velocity variances
1383  odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
1384  odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
1385  odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
1386 
1387  // publish vehicle local position data
1388  _vehicle_local_position_pub.update();
1389 
1390  // publish vehicle odometry data
1391  _vehicle_odometry_pub.publish(odom);
1392 
1393  // publish external visual odometry after fixed frame alignment if new odometry is received
1394  if (new_ev_data_received) {
1395  float q_ev2ekf[4];
1396  _ekf.get_ev2ekf_quaternion(q_ev2ekf); // rotates from EV to EKF navigation frame
1397  Quatf quat_ev2ekf(q_ev2ekf);
1398  Dcmf ev_rot_mat(quat_ev2ekf);
1399 
1400  vehicle_odometry_s aligned_ev_odom = _ev_odom;
1401 
1402  // Rotate external position and velocity into EKF navigation frame
1403  Vector3f aligned_pos = ev_rot_mat * Vector3f(_ev_odom.x, _ev_odom.y, _ev_odom.z);
1404  aligned_ev_odom.x = aligned_pos(0);
1405  aligned_ev_odom.y = aligned_pos(1);
1406  aligned_ev_odom.z = aligned_pos(2);
1407 
1408  Vector3f aligned_vel = ev_rot_mat * Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
1409  aligned_ev_odom.vx = aligned_vel(0);
1410  aligned_ev_odom.vy = aligned_vel(1);
1411  aligned_ev_odom.vz = aligned_vel(2);
1412 
1413  // Compute orientation in EKF navigation frame
1414  Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(_ev_odom.q) ;
1415  ev_quat_aligned.normalize();
1416 
1417  ev_quat_aligned.copyTo(aligned_ev_odom.q);
1418  quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
1419 
1420  _vehicle_visual_odometry_aligned_pub.publish(aligned_ev_odom);
1421  }
1422 
1423  if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
1424  // generate and publish global position data
1425  vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
1426 
1427  global_pos.timestamp = now;
1428 
1429  if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) {
1430  map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &global_pos.lat, &global_pos.lon);
1431  }
1432 
1433  global_pos.lat_lon_reset_counter = lpos.xy_reset_counter;
1434 
1435  global_pos.alt = -lpos.z + lpos.ref_alt; // Altitude AMSL in meters
1436  global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
1437 
1438  // global altitude has opposite sign of local down position
1439  global_pos.delta_alt = -lpos.delta_z;
1440 
1441  global_pos.vel_n = lpos.vx; // Ground north velocity, m/s
1442  global_pos.vel_e = lpos.vy; // Ground east velocity, m/s
1443  global_pos.vel_d = lpos.vz; // Ground downside velocity, m/s
1444 
1445  global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI.
1446 
1447  _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
1448 
1449  global_pos.terrain_alt_valid = lpos.dist_bottom_valid;
1450 
1451  if (global_pos.terrain_alt_valid) {
1452  global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
1453 
1454  } else {
1455  global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
1456  }
1457 
1458  global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
1459 
1460  _vehicle_global_position_pub.update();
1461  }
1462  }
1463 
1464  {
1465  // publish all corrected sensor readings and bias estimates after mag calibration is updated above
1466  sensor_bias_s bias{};
1467 
1468  bias.timestamp = now;
1469 
1470  // In-run bias estimates
1471  _ekf.get_gyro_bias(bias.gyro_bias);
1472  _ekf.get_accel_bias(bias.accel_bias);
1473 
1474  bias.mag_bias[0] = _last_valid_mag_cal[0];
1475  bias.mag_bias[1] = _last_valid_mag_cal[1];
1476  bias.mag_bias[2] = _last_valid_mag_cal[2];
1477 
1478  _sensor_bias_pub.publish(bias);
1479  }
1480 
1481  // publish estimator status
1482  estimator_status_s status;
1483  status.timestamp = now;
1484  _ekf.get_state_delayed(status.states);
1485  status.n_states = 24;
1486  _ekf.covariances_diagonal().copyTo(status.covariances);
1487  _ekf.get_gps_check_status(&status.gps_check_fail_flags);
1488  // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
1489  // the GPS Fix bit, which is always checked)
1490  status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
1491  status.control_mode_flags = control_status.value;
1492  _ekf.get_filter_fault_status(&status.filter_fault_flags);
1493  _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio,
1494  &status.vel_test_ratio, &status.pos_test_ratio,
1495  &status.hgt_test_ratio, &status.tas_test_ratio,
1496  &status.hagl_test_ratio, &status.beta_test_ratio);
1497 
1498  status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph;
1499  status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv;
1500  _ekf.get_ekf_soln_status(&status.solution_status_flags);
1501  _ekf.get_imu_vibe_metrics(status.vibe);
1502  status.time_slip = _last_time_slip_us / 1e6f;
1503  status.health_flags = 0.0f; // unused
1504  status.timeout_flags = 0.0f; // unused
1505  status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
1506  status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
1507  status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
1508  status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
1509  status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed;
1510 
1511  _estimator_status_pub.publish(status);
1512 
1513  // publish GPS drift data only when updated to minimise overhead
1514  float gps_drift[3];
1515  bool blocked;
1516 
1517  if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
1518  ekf_gps_drift_s drift_data;
1519  drift_data.timestamp = now;
1520  drift_data.hpos_drift_rate = gps_drift[0];
1521  drift_data.vpos_drift_rate = gps_drift[1];
1522  drift_data.hspd = gps_drift[2];
1523  drift_data.blocked = blocked;
1524 
1525  _ekf_gps_drift_pub.publish(drift_data);
1526  }
1527 
1528  {
1529  /* Check and save learned magnetometer bias estimates */
1530 
1531  // Check if conditions are OK for learning of magnetometer bias values
1532  if (!_vehicle_land_detected.landed && // not on ground
1533  (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && // vehicle is armed
1534  !status.filter_fault_flags && // there are no filter faults
1535  control_status.flags.mag_3D) { // the EKF is operating in the correct mode
1536 
1537  if (_last_magcal_us == 0) {
1538  _last_magcal_us = now;
1539 
1540  } else {
1541  _total_cal_time_us += now - _last_magcal_us;
1542  _last_magcal_us = now;
1543  }
1544 
1545  } else if (status.filter_fault_flags != 0) {
1546  // if a filter fault has occurred, assume previous learning was invalid and do not
1547  // count it towards total learning time.
1548  _total_cal_time_us = 0;
1549 
1550  for (bool &cal_available : _valid_cal_available) {
1551  cal_available = false;
1552  }
1553  }
1554 
1555  // Start checking mag bias estimates when we have accumulated sufficient calibration time
1556  if (_total_cal_time_us > 120_s) {
1557  // we have sufficient accumulated valid flight time to form a reliable bias estimate
1558  // check that the state variance for each axis is within a range indicating filter convergence
1559  const float max_var_allowed = 100.0f * _param_ekf2_magb_vref.get();
1560  const float min_var_allowed = 0.01f * _param_ekf2_magb_vref.get();
1561 
1562  // Declare all bias estimates invalid if any variances are out of range
1563  bool all_estimates_invalid = false;
1564 
1565  for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
1566  if (status.covariances[axis_index + 19] < min_var_allowed
1567  || status.covariances[axis_index + 19] > max_var_allowed) {
1568  all_estimates_invalid = true;
1569  }
1570  }
1571 
1572  // Store valid estimates and their associated variances
1573  if (!all_estimates_invalid) {
1574  for (uint8_t axis_index = 0; axis_index <= 2; axis_index++) {
1575  _last_valid_mag_cal[axis_index] = status.states[axis_index + 19];
1576  _valid_cal_available[axis_index] = true;
1577  _last_valid_variance[axis_index] = status.covariances[axis_index + 19];
1578  }
1579  }
1580  }
1581 
1582  // Check and save the last valid calibration when we are disarmed
1583  if ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
1584  && (status.filter_fault_flags == 0)
1585  && (_sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) {
1586 
1587  update_mag_bias(_param_ekf2_magbias_x, 0);
1588  update_mag_bias(_param_ekf2_magbias_y, 1);
1589  update_mag_bias(_param_ekf2_magbias_z, 2);
1590 
1591  // reset to prevent data being saved too frequently
1592  _total_cal_time_us = 0;
1593  }
1594 
1595  }
1596 
1597  publish_wind_estimate(now);
1598 
1599  if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
1600  _mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
1601  }
1602 
1603  {
1604  // publish estimator innovation data
1605  ekf2_innovations_s innovations;
1606  innovations.timestamp = now;
1607  _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
1608  _ekf.get_aux_vel_innov(&innovations.aux_vel_innov[0]);
1609  _ekf.get_mag_innov(&innovations.mag_innov[0]);
1610  _ekf.get_heading_innov(&innovations.heading_innov);
1611  _ekf.get_airspeed_innov(&innovations.airspeed_innov);
1612  _ekf.get_beta_innov(&innovations.beta_innov);
1613  _ekf.get_flow_innov(&innovations.flow_innov[0]);
1614  _ekf.get_hagl_innov(&innovations.hagl_innov);
1615  _ekf.get_drag_innov(&innovations.drag_innov[0]);
1616 
1617  _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
1618  _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
1619  _ekf.get_heading_innov_var(&innovations.heading_innov_var);
1620  _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
1621  _ekf.get_beta_innov_var(&innovations.beta_innov_var);
1622  _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
1623  _ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
1624  _ekf.get_drag_innov_var(&innovations.drag_innov_var[0]);
1625 
1626  _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
1627 
1628  // calculate noise filtered velocity innovations which are used for pre-flight checking
1629  if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
1630  float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f;
1631  runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations);
1632 
1633  } else {
1634  resetPreFlightChecks();
1635  }
1636 
1637  _estimator_innovations_pub.publish(innovations);
1638  }
1639  }
1640 
1641  // publish ekf2_timestamps
1642  _ekf2_timestamps_pub.publish(ekf2_timestamps);
1643  }
1644 }
1645 
1646 void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data)
1647 {
1648  msg.time_usec = data.timestamp;
1649  msg.lat = data.lat;
1650  msg.lon = data.lon;
1651  msg.alt = data.alt;
1652  msg.yaw = data.heading;
1653  msg.yaw_offset = data.heading_offset;
1654  msg.fix_type = data.fix_type;
1655  msg.eph = data.eph;
1656  msg.epv = data.epv;
1657  msg.sacc = data.s_variance_m_s;
1658  msg.vel_m_s = data.vel_m_s;
1659  msg.vel_ned[0] = data.vel_n_m_s;
1660  msg.vel_ned[1] = data.vel_e_m_s;
1661  msg.vel_ned[2] = data.vel_d_m_s;
1662  msg.vel_ned_valid = data.vel_ned_valid;
1663  msg.nsats = data.satellites_used;
1664  msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop);
1665 }
1666 
1667 void Ekf2::runPreFlightChecks(const float dt,
1668  const filter_control_status_u &control_status,
1669  const vehicle_status_s &vehicle_status,
1670  const ekf2_innovations_s &innov)
1671 {
1672  const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
1673 
1674  _preflt_checker.setVehicleCanObserveHeadingInFlight(can_observe_heading_in_flight);
1675  _preflt_checker.setUsingGpsAiding(control_status.flags.gps);
1676  _preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow);
1677  _preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos);
1678 
1679  _preflt_checker.update(dt, innov);
1680 }
1681 
1682 void Ekf2::resetPreFlightChecks()
1683 {
1684  _preflt_checker.reset();
1685 }
1686 
1687 int Ekf2::getRangeSubIndex()
1688 {
1689  for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
1690  distance_sensor_s report{};
1691 
1692  if (_range_finder_subs[i].update(&report)) {
1693  // only use the first instace which has the correct orientation
1694  if (report.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
1695  PX4_INFO("Found range finder with instance %d", i);
1696  return i;
1697  }
1698  }
1699  }
1700 
1701  return -1;
1702 }
1703 
1704 bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now)
1705 {
1706  if (_ekf.attitude_valid()) {
1707  // generate vehicle attitude quaternion data
1708  vehicle_attitude_s att;
1709  att.timestamp = now;
1710 
1711  const Quatf q{_ekf.calculate_quaternion()};
1712  q.copyTo(att.q);
1713 
1714  _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
1715 
1716  _att_pub.publish(att);
1717 
1718  return true;
1719 
1720  } else if (_replay_mode) {
1721  // in replay mode we have to tell the replay module not to wait for an update
1722  // we do this by publishing an attitude with zero timestamp
1723  vehicle_attitude_s att{};
1724  _att_pub.publish(att);
1725  }
1726 
1727  return false;
1728 }
1729 
1730 bool Ekf2::publish_wind_estimate(const hrt_abstime &timestamp)
1731 {
1732  if (_ekf.get_wind_status()) {
1733  // Publish wind estimate only if ekf declares them valid
1734  wind_estimate_s wind_estimate{};
1735  float velNE_wind[2];
1736  float wind_var[2];
1737  _ekf.get_wind_velocity(velNE_wind);
1738  _ekf.get_wind_velocity_var(wind_var);
1739  _ekf.get_airspeed_innov(&wind_estimate.tas_innov);
1740  _ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var);
1741  _ekf.get_beta_innov(&wind_estimate.beta_innov);
1742  _ekf.get_beta_innov_var(&wind_estimate.beta_innov_var);
1743  wind_estimate.timestamp = timestamp;
1744  wind_estimate.windspeed_north = velNE_wind[0];
1745  wind_estimate.windspeed_east = velNE_wind[1];
1746  wind_estimate.variance_north = wind_var[0];
1747  wind_estimate.variance_east = wind_var[1];
1748  wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
1749 
1750  _wind_pub.publish(wind_estimate);
1751 
1752  return true;
1753  }
1754 
1755  return false;
1756 }
1757 
1758 const Vector3f Ekf2::get_vel_body_wind()
1759 {
1760  // Used to correct baro data for positional errors
1761 
1762  matrix::Quatf q = _ekf.get_quaternion();
1763  matrix::Dcmf R_to_body(q.inversed());
1764 
1765  // Calculate wind-compensated velocity in body frame
1766  // Velocity of body origin in local NED frame (m/s)
1767  float velocity[3];
1768  _ekf.get_velocity(velocity);
1769 
1770  float velNE_wind[2];
1771  _ekf.get_wind_velocity(velNE_wind);
1772 
1773  Vector3f v_wind_comp = {velocity[0] - velNE_wind[0], velocity[1] - velNE_wind[1], velocity[2]};
1774 
1775  return R_to_body * v_wind_comp;
1776 }
1777 
1778 bool Ekf2::blend_gps_data()
1779 {
1780  // zero the blend weights
1781  memset(&_blend_weights, 0, sizeof(_blend_weights));
1782 
1783  /*
1784  * If both receivers have the same update rate, use the oldest non-zero time.
1785  * If two receivers with different update rates are used, use the slowest.
1786  * If time difference is excessive, use newest to prevent a disconnected receiver
1787  * from blocking updates.
1788  */
1789 
1790  // Calculate the time step for each receiver with some filtering to reduce the effects of jitter
1791  // Find the largest and smallest time step.
1792  float dt_max = 0.0f;
1793  float dt_min = 0.3f;
1794 
1795  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1796  float raw_dt = 0.001f * (float)(_gps_state[i].time_usec - _time_prev_us[i]);
1797 
1798  if (raw_dt > 0.0f && raw_dt < 0.3f) {
1799  _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
1800  }
1801 
1802  if (_gps_dt[i] > dt_max) {
1803  dt_max = _gps_dt[i];
1804  _gps_slowest_index = i;
1805  }
1806 
1807  if (_gps_dt[i] < dt_min) {
1808  dt_min = _gps_dt[i];
1809  }
1810  }
1811 
1812  // Find the receiver that is last be updated
1813  uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message
1814  uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message
1815 
1816  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1817  // Find largest and smallest times
1818  if (_gps_state[i].time_usec > max_us) {
1819  max_us = _gps_state[i].time_usec;
1820  _gps_newest_index = i;
1821  }
1822 
1823  if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) {
1824  min_us = _gps_state[i].time_usec;
1825  _gps_oldest_index = i;
1826  }
1827  }
1828 
1829  if ((max_us - min_us) > 300000) {
1830  // A receiver has timed out so fall out of blending
1831  return false;
1832  }
1833 
1834  /*
1835  * If the largest dt is less than 20% greater than the smallest, then we have receivers
1836  * running at the same rate then we wait until we have two messages with an arrival time
1837  * difference that is less than 50% of the smallest time step and use the time stamp from
1838  * the newest data.
1839  * Else we have two receivers at different update rates and use the slowest receiver
1840  * as the timing reference.
1841  */
1842 
1843  if ((dt_max - dt_min) < 0.2f * dt_min) {
1844  // both receivers assumed to be running at the same rate
1845  if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
1846  // data arrival within a short time window enables the two measurements to be blended
1847  _gps_time_ref_index = _gps_newest_index;
1848  _gps_new_output_data = true;
1849  }
1850 
1851  } else {
1852  // both receivers running at different rates
1853  _gps_time_ref_index = _gps_slowest_index;
1854 
1855  if (_gps_state[_gps_time_ref_index].time_usec > _time_prev_us[_gps_time_ref_index]) {
1856  // blend data at the rate of the slower receiver
1857  _gps_new_output_data = true;
1858  }
1859  }
1860 
1861  if (_gps_new_output_data) {
1862  _gps_blended_state.time_usec = _gps_state[_gps_time_ref_index].time_usec;
1863 
1864  // calculate the sum squared speed accuracy across all GPS sensors
1865  float speed_accuracy_sum_sq = 0.0f;
1866 
1867  if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) {
1868  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1869  if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc > 0.0f) {
1870  speed_accuracy_sum_sq += _gps_state[i].sacc * _gps_state[i].sacc;
1871 
1872  } else {
1873  // not all receivers support this metric so set it to zero and don't use it
1874  speed_accuracy_sum_sq = 0.0f;
1875  break;
1876  }
1877  }
1878  }
1879 
1880  // calculate the sum squared horizontal position accuracy across all GPS sensors
1881  float horizontal_accuracy_sum_sq = 0.0f;
1882 
1883  if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
1884  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1885  if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) {
1886  horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph;
1887 
1888  } else {
1889  // not all receivers support this metric so set it to zero and don't use it
1890  horizontal_accuracy_sum_sq = 0.0f;
1891  break;
1892  }
1893  }
1894  }
1895 
1896  // calculate the sum squared vertical position accuracy across all GPS sensors
1897  float vertical_accuracy_sum_sq = 0.0f;
1898 
1899  if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
1900  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1901  if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) {
1902  vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv;
1903 
1904  } else {
1905  // not all receivers support this metric so set it to zero and don't use it
1906  vertical_accuracy_sum_sq = 0.0f;
1907  break;
1908  }
1909  }
1910  }
1911 
1912  // Check if we can do blending using reported accuracy
1913  bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f
1914  || speed_accuracy_sum_sq > 0.0f);
1915 
1916  // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
1917  if (!can_do_blending) {
1918  return false;
1919  }
1920 
1921  float sum_of_all_weights = 0.0f;
1922 
1923  // calculate a weighting using the reported speed accuracy
1924  float spd_blend_weights[GPS_MAX_RECEIVERS] = {};
1925 
1926  if (speed_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) {
1927  // calculate the weights using the inverse of the variances
1928  float sum_of_spd_weights = 0.0f;
1929 
1930  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1931  if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc >= 0.001f) {
1932  spd_blend_weights[i] = 1.0f / (_gps_state[i].sacc * _gps_state[i].sacc);
1933  sum_of_spd_weights += spd_blend_weights[i];
1934  }
1935  }
1936 
1937  // normalise the weights
1938  if (sum_of_spd_weights > 0.0f) {
1939  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1940  spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights;
1941  }
1942 
1943  sum_of_all_weights += 1.0f;
1944  }
1945  }
1946 
1947  // calculate a weighting using the reported horizontal position
1948  float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
1949 
1950  if (horizontal_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) {
1951  // calculate the weights using the inverse of the variances
1952  float sum_of_hpos_weights = 0.0f;
1953 
1954  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1955  if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) {
1956  hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph);
1957  sum_of_hpos_weights += hpos_blend_weights[i];
1958  }
1959  }
1960 
1961  // normalise the weights
1962  if (sum_of_hpos_weights > 0.0f) {
1963  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1964  hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights;
1965  }
1966 
1967  sum_of_all_weights += 1.0f;
1968  }
1969  }
1970 
1971  // calculate a weighting using the reported vertical position accuracy
1972  float vpos_blend_weights[GPS_MAX_RECEIVERS] = {};
1973 
1974  if (vertical_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC)) {
1975  // calculate the weights using the inverse of the variances
1976  float sum_of_vpos_weights = 0.0f;
1977 
1978  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1979  if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) {
1980  vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv);
1981  sum_of_vpos_weights += vpos_blend_weights[i];
1982  }
1983  }
1984 
1985  // normalise the weights
1986  if (sum_of_vpos_weights > 0.0f) {
1987  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1988  vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights;
1989  }
1990 
1991  sum_of_all_weights += 1.0f;
1992  };
1993  }
1994 
1995  // calculate an overall weight
1996  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
1997  _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
1998  }
1999 
2000  // With updated weights we can calculate a blended GPS solution and
2001  // offsets for each physical receiver
2002  update_gps_blend_states();
2003  update_gps_offsets();
2004  _gps_select_index = 2;
2005 
2006  }
2007 
2008  return true;
2009 }
2010 
2011 /*
2012  * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical receiver solutions
2013  * with weights are calculated in calc_gps_blend_weights(). This internal state cannot be used directly by estimators
2014  * because if physical receivers have significant position differences, variation in receiver estimated accuracy will
2015  * cause undesirable variation in the position solution.
2016 */
2017 void Ekf2::update_gps_blend_states()
2018 {
2019  // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
2020  _gps_blended_state.time_usec = 0;
2021  _gps_blended_state.lat = 0;
2022  _gps_blended_state.lon = 0;
2023  _gps_blended_state.alt = 0;
2024  _gps_blended_state.fix_type = 0;
2025  _gps_blended_state.eph = FLT_MAX;
2026  _gps_blended_state.epv = FLT_MAX;
2027  _gps_blended_state.sacc = FLT_MAX;
2028  _gps_blended_state.vel_m_s = 0.0f;
2029  _gps_blended_state.vel_ned[0] = 0.0f;
2030  _gps_blended_state.vel_ned[1] = 0.0f;
2031  _gps_blended_state.vel_ned[2] = 0.0f;
2032  _gps_blended_state.vel_ned_valid = true;
2033  _gps_blended_state.nsats = 0;
2034  _gps_blended_state.pdop = FLT_MAX;
2035 
2036  _blended_antenna_offset.zero();
2037 
2038  // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
2039  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2040  // blend the timing data
2041  _gps_blended_state.time_usec += (uint64_t)((double)_gps_state[i].time_usec * (double)_blend_weights[i]);
2042 
2043  // use the highest status
2044  if (_gps_state[i].fix_type > _gps_blended_state.fix_type) {
2045  _gps_blended_state.fix_type = _gps_state[i].fix_type;
2046  }
2047 
2048  // calculate a blended average speed and velocity vector
2049  _gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * _blend_weights[i];
2050  _gps_blended_state.vel_ned[0] += _gps_state[i].vel_ned[0] * _blend_weights[i];
2051  _gps_blended_state.vel_ned[1] += _gps_state[i].vel_ned[1] * _blend_weights[i];
2052  _gps_blended_state.vel_ned[2] += _gps_state[i].vel_ned[2] * _blend_weights[i];
2053 
2054  // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers
2055  // If any receiver contributing has an invalid velocity, then report blended velocity as invalid
2056  if (_blend_weights[i] > 0.0f) {
2057 
2058  if (_gps_state[i].eph > 0.0f
2059  && _gps_state[i].eph < _gps_blended_state.eph) {
2060  _gps_blended_state.eph = _gps_state[i].eph;
2061  }
2062 
2063  if (_gps_state[i].epv > 0.0f
2064  && _gps_state[i].epv < _gps_blended_state.epv) {
2065  _gps_blended_state.epv = _gps_state[i].epv;
2066  }
2067 
2068  if (_gps_state[i].sacc > 0.0f
2069  && _gps_state[i].sacc < _gps_blended_state.sacc) {
2070  _gps_blended_state.sacc = _gps_state[i].sacc;
2071  }
2072 
2073  if (_gps_state[i].pdop > 0
2074  && _gps_state[i].pdop < _gps_blended_state.pdop) {
2075  _gps_blended_state.pdop = _gps_state[i].pdop;
2076  }
2077 
2078  if (_gps_state[i].nsats > 0
2079  && _gps_state[i].nsats > _gps_blended_state.nsats) {
2080  _gps_blended_state.nsats = _gps_state[i].nsats;
2081  }
2082 
2083  if (!_gps_state[i].vel_ned_valid) {
2084  _gps_blended_state.vel_ned_valid = false;
2085  }
2086 
2087  }
2088 
2089  // TODO read parameters for individual GPS antenna positions and blend
2090  // Vector3f temp_antenna_offset = _antenna_offset[i];
2091  // temp_antenna_offset *= _blend_weights[i];
2092  // _blended_antenna_offset += temp_antenna_offset;
2093 
2094  }
2095 
2096  /*
2097  * Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state.
2098  * This is statistically the most likely location, but may not be stable enough for direct use by the EKF.
2099  */
2100 
2101  // Use the GPS with the highest weighting as the reference position
2102  float best_weight = 0.0f;
2103 
2104  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2105  if (_blend_weights[i] > best_weight) {
2106  best_weight = _blend_weights[i];
2107  _gps_best_index = i;
2108  _gps_blended_state.lat = _gps_state[i].lat;
2109  _gps_blended_state.lon = _gps_state[i].lon;
2110  _gps_blended_state.alt = _gps_state[i].alt;
2111  }
2112  }
2113 
2114  // Convert each GPS position to a local NEU offset relative to the reference position
2115  Vector2f blended_NE_offset_m;
2116  blended_NE_offset_m.zero();
2117  float blended_alt_offset_mm = 0.0f;
2118 
2119  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2120  if ((_blend_weights[i] > 0.0f) && (i != _gps_best_index)) {
2121  // calculate the horizontal offset
2122  Vector2f horiz_offset{};
2123  get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
2124  (_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
2125  &horiz_offset(0), &horiz_offset(1));
2126 
2127  // sum weighted offsets
2128  blended_NE_offset_m += horiz_offset * _blend_weights[i];
2129 
2130  // calculate vertical offset
2131  float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt);
2132 
2133  // sum weighted offsets
2134  blended_alt_offset_mm += vert_offset * _blend_weights[i];
2135  }
2136  }
2137 
2138  // Add the sum of weighted offsets to the reference position to obtain the blended position
2139  double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7;
2140  double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7;
2141  double lat_deg_res, lon_deg_res;
2142  add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res,
2143  &lon_deg_res);
2144  _gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res);
2145  _gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
2146  _gps_blended_state.alt += (int32_t)blended_alt_offset_mm;
2147 
2148  // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
2149  uint8_t gps_best_yaw_index = 0;
2150  best_weight = 0.0f;
2151 
2152  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2153  if (PX4_ISFINITE(_gps_state[i].yaw) && (_blend_weights[i] > best_weight)) {
2154  best_weight = _blend_weights[i];
2155  gps_best_yaw_index = i;
2156  }
2157  }
2158 
2159  _gps_blended_state.yaw = _gps_state[gps_best_yaw_index].yaw;
2160  _gps_blended_state.yaw_offset = _gps_state[gps_best_yaw_index].yaw_offset;
2161 }
2162 
2163 /*
2164  * The location in _gps_blended_state will move around as the relative accuracy changes.
2165  * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
2166  * calculated.
2167 */
2168 void Ekf2::update_gps_offsets()
2169 {
2170 
2171  // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
2172  // Increase the filter time constant proportional to the inverse of the weighting
2173  // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
2174  float alpha[GPS_MAX_RECEIVERS] = {};
2175  float omega_lpf = 1.0f / fmaxf(_param_ekf2_gps_tau.get(), 1.0f);
2176 
2177  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2178  if (_gps_state[i].time_usec - _time_prev_us[i] > 0) {
2179  // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
2180  float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]),
2181  0.0f, 1.0f);
2182 
2183  // scale the filter coefficient so that time constant is inversely proprtional to weighting
2184  if (_blend_weights[i] > min_alpha) {
2185  alpha[i] = min_alpha / _blend_weights[i];
2186 
2187  } else {
2188  alpha[i] = 1.0f;
2189  }
2190 
2191  _time_prev_us[i] = _gps_state[i].time_usec;
2192  }
2193  }
2194 
2195  // Calculate a filtered position delta for each GPS relative to the blended solution state
2196  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2197  Vector2f offset;
2198  get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
2199  (_gps_blended_state.lat / 1.0e7), (_gps_blended_state.lon / 1.0e7), &offset(0), &offset(1));
2200  _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
2201  _hgt_offset_mm[i] = (float)(_gps_blended_state.alt - _gps_state[i].alt) * alpha[i] +
2202  _hgt_offset_mm[i] * (1.0f - alpha[i]);
2203  }
2204 
2205  // calculate offset limits from the largest difference between receivers
2206  Vector2f max_ne_offset{};
2207  float max_alt_offset = 0;
2208 
2209  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2210  for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) {
2211  if (i != j) {
2212  Vector2f offset;
2213  get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
2214  (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), &offset(0), &offset(1));
2215  max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0)));
2216  max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1)));
2217  max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt)));
2218  }
2219  }
2220  }
2221 
2222  // apply offset limits
2223  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2224  _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0));
2225  _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1));
2226  _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset);
2227  }
2228 
2229 }
2230 
2231 
2232 /*
2233  * Apply the steady state physical receiver offsets calculated by update_gps_offsets().
2234 */
2235 void Ekf2::apply_gps_offsets()
2236 {
2237  // calculate offset corrected output for each physical GPS.
2238  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2239  // Add the sum of weighted offsets to the reference position to obtain the blended position
2240  double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7;
2241  double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7;
2242  double lat_deg_res, lon_deg_res;
2243  add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res,
2244  &lon_deg_res);
2245  _gps_output[i].lat = (int32_t)(1.0E7 * lat_deg_res);
2246  _gps_output[i].lon = (int32_t)(1.0E7 * lon_deg_res);
2247  _gps_output[i].alt = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i];
2248 
2249  // other receiver data is used uncorrected
2250  _gps_output[i].time_usec = _gps_state[i].time_usec;
2251  _gps_output[i].fix_type = _gps_state[i].fix_type;
2252  _gps_output[i].vel_m_s = _gps_state[i].vel_m_s;
2253  _gps_output[i].vel_ned[0] = _gps_state[i].vel_ned[0];
2254  _gps_output[i].vel_ned[1] = _gps_state[i].vel_ned[1];
2255  _gps_output[i].vel_ned[2] = _gps_state[i].vel_ned[2];
2256  _gps_output[i].eph = _gps_state[i].eph;
2257  _gps_output[i].epv = _gps_state[i].epv;
2258  _gps_output[i].sacc = _gps_state[i].sacc;
2259  _gps_output[i].pdop = _gps_state[i].pdop;
2260  _gps_output[i].nsats = _gps_state[i].nsats;
2261  _gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid;
2262  _gps_output[i].yaw = _gps_state[i].yaw;
2263  _gps_output[i].yaw_offset = _gps_state[i].yaw_offset;
2264 
2265  }
2266 }
2267 
2268 /*
2269  Calculate GPS output that is a blend of the offset corrected physical receiver data
2270 */
2271 void Ekf2::calc_gps_blend_output()
2272 {
2273  // Convert each GPS position to a local NEU offset relative to the reference position
2274  // which is defined as the positon of the blended solution calculated from non offset corrected data
2275  Vector2f blended_NE_offset_m;
2276  blended_NE_offset_m.zero();
2277  float blended_alt_offset_mm = 0.0f;
2278 
2279  for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
2280  if (_blend_weights[i] > 0.0f) {
2281  // calculate the horizontal offset
2282  Vector2f horiz_offset{};
2283  get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
2284  (_gps_blended_state.lon / 1.0e7),
2285  (_gps_output[i].lat / 1.0e7),
2286  (_gps_output[i].lon / 1.0e7),
2287  &horiz_offset(0),
2288  &horiz_offset(1));
2289 
2290  // sum weighted offsets
2291  blended_NE_offset_m += horiz_offset * _blend_weights[i];
2292 
2293  // calculate vertical offset
2294  float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt);
2295 
2296  // sum weighted offsets
2297  blended_alt_offset_mm += vert_offset * _blend_weights[i];
2298  }
2299  }
2300 
2301  // Add the sum of weighted offsets to the reference position to obtain the blended position
2302  double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7;
2303  double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7;
2304  double lat_deg_res, lon_deg_res;
2305  add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res,
2306  &lon_deg_res);
2307  _gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res);
2308  _gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res);
2309  _gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
2310 
2311  // Copy remaining data from internal states to output
2312  _gps_output[GPS_BLENDED_INSTANCE].time_usec = _gps_blended_state.time_usec;
2313  _gps_output[GPS_BLENDED_INSTANCE].fix_type = _gps_blended_state.fix_type;
2314  _gps_output[GPS_BLENDED_INSTANCE].vel_m_s = _gps_blended_state.vel_m_s;
2315  _gps_output[GPS_BLENDED_INSTANCE].vel_ned[0] = _gps_blended_state.vel_ned[0];
2316  _gps_output[GPS_BLENDED_INSTANCE].vel_ned[1] = _gps_blended_state.vel_ned[1];
2317  _gps_output[GPS_BLENDED_INSTANCE].vel_ned[2] = _gps_blended_state.vel_ned[2];
2318  _gps_output[GPS_BLENDED_INSTANCE].eph = _gps_blended_state.eph;
2319  _gps_output[GPS_BLENDED_INSTANCE].epv = _gps_blended_state.epv;
2320  _gps_output[GPS_BLENDED_INSTANCE].sacc = _gps_blended_state.sacc;
2321  _gps_output[GPS_BLENDED_INSTANCE].pdop = _gps_blended_state.pdop;
2322  _gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats;
2323  _gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid;
2324  _gps_output[GPS_BLENDED_INSTANCE].yaw = _gps_blended_state.yaw;
2325  _gps_output[GPS_BLENDED_INSTANCE].yaw_offset = _gps_blended_state.yaw_offset;
2326 
2327 }
2328 
2329 float Ekf2::filter_altitude_ellipsoid(float amsl_hgt)
2330 {
2331 
2332  float height_diff = static_cast<float>(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt;
2333 
2334  if (_gps_alttitude_ellipsoid_previous_timestamp[0] == 0) {
2335 
2336  _wgs84_hgt_offset = height_diff;
2337  _gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec;
2338 
2339  } else if (_gps_state[0].time_usec != _gps_alttitude_ellipsoid_previous_timestamp[0]) {
2340 
2341  // apply a 10 second first order low pass filter to baro offset
2342  float dt = 1e-6f * static_cast<float>(_gps_state[0].time_usec - _gps_alttitude_ellipsoid_previous_timestamp[0]);
2343  _gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec;
2344  float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
2345  _wgs84_hgt_offset += dt * math::constrain(offset_rate_correction, -0.1f, 0.1f);
2346  }
2347 
2348  return amsl_hgt + _wgs84_hgt_offset;
2349 }
2350 
2351 int Ekf2::custom_command(int argc, char *argv[])
2352 {
2353  return print_usage("unknown command");
2354 }
2355 
2356 int Ekf2::task_spawn(int argc, char *argv[])
2357 {
2358  bool replay_mode = false;
2359 
2360  if (argc > 1 && !strcmp(argv[1], "-r")) {
2361  PX4_INFO("replay mode enabled");
2362  replay_mode = true;
2363  }
2364 
2365  Ekf2 *instance = new Ekf2(replay_mode);
2366 
2367  if (instance) {
2368  _object.store(instance);
2369  _task_id = task_id_is_work_queue;
2370 
2371  if (instance->init()) {
2372  return PX4_OK;
2373  }
2374 
2375  } else {
2376  PX4_ERR("alloc failed");
2377  }
2378 
2379  delete instance;
2380  _object.store(nullptr);
2381  _task_id = -1;
2382 
2383  return PX4_ERROR;
2384 }
2385 
2386 int Ekf2::print_usage(const char *reason)
2387 {
2388  if (reason) {
2389  PX4_WARN("%s\n", reason);
2390  }
2391 
2392  PRINT_MODULE_DESCRIPTION(
2393  R"DESCR_STR(
2394 ### Description
2395 Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing.
2396 
2397 The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/en/advanced_config/tuning_the_ecl_ekf.html) page.
2398 
2399 ekf2 can be started in replay mode (`-r`): in this mode it does not access the system time, but only uses the
2400 timestamps from the sensor topics.
2401 
2402 )DESCR_STR");
2403 
2404  PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
2405  PRINT_MODULE_USAGE_COMMAND("start");
2406  PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
2407  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
2408 
2409  return 0;
2410 }
2411 
2412 extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
2413 {
2414  return Ekf2::main(argc, argv);
2415 }
vehicle_local_position_s::z_global
bool z_global
Definition: vehicle_local_position.h:92
distance_sensor_s::min_distance
float min_distance
Definition: distance_sensor.h:73
ekf2_innovations_s::beta_innov
float beta_innov
Definition: ekf2_innovations.h:58
VEHICLE_TYPE_FIXED_WING
#define VEHICLE_TYPE_FIXED_WING
Definition: commander_helper.cpp:70
Ekf2::getRangeSubIndex
int getRangeSubIndex()
get subscription index of first downward-facing range sensor
Definition: ekf2_main.cpp:1687
EstimatorInterface::setIMUData
void setIMUData(const imuSample &imu_sample)
Definition: estimator_interface.cpp:49
math::constrain
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
Ekf2::_ekf_gps_drift_pub
uORB::Publication< ekf_gps_drift_s > _ekf_gps_drift_pub
Definition: ekf2_main.cpp:272
estimator_status_s::covariances
float covariances[24]
Definition: estimator_status.h:90
EstimatorInterface::setMagData
void setMagData(uint64_t time_usec, float(&data)[3])
Definition: estimator_interface.cpp:161
landing_target_pose_s::cov_vy_rel
float cov_vy_rel
Definition: landing_target_pose.h:62
estimator::filter_control_status_u::flags
struct estimator::filter_control_status_u::@60 flags
vehicle_magnetometer_s::timestamp
uint64_t timestamp
Definition: vehicle_magnetometer.h:53
PreFlightChecker::hasHeightFailed
bool hasHeightFailed() const
Definition: PreFlightChecker.hpp:83
time_literals
optical_flow.h
estimator::gps_message::eph
float eph
GPS horizontal position accuracy in m.
Definition: common.h:65
Ekf2::_parameter_update_sub
uORB::Subscription _parameter_update_sub
Definition: ekf2_main.cpp:252
vehicle_global_position_s
Definition: vehicle_global_position.h:51
ekf_gps_position_s::lat
int32_t lat
Definition: ekf_gps_position.h:54
estimator::imuSample::delta_ang
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
Definition: common.h:107
vehicle_local_position_s::y
float y
Definition: vehicle_local_position.h:58
_airdata_sub
static int _airdata_sub
Definition: messages.cpp:63
vehicle_gps_position_s::fix_type
uint8_t fix_type
Definition: vehicle_gps_position.h:75
PreFlightChecker::setUsingFlowAiding
void setUsingFlowAiding(bool val)
Definition: PreFlightChecker.hpp:77
estimator::gps_message::yaw_offset
float yaw_offset
Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET.
Definition: common.h:63
landing_target_pose_s::rel_vel_valid
bool rel_vel_valid
Definition: landing_target_pose.h:68
sensor_selection_s::timestamp
uint64_t timestamp
Definition: sensor_selection.h:53
Ekf2::_last_magcal_us
hrt_abstime _last_magcal_us
last time the EKF was operating a mode that estimates magnetomer biases (uSec)
Definition: ekf2_main.cpp:200
vehicle_air_data_s::timestamp
uint64_t timestamp
Definition: vehicle_air_data.h:53
estimator_status_s::control_mode_flags
uint32_t control_mode_flags
Definition: estimator_status.h:91
Ekf2::_sensor_bias_pub
uORB::Publication< sensor_bias_s > _sensor_bias_pub
Definition: ekf2_main.cpp:275
vehicle_local_position_s::az
float az
Definition: vehicle_local_position.h:70
vehicle_local_position_s::eph
float eph
Definition: vehicle_local_position.h:75
Subscription.hpp
BLEND_MASK_USE_HPOS_ACC
#define BLEND_MASK_USE_HPOS_ACC
Definition: ekf2_main.cpp:85
estimator::gps_message::epv
float epv
GPS vertical position accuracy in m.
Definition: common.h:66
Ekf2::_airdata_sub
uORB::Subscription _airdata_sub
Definition: ekf2_main.cpp:246
EstimatorInterface::get_hagl_innov
void get_hagl_innov(float *hagl_innov)
Definition: estimator_interface.h:136
status
static struct vehicle_status_s status
Definition: Commander.cpp:138
Ekf::get_ekf_vel_accuracy
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override
Definition: ekf_helper.cpp:1041
EstimatorInterface::setExtVisionData
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
Definition: estimator_interface.cpp:419
vehicle_status_s::vehicle_type
uint8_t vehicle_type
Definition: vehicle_status.h:108
ekf_gps_position_s
Definition: ekf_gps_position.h:51
sensor_bias.h
optical_flow_s::gyro_x_rate_integral
float gyro_x_rate_integral
Definition: optical_flow.h:56
vehicle_gps_position_s::lon
int32_t lon
Definition: vehicle_gps_position.h:56
ekf2_innovations_s::hagl_innov
float hagl_innov
Definition: ekf2_innovations.h:60
vehicle_gps_position_s::alt
int32_t alt
Definition: vehicle_gps_position.h:57
estimator::gps_message::nsats
uint8_t nsats
number of satellites used
Definition: common.h:71
PC_ELAPSED
measure the time elapsed performing an event
Definition: perf_counter.h:56
Ekf2::_wgs84_hgt_offset
float _wgs84_hgt_offset
height offset between AMSL and WGS84
Definition: ekf2_main.cpp:238
EstimatorInterface::setAuxVelData
void setAuxVelData(uint64_t time_usec, float(&data)[2], float(&variance)[2])
Definition: estimator_interface.cpp:459
EstimatorInterface::set_optical_flow_limits
void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
Definition: estimator_interface.h:262
vehicle_local_position_s::vxy_max
float vxy_max
Definition: vehicle_local_position.h:79
ekf2_innovations.h
estimator::gps_message::time_usec
uint64_t time_usec
Definition: common.h:58
estimator::gps_message::vel_m_s
float vel_m_s
GPS ground speed (m/sec)
Definition: common.h:68
Ekf::calculate_quaternion
Quatf calculate_quaternion() const
Definition: ekf.cpp:680
vehicle_local_position_s::delta_xy
float delta_xy[2]
Definition: vehicle_local_position.h:60
uORB::SubscriptionInterval::update
bool update(void *dst)
Copy the struct if updated.
Definition: SubscriptionInterval.hpp:95
ekf2_innovations_s::timestamp
uint64_t timestamp
Definition: ekf2_innovations.h:53
ekf2_innovations_s::airspeed_innov
float airspeed_innov
Definition: ekf2_innovations.h:57
ekf_gps_drift.h
WaypointType::position
Ekf2::_vehicle_global_position_pub
uORB::PublicationData< vehicle_global_position_s > _vehicle_global_position_pub
Definition: ekf2_main.cpp:279
distance_sensor_s
Definition: distance_sensor.h:70
Ekf2::_landing_target_pose_sub
uORB::Subscription _landing_target_pose_sub
Definition: ekf2_main.cpp:249
Ekf::get_ekf_origin
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override
Definition: ekf_helper.cpp:959
Ekf2::_vehicle_odometry_pub
uORB::Publication< vehicle_odometry_s > _vehicle_odometry_pub
Definition: ekf2_main.cpp:277
optical_flow_s::timestamp
uint64_t timestamp
Definition: optical_flow.h:53
vehicle_attitude_s
Definition: vehicle_attitude.h:51
Ekf2::_sensor_selection_sub
uORB::Subscription _sensor_selection_sub
Definition: ekf2_main.cpp:253
is_fixed_wing
bool is_fixed_wing(const struct vehicle_status_s *current_status)
Definition: commander_helper.cpp:119
Ekf::get_state_delayed
void get_state_delayed(float *state) override
Definition: ekf_helper.cpp:902
estimator_status_s::pos_vert_accuracy
float pos_vert_accuracy
Definition: estimator_status.h:93
ekf2_innovations_s::heading_innov_var
float heading_innov_var
Definition: ekf2_innovations.h:63
Ekf::get_heading_innov
void get_heading_innov(float *heading_innov) override
Definition: ekf_helper.cpp:859
vehicle_land_detected_s::landed
bool landed
Definition: vehicle_land_detected.h:58
matrix::Dcmf
Dcm< float > Dcmf
Definition: Dcm.hpp:185
EstimatorInterface::get_terrain_vert_pos
void get_terrain_vert_pos(float *ret)
Definition: estimator_interface.h:282
optical_flow_s::pixel_flow_y_integral
float pixel_flow_y_integral
Definition: optical_flow.h:55
get_vector_to_next_waypoint
void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
Definition: geo.cpp:337
Ekf2::_range_finder_subs
uORB::Subscription _range_finder_subs[ORB_MULTI_MAX_INSTANCES]
Definition: ekf2_main.cpp:260
airspeed_s::true_airspeed_m_s
float true_airspeed_m_s
Definition: airspeed.h:55
Ekf2::update_mag_decl
bool update_mag_decl(Param &mag_decl_param)
Definition: ekf2_main.cpp:689
estimator_status_s::pre_flt_fail_innov_vel_horiz
bool pre_flt_fail_innov_vel_horiz
Definition: estimator_status.h:108
Ekf2::runPreFlightChecks
void runPreFlightChecks(float dt, const filter_control_status_u &control_status, const vehicle_status_s &vehicle_status, const ekf2_innovations_s &innov)
Definition: ekf2_main.cpp:1667
EstimatorInterface::set_is_fixed_wing
void set_is_fixed_wing(bool is_fixed_wing)
Definition: estimator_interface.h:237
sensor_bias_s
Definition: sensor_bias.h:51
ekf_gps_position_s::timestamp
uint64_t timestamp
Definition: ekf_gps_position.h:53
main
int main(int argc, char **argv)
Definition: main.cpp:3
Ekf::get_gyro_bias
void get_gyro_bias(float bias[3]) override
Definition: ekf_helper.cpp:948
vehicle_magnetometer.h
vehicle_gps_position_s::vdop
float vdop
Definition: vehicle_gps_position.h:64
vehicle_global_position_s::yaw
float yaw
Definition: vehicle_global_position.h:62
Ekf2::_blended_gps_pub
uORB::Publication< ekf_gps_position_s > _blended_gps_pub
Definition: ekf2_main.cpp:273
optical_flow_s::max_flow_rate
float max_flow_rate
Definition: optical_flow.h:62
vehicle_local_position.h
vehicle_odometry.h
Ekf::get_output_tracking_error
void get_output_tracking_error(float error[3]) override
Definition: ekf_helper.cpp:969
vehicle_local_position_s::vxy_reset_counter
uint8_t vxy_reset_counter
Definition: vehicle_local_position.h:89
estimator::gps_message::sacc
float sacc
GPS speed accuracy in m/s.
Definition: common.h:67
vehicle_global_position_s::delta_alt
float delta_alt
Definition: vehicle_global_position.h:58
__EXPORT
Definition: I2C.hpp:51
Ekf2::Run
void Run() override
Definition: ekf2_main.cpp:703
vehicle_odometry_s
Definition: vehicle_odometry.h:66
Ekf::get_drag_innov_var
void get_drag_innov_var(float drag_innov_var[2]) override
Definition: optflow_fusion.cpp:535
estimator_status_s::health_flags
uint8_t health_flags
Definition: estimator_status.h:112
vehicle_local_position_s::xy_reset_counter
uint8_t xy_reset_counter
Definition: vehicle_local_position.h:87
Ekf::get_mag_innov_var
void get_mag_innov_var(float mag_innov_var[3]) override
Definition: ekf_helper.cpp:872
estimator::flow_message::gyrodata
Vector3f gyrodata
Gyro rates about the XYZ body axes (rad/sec)
Definition: common.h:78
wind_estimate_s
Definition: wind_estimate.h:51
ekf_gps_position_s::selected
uint8_t selected
Definition: ekf_gps_position.h:70
EstimatorInterface::set_fuse_beta_flag
void set_fuse_beta_flag(bool fuse_beta)
Definition: estimator_interface.h:240
Ekf2::_replay_mode
const bool _replay_mode
true when we use replay data from a log
Definition: ekf2_main.cpp:174
EstimatorInterface::set_gnd_effect_flag
void set_gnd_effect_flag(bool gnd_effect)
Definition: estimator_interface.h:245
GPS_MAX_RECEIVERS
#define GPS_MAX_RECEIVERS
Definition: ekf2_main.cpp:89
wind_estimate.h
estimator::ext_vision_message
Definition: common.h:82
vehicle_local_position_s::v_xy_valid
bool v_xy_valid
Definition: vehicle_local_position.h:85
vehicle_local_position_s::dist_bottom_valid
bool dist_bottom_valid
Definition: vehicle_local_position.h:93
estimator_status_s::beta_test_ratio
float beta_test_ratio
Definition: estimator_status.h:100
estimator::gps_message::fix_type
uint8_t fix_type
0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic ...
Definition: common.h:64
Ekf2::calc_gps_blend_output
void calc_gps_blend_output()
Definition: ekf2_main.cpp:2271
estimator::filter_control_status_u::mag_field_disturbed
uint32_t mag_field_disturbed
16 - true when the mag field does not match the expected strength
Definition: common.h:457
vehicle_odometry_s::vy
float vy
Definition: vehicle_odometry.h:76
print_status
void print_status()
Definition: Commander.cpp:517
vehicle_global_position_s::terrain_alt
float terrain_alt
Definition: vehicle_global_position.h:65
optical_flow_s::min_ground_distance
float min_ground_distance
Definition: optical_flow.h:63
parameter_update_s
Definition: parameter_update.h:51
optical_flow_s::pixel_flow_x_integral
float pixel_flow_x_integral
Definition: optical_flow.h:54
Ekf2::print_status
int print_status() override
Definition: ekf2_main.cpp:657
vehicle_global_position_s::eph
float eph
Definition: vehicle_global_position.h:63
estimator_status_s::filter_fault_flags
uint16_t filter_fault_flags
Definition: estimator_status.h:103
sensor_combined_s::accelerometer_m_s2
float accelerometer_m_s2[3]
Definition: sensor_combined.h:58
Ekf2::Ekf2
Ekf2(bool replay_mode=false)
Definition: ekf2_main.cpp:533
vehicle_gps_position_s::heading_offset
float heading_offset
Definition: vehicle_gps_position.h:74
estimator::gps_message::pdop
float pdop
position dilution of precision
Definition: common.h:72
vehicle_local_position_s::xy_valid
bool xy_valid
Definition: vehicle_local_position.h:83
ekf2_timestamps_s::timestamp
uint64_t timestamp
Definition: ekf2_timestamps.h:54
vehicle_global_position_s::terrain_alt_valid
bool terrain_alt_valid
Definition: vehicle_global_position.h:68
estimator_status_s::vel_test_ratio
float vel_test_ratio
Definition: estimator_status.h:95
Ekf2::print_usage
static int print_usage(const char *reason=nullptr)
Definition: ekf2_main.cpp:2386
vehicle_local_position_s::z_valid
bool z_valid
Definition: vehicle_local_position.h:84
estimator::flow_message
Definition: common.h:75
print_usage
static void print_usage()
Definition: uavcan_main.cpp:996
Ekf::get_imu_vibe_metrics
void get_imu_vibe_metrics(float vibe[3]) override
Definition: ekf_helper.cpp:980
EstimatorInterface::get_velocity
void get_velocity(float *vel)
Definition: estimator_interface.h:293
vehicle_local_position_s::dist_bottom
float dist_bottom
Definition: vehicle_local_position.h:73
vehicle_local_position_s::vz_max
float vz_max
Definition: vehicle_local_position.h:80
EstimatorInterface::setGpsData
void setGpsData(uint64_t time_usec, const gps_message &gps)
Definition: estimator_interface.cpp:193
Ekf2::_estimator_innovations_pub
uORB::Publication< ekf2_innovations_s > _estimator_innovations_pub
Definition: ekf2_main.cpp:270
Ekf2::_sensors_sub
uORB::SubscriptionCallbackWorkItem _sensors_sub
Definition: ekf2_main.cpp:257
vehicle_status_s
Definition: vehicle_status.h:94
vehicle_gps_position_s::heading
float heading
Definition: vehicle_gps_position.h:73
vehicle_global_position_s::vel_d
float vel_d
Definition: vehicle_global_position.h:61
vehicle_status.h
ll40ls::instance
LidarLite * instance
Definition: ll40ls.cpp:65
vehicle_global_position_s::lon
double lon
Definition: vehicle_global_position.h:55
vehicle_local_position_s::delta_vz
float delta_vz
Definition: vehicle_local_position.h:67
estimator::gps_message::vel_ned
float vel_ned[3]
GPS ground speed NED.
Definition: common.h:69
vehicle_local_position_s::evh
float evh
Definition: vehicle_local_position.h:77
PreFlightChecker::setUsingGpsAiding
void setUsingGpsAiding(bool val)
Definition: PreFlightChecker.hpp:76
map_projection_reference_s::lat_rad
double lat_rad
Definition: geo.h:77
Ekf::get_flow_innov_var
void get_flow_innov_var(float flow_innov_var[2]) override
Definition: optflow_fusion.cpp:524
ekf2_innovations_s::beta_innov_var
float beta_innov_var
Definition: ekf2_innovations.h:65
Ekf2::update_mag_bias
void update_mag_bias(Param &mag_bias_param, int axis_index)
Definition: ekf2_main.cpp:670
estimator::flow_message::dt
uint32_t dt
integration time of flow samples (sec)
Definition: common.h:79
matrix::Matrix::copyTo
void copyTo(Type dst[M *N]) const
Definition: Matrix.hpp:115
landing_target_pose_s::vy_rel
float vy_rel
Definition: landing_target_pose.h:58
FLT_EPSILON
#define FLT_EPSILON
Definition: stdlib_imports.hpp:26
matrix::Quaternion
Quaternion class.
Definition: Dcm.hpp:24
drv_hrt.h
High-resolution timer with callouts and timekeeping.
vehicle_gps_position_s::hdop
float hdop
Definition: vehicle_gps_position.h:63
ekf2_innovations_s::hagl_innov_var
float hagl_innov_var
Definition: ekf2_innovations.h:67
vehicle_air_data_s::baro_alt_meter
float baro_alt_meter
Definition: vehicle_air_data.h:54
Ekf::get_wind_velocity
void get_wind_velocity(float *wind) override
Definition: airspeed_fusion.cpp:239
estimator::flow_message::quality
uint8_t quality
Quality of Flow data.
Definition: common.h:76
CONSTANTS_ONE_G
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
EstimatorInterface::get_control_mode
void get_control_mode(uint32_t *val)
Definition: estimator_interface.h:352
Ekf2::_vehicle_local_position_pub
uORB::PublicationData< vehicle_local_position_s > _vehicle_local_position_pub
Definition: ekf2_main.cpp:280
estimator_status_s::time_slip
float time_slip
Definition: estimator_status.h:101
vehicle_global_position_s::vel_n
float vel_n
Definition: vehicle_global_position.h:59
PreFlightChecker::update
void update(float dt, const ekf2_innovations_s &innov)
Definition: PreFlightChecker.cpp:41
Ekf2::update_gps_offsets
void update_gps_offsets()
Definition: ekf2_main.cpp:2168
vehicle_local_position_s::ref_lon
double ref_lon
Definition: vehicle_local_position.h:56
ekf_gps_position_s::lon
int32_t lon
Definition: ekf_gps_position.h:55
BLEND_MASK_USE_VPOS_ACC
#define BLEND_MASK_USE_VPOS_ACC
Definition: ekf2_main.cpp:86
vehicle_local_position_s::vz_reset_counter
uint8_t vz_reset_counter
Definition: vehicle_local_position.h:90
vehicle_local_position_s::ax
float ax
Definition: vehicle_local_position.h:68
vehicle_local_position_s::xy_global
bool xy_global
Definition: vehicle_local_position.h:91
estimator_status_s::timestamp
uint64_t timestamp
Definition: estimator_status.h:87
ekf_gps_position_s::satellites_used
uint8_t satellites_used
Definition: ekf_gps_position.h:69
estimator_status_s::pre_flt_fail_innov_vel_vert
bool pre_flt_fail_innov_vel_vert
Definition: estimator_status.h:109
estimator_status_s
Definition: estimator_status.h:85
ekf_gps_position_s::eph
float eph
Definition: ekf_gps_position.h:59
Ekf2::_gps_slowest_index
uint8_t _gps_slowest_index
index of the physical receiver with the slowest update rate
Definition: ekf2_main.cpp:231
Ekf2::_gps_subs
uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS]
Definition: ekf2_main.cpp:264
vehicle_gps_position_s::vel_e_m_s
float vel_e_m_s
Definition: vehicle_gps_position.h:69
vehicle_odometry_s::q
float q[4]
Definition: vehicle_odometry.h:72
vehicle_status_s::arming_state
uint8_t arming_state
Definition: vehicle_status.h:102
msg
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
optical_flow_s::max_ground_distance
float max_ground_distance
Definition: optical_flow.h:64
Ekf2::resetPreFlightChecks
void resetPreFlightChecks()
Definition: ekf2_main.cpp:1682
ekf_gps_drift_s::blocked
bool blocked
Definition: ekf_gps_drift.h:57
Ekf2::_vehicle_status
vehicle_status_s _vehicle_status
Definition: ekf2_main.cpp:268
vehicle_air_data_s::rho
float rho
Definition: vehicle_air_data.h:57
Ekf2::_ev_odom_sub
uORB::Subscription _ev_odom_sub
Definition: ekf2_main.cpp:248
ekf_gps_drift_s::timestamp
uint64_t timestamp
Definition: ekf_gps_drift.h:53
Ekf2::_gps_output
gps_message _gps_output[GPS_MAX_RECEIVERS+1]
output state data for the physical and blended GPS
Definition: ekf2_main.cpp:219
Ekf::get_airspeed_innov_var
void get_airspeed_innov_var(float *airspeed_innov_var) override
Definition: ekf_helper.cpp:878
estimator::ext_vision_message::velErr
float velErr
1-Sigma velocity accuracy (m/sec)
Definition: common.h:88
PreFlightChecker::hasFailed
bool hasFailed() const
Definition: PreFlightChecker.hpp:89
ORB_ID
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Ekf::get_accel_bias
void get_accel_bias(float bias[3]) override
Definition: ekf_helper.cpp:938
ekf_gps_position_s::vel_m_s
float vel_m_s
Definition: ekf_gps_position.h:61
vehicle_local_position_s::hagl_min
float hagl_min
Definition: vehicle_local_position.h:81
ekf_gps_drift_s::hpos_drift_rate
float hpos_drift_rate
Definition: ekf_gps_drift.h:54
PreFlightChecker::hasHorizVelFailed
bool hasHorizVelFailed() const
Definition: PreFlightChecker.hpp:81
vehicle_local_position_s::evv
float evv
Definition: vehicle_local_position.h:78
vehicle_magnetometer_s::magnetometer_ga
float magnetometer_ga[3]
Definition: vehicle_magnetometer.h:54
estimator_status_s::pre_flt_fail_innov_heading
bool pre_flt_fail_innov_heading
Definition: estimator_status.h:107
Ekf2::_preflt_checker
PreFlightChecker _preflt_checker
Definition: ekf2_main.cpp:120
estimator::ext_vision_message::hgtErr
float hgtErr
1-Sigma height accuracy (m)
Definition: common.h:87
uORB::Publication::publish
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
PublicationMulti.hpp
distance_sensor_s::timestamp
uint64_t timestamp
Definition: distance_sensor.h:72
ekf_gps_position_s::vel_e_m_s
float vel_e_m_s
Definition: ekf_gps_position.h:63
vehicle_local_position_s
Definition: vehicle_local_position.h:51
vehicle_attitude.h
Ekf::get_heading_innov_var
void get_heading_innov_var(float *heading_innov_var) override
Definition: ekf_helper.cpp:890
ekf2_main
__EXPORT int ekf2_main(int argc, char *argv[])
Definition: ekf2_main.cpp:2412
vehicle_odometry_s::q_offset
float q_offset[4]
Definition: vehicle_odometry.h:73
perf_ctr_header
Header common to all counters.
Definition: perf_counter.cpp:65
Ekf2::update_gps_blend_states
void update_gps_blend_states()
Definition: ekf2_main.cpp:2017
Ekf2::_gps_alttitude_ellipsoid_previous_timestamp
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS]
storage for previous timestamp to compute dt
Definition: ekf2_main.cpp:237
perf_free
void perf_free(perf_counter_t handle)
Free a counter.
Definition: perf_counter.cpp:185
ekf_gps_position_s::s_variance_m_s
float s_variance_m_s
Definition: ekf_gps_position.h:58
map_projection_reproject
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using...
Definition: geo.cpp:166
ekf_gps_position_s::vel_ned_valid
bool vel_ned_valid
Definition: ekf_gps_position.h:68
vehicle_local_position_s::z_reset_counter
uint8_t z_reset_counter
Definition: vehicle_local_position.h:88
ToneAlarmInterface::init
void init()
Activates/configures the hardware registers.
estimator::gps_message::vel_ned_valid
bool vel_ned_valid
GPS ground speed is valid.
Definition: common.h:70
Ekf2::publish_wind_estimate
bool publish_wind_estimate(const hrt_abstime &timestamp)
Definition: ekf2_main.cpp:1730
ekf2_timestamps.h
Ekf::get_velD_reset
void get_velD_reset(float *delta, uint8_t *counter) override
Definition: ekf.h:226
sensor_combined_s::gyro_rad
float gyro_rad[3]
Definition: sensor_combined.h:55
EstimatorInterface::setBaroData
void setBaroData(uint64_t time_usec, float data)
Definition: estimator_interface.cpp:254
Ekf2::_gps_time_ref_index
uint8_t _gps_time_ref_index
index of the receiver that is used as the timing reference for the blending update ...
Definition: ekf2_main.cpp:227
Ekf::get_vel_pos_innov_var
void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override
Definition: ekf_helper.cpp:866
Ekf::get_posNE_reset
void get_posNE_reset(float delta[2], uint8_t *counter) override
Definition: ekf.h:229
vehicle_local_position_s::timestamp
uint64_t timestamp
Definition: vehicle_local_position.h:53
vehicle_local_position_s::vy
float vy
Definition: vehicle_local_position.h:63
ekf2_innovations_s::vel_pos_innov_var
float vel_pos_innov_var[6]
Definition: ekf2_innovations.h:61
Ekf2::_params
parameters * _params
pointer to ekf parameter struct (located in _ekf class instance)
Definition: ekf2_main.cpp:285
estimator_status_s::solution_status_flags
uint16_t solution_status_flags
Definition: estimator_status.h:105
ekf_gps_position_s::fix_type
uint8_t fix_type
Definition: ekf_gps_position.h:67
Ekf::reset_imu_bias
bool reset_imu_bias() override
Definition: ekf_helper.cpp:1122
vehicle_gps_position_s::eph
float eph
Definition: vehicle_gps_position.h:61
vehicle_local_position_s::delta_vxy
float delta_vxy[2]
Definition: vehicle_local_position.h:66
Ekf2::_ekf2_timestamps_pub
uORB::Publication< ekf2_timestamps_s > _ekf2_timestamps_pub
Definition: ekf2_main.cpp:271
ekf2_innovations_s::drag_innov_var
float drag_innov_var[2]
Definition: ekf2_innovations.h:70
data
uint8_t * data
Definition: dataman.cpp:149
estimator::imuSample::time_us
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:111
vehicle_odometry_s::z
float z
Definition: vehicle_odometry.h:71
vehicle_gps_position.h
Ekf::get_ekf_gpos_accuracy
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) override
Definition: ekf_helper.cpp:1005
distance_sensor_s::max_distance
float max_distance
Definition: distance_sensor.h:74
Ekf2::_airspeed_sub
uORB::Subscription _airspeed_sub
Definition: ekf2_main.cpp:247
EstimatorInterface::get_vel_deriv_ned
void get_vel_deriv_ned(float *vel_deriv)
Definition: estimator_interface.h:303
BLEND_MASK_USE_SPD_ACC
#define BLEND_MASK_USE_SPD_ACC
Definition: ekf2_main.cpp:84
EstimatorInterface::get_filter_fault_status
void get_filter_fault_status(uint16_t *val)
Definition: estimator_interface.h:358
perf_alloc_once
perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name)
Get the reference to an existing counter or create a new one if it does not exist.
Definition: perf_counter.cpp:156
vehicle_local_position_s::v_z_valid
bool v_z_valid
Definition: vehicle_local_position.h:86
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Ekf::get_gps_drift_metrics
bool get_gps_drift_metrics(float drift[3], bool *blocked) override
Definition: ekf_helper.cpp:993
matrix::Vector2f
Vector2< float > Vector2f
Definition: Vector2.hpp:69
add_vector_to_global_position
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
Definition: geo.cpp:364
Ekf2::_gps_alttitude_ellipsoid
int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS]
altitude in 1E-3 meters (millimeters) above ellipsoid
Definition: ekf2_main.cpp:236
ekf_gps_position_s::epv
float epv
Definition: ekf_gps_position.h:60
estimator::ext_vision_message::pos
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Definition: common.h:83
sensor_selection_s
Definition: sensor_selection.h:51
Ekf::get_airspeed_innov
void get_airspeed_innov(float *airspeed_innov) override
Definition: ekf_helper.cpp:847
matrix::Eulerf
Euler< float > Eulerf
Definition: Euler.hpp:156
PreFlightChecker::hasHorizFailed
bool hasHorizFailed() const
Definition: PreFlightChecker.hpp:95
airspeed_s::indicated_airspeed_m_s
float indicated_airspeed_m_s
Definition: airspeed.h:54
ekf_gps_drift_s::hspd
float hspd
Definition: ekf_gps_drift.h:56
vehicle_global_position_s::alt
float alt
Definition: vehicle_global_position.h:56
Ekf2::_gps_state
gps_message _gps_state[GPS_MAX_RECEIVERS]
internal state data for the physical GPS
Definition: ekf2_main.cpp:217
vehicle_attitude_s::q
float q[4]
Definition: vehicle_attitude.h:54
estimator::gps_message::lat
int32_t lat
Latitude in 1E-7 degrees.
Definition: common.h:59
vehicle_odometry_s::vz
float vz
Definition: vehicle_odometry.h:77
ekf2_innovations_s::drag_innov
float drag_innov[2]
Definition: ekf2_innovations.h:69
estimator_status_s::vibe
float vibe[3]
Definition: estimator_status.h:89
EstimatorInterface::get_hagl_innov_var
void get_hagl_innov_var(float *hagl_innov_var)
Definition: estimator_interface.h:134
Ekf2::_ev_odom
vehicle_odometry_s _ev_odom
Definition: ekf2_main.cpp:244
ekf2_innovations_s::flow_innov_var
float flow_innov_var[2]
Definition: ekf2_innovations.h:66
estimator_status_s::pre_flt_fail_mag_field_disturbed
bool pre_flt_fail_mag_field_disturbed
Definition: estimator_status.h:111
Ekf::get_ev2ekf_quaternion
void get_ev2ekf_quaternion(float *quat) override
Definition: ekf_helper.cpp:1680
Ekf::get_ekf_ctrl_limits
void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) override
Definition: ekf_helper.cpp:1079
Ekf2::_wind_pub
uORB::PublicationMulti< wind_estimate_s > _wind_pub
Definition: ekf2_main.cpp:278
Ekf::get_ekf_lpos_accuracy
void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override
Definition: ekf_helper.cpp:1024
perf_end
void perf_end(perf_counter_t handle)
End a performance event.
Definition: perf_counter.cpp:275
Ekf::get_ekf_soln_status
void get_ekf_soln_status(uint16_t *status) override
Definition: ekf_helper.cpp:1178
ekf2_innovations_s::vel_pos_innov
float vel_pos_innov[6]
Definition: ekf2_innovations.h:54
ekf_gps_position_s::heading
float heading
Definition: ekf_gps_position.h:65
estimator_status_s::mag_test_ratio
float mag_test_ratio
Definition: estimator_status.h:94
Ekf2::_estimator_status_pub
uORB::Publication< estimator_status_s > _estimator_status_pub
Definition: ekf2_main.cpp:274
sensor_combined_s::gyro_integral_dt
uint32_t gyro_integral_dt
Definition: sensor_combined.h:56
estimator::filter_control_status_u
Definition: common.h:439
uORB::Subscription::updated
bool updated()
Check if there is a new update.
Definition: Subscription.hpp:98
vehicle_magnetometer_s
Definition: vehicle_magnetometer.h:51
Ekf::get_posD_reset
void get_posD_reset(float *delta, uint8_t *counter) override
Definition: ekf.h:223
Ekf2::apply_gps_offsets
void apply_gps_offsets()
Definition: ekf2_main.cpp:2235
vehicle_local_position_s::ref_lat
double ref_lat
Definition: vehicle_local_position.h:55
hrt_abstime
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
EstimatorInterface::setAirspeedData
void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas)
Definition: estimator_interface.cpp:287
landing_target_pose_s::timestamp
uint64_t timestamp
Definition: landing_target_pose.h:53
distance_sensor_s::current_distance
float current_distance
Definition: distance_sensor.h:75
sensor_combined_s
Definition: sensor_combined.h:52
estimator_status_s::timeout_flags
uint8_t timeout_flags
Definition: estimator_status.h:113
ekf2_timestamps_s
Definition: ekf2_timestamps.h:52
vehicle_land_detected_s
Definition: vehicle_land_detected.h:51
vehicle_odometry_s::y
float y
Definition: vehicle_odometry.h:70
Ekf2::_gps_blended_state
gps_message _gps_blended_state
internal state data for the blended GPS
Definition: ekf2_main.cpp:218
estimator_status_s::states
float states[24]
Definition: estimator_status.h:88
Ekf2::_mag_sample_count
uint8_t _mag_sample_count
number of magnetometer measurements summed during downsampling
Definition: ekf2_main.cpp:189
vehicle_gps_position_s
Definition: vehicle_gps_position.h:51
vehicle_global_position_s::dead_reckoning
bool dead_reckoning
Definition: vehicle_global_position.h:69
estimator::flow_message::flowdata
Vector2f flowdata
Optical flow rates about the X and Y body axes (rad/sec)
Definition: common.h:77
estimator_status_s::pos_horiz_accuracy
float pos_horiz_accuracy
Definition: estimator_status.h:92
ekf2_innovations_s::airspeed_innov_var
float airspeed_innov_var
Definition: ekf2_innovations.h:64
vehicle_global_position_s::alt_ellipsoid
float alt_ellipsoid
Definition: vehicle_global_position.h:57
vehicle_air_data_s
Definition: vehicle_air_data.h:51
ekf_gps_position_s::vel_n_m_s
float vel_n_m_s
Definition: ekf_gps_position.h:62
Ekf::get_flow_innov
void get_flow_innov(float flow_innov[2]) override
Definition: optflow_fusion.cpp:518
Ekf2::blend_gps_data
bool blend_gps_data()
Definition: ekf2_main.cpp:1778
vehicle_attitude_s::timestamp
uint64_t timestamp
Definition: vehicle_attitude.h:53
PreFlightChecker::setVehicleCanObserveHeadingInFlight
void setVehicleCanObserveHeadingInFlight(bool val)
Definition: PreFlightChecker.hpp:74
map_projection_reference_s
Definition: geo.h:75
sensor_combined_s::timestamp
uint64_t timestamp
Definition: sensor_combined.h:54
Ekf2::custom_command
static int custom_command(int argc, char *argv[])
Definition: ekf2_main.cpp:2351
Ekf::get_mag_innov
void get_mag_innov(float mag_innov[3]) override
Definition: ekf_helper.cpp:841
EstimatorInterface::get_mag_decl_deg
bool get_mag_decl_deg(float *val)
Definition: estimator_interface.h:336
vehicle_local_position_s::z_deriv
float z_deriv
Definition: vehicle_local_position.h:65
vehicle_global_position_s::timestamp
uint64_t timestamp
Definition: vehicle_global_position.h:53
vehicle_gps_position_s::vel_d_m_s
float vel_d_m_s
Definition: vehicle_gps_position.h:70
sensors
Definition: common.h:43
Ekf::get_aux_vel_innov
void get_aux_vel_innov(float aux_vel_innov[2]) override
Definition: ekf_helper.cpp:835
sensor_bias_s::timestamp
uint64_t timestamp
Definition: sensor_bias.h:53
estimator::ext_vision_message::vel
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
Definition: common.h:84
matrix::Vector3f
Vector3< float > Vector3f
Definition: Vector3.hpp:136
vehicle_odometry_s::timestamp
uint64_t timestamp
Definition: vehicle_odometry.h:68
estimator::gps_message::yaw
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:62
vehicle_local_position_s::ay
float ay
Definition: vehicle_local_position.h:69
EstimatorInterface::setRangeData
void setRangeData(uint64_t time_usec, float data, int8_t quality)
Definition: estimator_interface.cpp:317
estimator::filter_control_status_u::opt_flow
uint32_t opt_flow
3 - true if optical flow measurements are being fused
Definition: common.h:444
matrix::Quaternion::inversed
Quaternion inversed() const
Invert quaternion.
Definition: Quaternion.hpp:343
EstimatorInterface::set_rangefinder_limits
void set_rangefinder_limits(float min_distance, float max_distance)
Definition: estimator_interface.h:255
Ekf::covariances_diagonal
matrix::Vector< float, 24 > covariances_diagonal() const
Definition: ekf.h:132
vehicle_land_detected_s::in_ground_effect
bool in_ground_effect
Definition: vehicle_land_detected.h:59
Ekf::get_velNE_reset
void get_velNE_reset(float delta[2], uint8_t *counter) override
Definition: ekf.h:236
vehicle_local_position_s::yaw
float yaw
Definition: vehicle_local_position.h:71
estimator_status.h
Ekf2::task_spawn
static int task_spawn(int argc, char *argv[])
Definition: ekf2_main.cpp:2356
airspeed_s::timestamp
uint64_t timestamp
Definition: airspeed.h:53
ekf2_innovations_s::mag_innov
float mag_innov[3]
Definition: ekf2_innovations.h:55
Ekf
Definition: ekf.h:47
ekf2_innovations_s
Definition: ekf2_innovations.h:51
vehicle_local_position_s::vz
float vz
Definition: vehicle_local_position.h:64
vehicle_attitude_s::quat_reset_counter
uint8_t quat_reset_counter
Definition: vehicle_attitude.h:56
vehicle_global_position.h
landing_target_pose_s::vx_rel
float vx_rel
Definition: landing_target_pose.h:57
estimator_status_s::hgt_test_ratio
float hgt_test_ratio
Definition: estimator_status.h:97
PreFlightChecker::hasVertFailed
bool hasVertFailed() const
Definition: PreFlightChecker.hpp:101
vehicle_local_position_s::epv
float epv
Definition: vehicle_local_position.h:76
vehicle_global_position_s::epv
float epv
Definition: vehicle_global_position.h:64
estimator::filter_control_status_u::mag_3D
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Definition: common.h:446
Ekf2::_att_pub
uORB::Publication< vehicle_attitude_s > _att_pub
Definition: ekf2_main.cpp:276
distance_sensor.h
matrix::Quatf
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
vehicle_gps_position_s::satellites_used
uint8_t satellites_used
Definition: vehicle_gps_position.h:77
estimator_status_s::hagl_test_ratio
float hagl_test_ratio
Definition: estimator_status.h:99
estimator::gps_message
Definition: common.h:57
Ekf::set_min_required_gps_health_time
void set_min_required_gps_health_time(uint32_t time_us)
Definition: ekf.h:266
distance_sensor_s::signal_quality
int8_t signal_quality
Definition: distance_sensor.h:80
estimator::ext_vision_message::angErr
float angErr
1-Sigma angular error (rad)
Definition: common.h:89
dt
float dt
Definition: px4io.c:73
estimator_status_s::pos_test_ratio
float pos_test_ratio
Definition: estimator_status.h:96
Ekf2::_vehicle_land_detected
vehicle_land_detected_s _vehicle_land_detected
Definition: ekf2_main.cpp:267
M_PI
#define M_PI
Definition: gps_helper.cpp:38
vehicle_gps_position_s::s_variance_m_s
float s_variance_m_s
Definition: vehicle_gps_position.h:59
vehicle_gps_position_s::alt_ellipsoid
int32_t alt_ellipsoid
Definition: vehicle_gps_position.h:58
airspeed_s
Definition: airspeed.h:51
vehicle_local_position_s::hagl_max
float hagl_max
Definition: vehicle_local_position.h:82
vehicle_gps_position_s::vel_n_m_s
float vel_n_m_s
Definition: vehicle_gps_position.h:68
vehicle_attitude_s::delta_q_reset
float delta_q_reset[4]
Definition: vehicle_attitude.h:55
vehicle_odometry_s::pose_covariance
float pose_covariance[21]
Definition: vehicle_odometry.h:74
Ekf::get_vel_pos_innov
void get_vel_pos_innov(float vel_pos_innov[6]) override
Definition: ekf_helper.cpp:829
perf_print_counter
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
Definition: perf_counter.cpp:437
ORB_MULTI_MAX_INSTANCES
#define ORB_MULTI_MAX_INSTANCES
Maximum number of multi topic instances.
Definition: uORB.h:62
ekf2_innovations_s::mag_innov_var
float mag_innov_var[3]
Definition: ekf2_innovations.h:62
SubscriptionCallback.hpp
PreFlightChecker.hpp
Class handling the EKF2 innovation pre flight checks.
landing_target_pose_s::is_static
bool is_static
Definition: landing_target_pose.h:66
vehicle_gps_position_s::vel_ned_valid
bool vel_ned_valid
Definition: vehicle_gps_position.h:76
estimator::gps_message::alt
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
Definition: common.h:61
GPS_BLENDED_INSTANCE
#define GPS_BLENDED_INSTANCE
Definition: ekf2_main.cpp:90
Ekf2::_ekf_update_perf
perf_counter_t _ekf_update_perf
Definition: ekf2_main.cpp:181
px4
Definition: bst.cpp:62
estimator::parameters::sensor_interval_min_ms
int32_t sensor_interval_min_ms
minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation b...
Definition: common.h:224
ekf_gps_position.h
ekf2_innovations_s::heading_innov
float heading_innov
Definition: ekf2_innovations.h:56
estimator_status_s::gps_check_fail_flags
uint16_t gps_check_fail_flags
Definition: estimator_status.h:102
optical_flow_s::integration_timespan
uint32_t integration_timespan
Definition: optical_flow.h:60
Ekf2::get_vel_body_wind
const Vector3f get_vel_body_wind()
Definition: ekf2_main.cpp:1758
Ekf2::publish_attitude
bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now)
Definition: ekf2_main.cpp:1704
PreFlightChecker::setUsingEvPosAiding
void setUsingEvPosAiding(bool val)
Definition: PreFlightChecker.hpp:78
Publication.hpp
Ekf::get_gps_check_status
void get_gps_check_status(uint16_t *val) override
Definition: ekf_helper.cpp:896
estimator::ext_vision_message::quat
Quatf quat
quaternion defining rotation from body to earth frame
Definition: common.h:85
Ekf2::_sensor_selection
sensor_selection_s _sensor_selection
Definition: ekf2_main.cpp:266
estimator::filter_control_status_u::ev_pos
uint32_t ev_pos
12 - true when local position data from external vision is being fused
Definition: common.h:453
vehicle_local_position_s::delta_z
float delta_z
Definition: vehicle_local_position.h:61
vehicle_global_position_s::lat_lon_reset_counter
uint8_t lat_lon_reset_counter
Definition: vehicle_global_position.h:66
Ekf2::_gps_newest_index
uint8_t _gps_newest_index
index of the physical receiver with the newest data
Definition: ekf2_main.cpp:230
EstimatorInterface::get_pos_d_deriv
void get_pos_d_deriv(float *pos_d_deriv)
Definition: estimator_interface.h:311
sensor_selection_s::mag_device_id
uint32_t mag_device_id
Definition: sensor_selection.h:57
sensor_selection_s::accel_device_id
uint32_t accel_device_id
Definition: sensor_selection.h:54
vehicle_gps_position_s::vel_m_s
float vel_m_s
Definition: vehicle_gps_position.h:67
vehicle_local_position_s::z
float z
Definition: vehicle_local_position.h:59
WaypointType::velocity
parameter_update.h
estimator_status_s::tas_test_ratio
float tas_test_ratio
Definition: estimator_status.h:98
Ekf2::fillGpsMsgWithVehicleGpsPosData
void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data)
Definition: ekf2_main.cpp:1646
Ekf2::_vehicle_land_detected_sub
uORB::Subscription _vehicle_land_detected_sub
Definition: ekf2_main.cpp:255
map_projection_reference_s::lon_rad
double lon_rad
Definition: geo.h:78
landing_target_pose_s
Definition: landing_target_pose.h:51
EstimatorInterface::set_in_air_status
void set_in_air_status(bool in_air)
Definition: estimator_interface.h:218
vehicle_local_position_s::x
float x
Definition: vehicle_local_position.h:57
sensor_selection_s::gyro_device_id
uint32_t gyro_device_id
Definition: sensor_selection.h:56
uORB::Subscription::update
bool update(void *dst)
Update the struct.
Definition: Subscription.hpp:104
estimator_status_s::pre_flt_fail_innov_height
bool pre_flt_fail_innov_height
Definition: estimator_status.h:110
estimator::filter_control_status_u::gps
uint32_t gps
2 - true if GPS measurements are being fused
Definition: common.h:443
Ekf::get_wind_velocity_var
void get_wind_velocity_var(float *wind_var) override
Definition: airspeed_fusion.cpp:245
estimator::filter_control_status_u::tilt_align
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
Definition: common.h:441
Ekf2::_ekf
Ekf _ekf
Definition: ekf2_main.cpp:283
optical_flow_s::gyro_z_rate_integral
float gyro_z_rate_integral
Definition: optical_flow.h:58
Ekf::get_quat_reset
void get_quat_reset(float delta_quat[4], uint8_t *counter) override
Definition: ekf.h:243
Ekf2::filter_altitude_ellipsoid
float filter_altitude_ellipsoid(float amsl_hgt)
Definition: ekf2_main.cpp:2329
Ekf2::_imu_bias_reset_request
bool _imu_bias_reset_request
Definition: ekf2_main.cpp:240
optical_flow_s::gyro_y_rate_integral
float gyro_y_rate_integral
Definition: optical_flow.h:57
ekf2_innovations_s::flow_innov
float flow_innov[2]
Definition: ekf2_innovations.h:59
uORB::PublicationMulti::publish
bool publish(const T &data)
Publish the struct.
Definition: PublicationMulti.hpp:73
Ekf2::_gps_select_index
uint8_t _gps_select_index
0 = GPS1, 1 = GPS2, 2 = blended
Definition: ekf2_main.cpp:226
vehicle_gps_position_s::epv
float epv
Definition: vehicle_gps_position.h:62
vehicle_global_position_s::lat
double lat
Definition: vehicle_global_position.h:54
ekf2_innovations_s::aux_vel_innov
float aux_vel_innov[2]
Definition: ekf2_innovations.h:71
sensor_combined.h
vehicle_local_position_s::ref_timestamp
uint64_t ref_timestamp
Definition: vehicle_local_position.h:54
vehicle_odometry_s::vx
float vx
Definition: vehicle_odometry.h:75
vehicle_land_detected.h
Ekf::get_beta_innov
void get_beta_innov(float *beta_innov) override
Definition: ekf_helper.cpp:853
estimator::imuSample::delta_vel
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
Definition: common.h:108
vehicle_air_data.h
Ekf::get_innovation_test_status
void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) override
Definition: ekf_helper.cpp:1157
estimator::filter_control_status_u::value
uint32_t value
Definition: common.h:468
ekf_gps_position_s::heading_offset
float heading_offset
Definition: ekf_gps_position.h:66
optical_flow_s
Definition: optical_flow.h:51
Ekf2::~Ekf2
~Ekf2() override
Definition: ekf2_main.cpp:641
landing_target_pose.h
estimator_status_s::innovation_check_flags
uint16_t innovation_check_flags
Definition: estimator_status.h:104
uORB::Subscription::copy
bool copy(void *dst)
Copy the struct.
Definition: Subscription.hpp:121
vehicle_gps_position_s::lat
int32_t lat
Definition: vehicle_gps_position.h:55
vehicle_local_position_s::dist_bottom_rate
float dist_bottom_rate
Definition: vehicle_local_position.h:74
landing_target_pose_s::cov_vx_rel
float cov_vx_rel
Definition: landing_target_pose.h:61
estimator::imuSample::delta_ang_dt
float delta_ang_dt
delta angle integration period (sec)
Definition: common.h:109
Ekf::get_drag_innov
void get_drag_innov(float drag_innov[2]) override
Definition: optflow_fusion.cpp:529
PreFlightChecker::hasVertVelFailed
bool hasVertVelFailed() const
Definition: PreFlightChecker.hpp:82
Ekf2::init
bool init()
Definition: ekf2_main.cpp:647
ekf.h
Class for core functions for ekf attitude and position estimator.
PreFlightChecker::hasHeadingFailed
bool hasHeadingFailed() const
Definition: PreFlightChecker.hpp:80
Ekf::update
bool update() override
Definition: ekf.cpp:117
vehicle_gps_position_s::timestamp
uint64_t timestamp
Definition: vehicle_gps_position.h:53
ekf2_innovations_s::output_tracking_error
float output_tracking_error[3]
Definition: ekf2_innovations.h:68
EstimatorInterface::set_air_density
void set_air_density(float air_density)
Definition: estimator_interface.h:252
perf_begin
void perf_begin(perf_counter_t handle)
Begin a performance event.
Definition: perf_counter.cpp:258
Ekf::get_beta_innov_var
void get_beta_innov_var(float *beta_innov_var) override
Definition: ekf_helper.cpp:884
Ekf2::_optical_flow_sub
uORB::Subscription _optical_flow_sub
Definition: ekf2_main.cpp:251
sensor_combined_s::accelerometer_integral_dt
uint32_t accelerometer_integral_dt
Definition: sensor_combined.h:59
ekf_gps_position_s::vel_d_m_s
float vel_d_m_s
Definition: ekf_gps_position.h:64
estimator::imuSample::delta_vel_dt
float delta_vel_dt
delta velocity integration period (sec)
Definition: common.h:110
estimator_status_s::n_states
uint8_t n_states
Definition: estimator_status.h:106
vehicle_local_position_s::ref_alt
float ref_alt
Definition: vehicle_local_position.h:72
Ekf::global_position_is_valid
bool global_position_is_valid() override
Definition: ekf_helper.cpp:1317
estimator::parameters::gps_check_mask
int32_t gps_check_mask
bitmask used to control which GPS quality checks are used
Definition: common.h:315
sensor_selection.h
Ekf2::_start_time_us
uint64_t _start_time_us
system time at EKF start (uSec)
Definition: ekf2_main.cpp:178
vehicle_global_position_s::vel_e
float vel_e
Definition: vehicle_global_position.h:60
EstimatorInterface::get_quaternion
const matrix::Quatf & get_quaternion() const
Definition: estimator_interface.h:287
ekf_gps_drift_s::vpos_drift_rate
float vpos_drift_rate
Definition: ekf_gps_drift.h:55
estimator::gps_message::lon
int32_t lon
Longitude in 1E-7 degrees.
Definition: common.h:60
estimator::ext_vision_message::posErr
float posErr
1-Sigma horizontal position accuracy (m)
Definition: common.h:86
ekf_gps_position_s::alt
int32_t alt
Definition: ekf_gps_position.h:56
Ekf2::_status_sub
uORB::Subscription _status_sub
Definition: ekf2_main.cpp:254
optical_flow_s::quality
uint8_t quality
Definition: optical_flow.h:68
ekf_gps_drift_s
Definition: ekf_gps_drift.h:51
EstimatorInterface::get_position
void get_position(float *pos)
Definition: estimator_interface.h:318
Ekf2::_balt_sample_count
uint8_t _balt_sample_count
number of barometric altitude measurements summed
Definition: ekf2_main.cpp:195
perf_counter.h
Performance measuring tools.
_airspeed_sub
static int _airspeed_sub
Definition: messages.cpp:64
vehicle_local_position_s::vx
float vx
Definition: vehicle_local_position.h:62
Ekf2::_magnetometer_sub
uORB::Subscription _magnetometer_sub
Definition: ekf2_main.cpp:250
Ekf2::_vehicle_visual_odometry_aligned_pub
uORB::PublicationData< vehicle_odometry_s > _vehicle_visual_odometry_aligned_pub
Definition: ekf2_main.cpp:281
vehicle_odometry_s::x
float x
Definition: vehicle_odometry.h:69
EstimatorInterface::setOpticalFlowData
void setOpticalFlowData(uint64_t time_usec, flow_message *flow)
Definition: estimator_interface.cpp:347