PX4 Firmware
PX4 Autopilot Software http://px4.io
test_EKF_basics.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 ECL 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 #include <gtest/gtest.h>
35 #include <math.h>
36 #include "EKF/ekf.h"
37 
38 class EkfInitializationTest : public ::testing::Test {
39  public:
40 
41  Ekf _ekf{};
42 
43  // Basics sensors
44  const uint32_t _imu_dt_us{4000}; // 250 Hz Period between IMU updates
45  const uint32_t _baro_dt_us{12500}; // 80 Hz Period between barometer updates
46  const uint32_t _mag_dt_us{12500}; // 80 Hz Period between magnetometer updates
47  const uint32_t _gps_dt_us{200000}; // 5 Hz Period between GPS updates
48 
49  // Flags that control if a sensor is fused
50  bool _fuse_imu{true};
51  bool _fuse_baro{true};
52  bool _fuse_mag{true};
53  bool _fuse_gps{false}; // GPS measurements are expected to not come in from beginning
54 
55  // GPS message
57 
58  uint32_t _update_dt_us{}; // greatest common divider of all basic sensor periods
59  const uint32_t _init_duration_us{2000000}; // 2s Duration of
60 
61  // counter of how many sensor measurement are put into Ekf
62  uint32_t _counter_imu{0};
63  uint32_t _counter_baro{0};
64  uint32_t _counter_mag{0};
65 
66  uint32_t _t_us{0};
67 
68  // Setup the Ekf with synthetic measurements
69  void SetUp() override
70  {
71 
72  _ekf.init(0);
73 
74  // setup gps message to reasonable default values
76  _gps_message.lat = 473566094;
77  _gps_message.lon = 85190237;
78  _gps_message.alt = 422056;
79  _gps_message.yaw = 0.0f;
80  _gps_message.yaw_offset = 0.0f;
82  _gps_message.eph = 0.5f;
83  _gps_message.epv = 0.8f;
84  _gps_message.sacc = 0.2f;
85  _gps_message.vel_m_s = 0.0;
86  _gps_message.vel_ned[0] = 0.0f;
87  _gps_message.vel_ned[1] = 0.0f;
88  _gps_message.vel_ned[2] = 0.0f;
90  _gps_message.nsats = 16;
91  _gps_message.pdop = 0.0f;
92 
94 
95  // output how many sensor measurement were put into the EKF
96  // std::cout << "Initialized EKF with:" << std::endl;
97  // std::cout << "update_dt_us: " << _update_dt_us << std::endl;
98  // std::cout << "counter_imu: " << _counter_imu << std::endl
99  // << "counter_baro: " << _counter_baro << std::endl
100  // << "counter_mag: " << _counter_mag << std::endl;
101  }
102 
103  void update_with_const_sensors(uint32_t duration_us,
104  Vector3f ang_vel = Vector3f{0.0f,0.0f,0.0f},
105  Vector3f accel = Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
106  Vector3f mag_data = Vector3f{0.2f, 0.0f, 0.4f},
107  float baro_data = 122.2f)
108  {
109  // store start time
110  uint32_t start_time_us = _t_us;
111 
112  // compute update time step such that we can update the basic sensor at different rates
113  _update_dt_us = std::__gcd(_imu_dt_us,std::__gcd(_mag_dt_us,std::__gcd(_baro_dt_us,_gps_dt_us)));
114 
115  // update EKF with synthetic sensor measurements
116  for( ; _t_us < start_time_us+duration_us; _t_us += _update_dt_us)
117  {
118  // Check which sensors update we should do
119  if(_fuse_imu && !(_t_us %_imu_dt_us))
120  {
121  // push imu data into estimator
122  imuSample imu_sample_new;
123  imu_sample_new.time_us = _t_us;
124  imu_sample_new.delta_ang_dt = _imu_dt_us * 1.e-6f;
125  imu_sample_new.delta_ang = ang_vel * imu_sample_new.delta_ang_dt;
126  imu_sample_new.delta_vel_dt = _imu_dt_us * 1.e-6f;
127  imu_sample_new.delta_vel = accel * imu_sample_new.delta_vel_dt;
128 
129  _ekf.setIMUData(imu_sample_new);
130  _counter_imu++;
131  }
132  if(_fuse_baro && !(_t_us % _baro_dt_us))
133  {
134  _ekf.setBaroData(_t_us,baro_data);
135  _counter_baro++;
136  }
137  if(_fuse_mag && !(_t_us % _mag_dt_us))
138  {
139  float mag[3];
140  mag_data.copyTo(mag);
141  _ekf.setMagData(_t_us,mag);
142  _counter_mag++;
143  }
144  if(_fuse_gps && !(_t_us % _gps_dt_us))
145  {
148  _counter_mag++;
149  }
150 
151  _ekf.update();
152  }
153  }
154 
155  // Use this method to clean up any memory, network etc. after each test
156  void TearDown() override
157  {
158 
159  }
160 };
161 
162 
164 {
165  // GIVEN: reasonable static sensor data for some duration
166  // THEN: EKF should tilt align
167  EXPECT_EQ(true,_ekf.attitude_valid());
168 }
169 
170 TEST_F(EkfInitializationTest, initialControlMode)
171 {
172  // GIVEN: reasonable static sensor data for some duration
173  // THEN: EKF control status should be reasonable
174  filter_control_status_u control_status;
175  _ekf.get_control_mode(&control_status.value);
176 
177  EXPECT_EQ(1, (int) control_status.flags.tilt_align);
178  EXPECT_EQ(1, (int) control_status.flags.yaw_align);
179  EXPECT_EQ(0, (int) control_status.flags.gps);
180  EXPECT_EQ(0, (int) control_status.flags.opt_flow);
181  EXPECT_EQ(1, (int) control_status.flags.mag_hdg);
182  EXPECT_EQ(0, (int) control_status.flags.mag_3D);
183  EXPECT_EQ(0, (int) control_status.flags.mag_dec);
184  EXPECT_EQ(0, (int) control_status.flags.in_air);
185  EXPECT_EQ(0, (int) control_status.flags.wind);
186  EXPECT_EQ(1, (int) control_status.flags.baro_hgt);
187  EXPECT_EQ(0, (int) control_status.flags.rng_hgt);
188  EXPECT_EQ(0, (int) control_status.flags.gps_hgt);
189  EXPECT_EQ(0, (int) control_status.flags.ev_pos);
190  EXPECT_EQ(0, (int) control_status.flags.ev_yaw);
191  EXPECT_EQ(0, (int) control_status.flags.ev_hgt);
192  EXPECT_EQ(0, (int) control_status.flags.fuse_beta);
193  EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed);
194  EXPECT_EQ(0, (int) control_status.flags.fixed_wing);
195  EXPECT_EQ(0, (int) control_status.flags.mag_fault);
196  EXPECT_EQ(0, (int) control_status.flags.gnd_effect);
197  EXPECT_EQ(0, (int) control_status.flags.rng_stuck);
198  EXPECT_EQ(0, (int) control_status.flags.gps_yaw);
199  EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight);
200  EXPECT_EQ(0, (int) control_status.flags.ev_vel);
201  EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
202 }
203 
204 TEST_F(EkfInitializationTest, convergesToZero)
205 {
206  // GIVEN: initialized EKF with default IMU, baro and mag input for 2s
207  // WHEN: Added more defautl sensor measurements
208  update_with_const_sensors(4000000); // for further 4s
209 
210  float converged_pos[3];
211  float converged_vel[3];
212  float converged_accel_bias[3];
213  float converged_gyro_bias[3];
214  _ekf.get_position(converged_pos);
215  _ekf.get_velocity(converged_vel);
216  _ekf.get_accel_bias(converged_accel_bias);
217  _ekf.get_gyro_bias(converged_gyro_bias);
218 
219  // THEN: EKF should stay or converge to zero
220  for(int i=0; i<3; ++i)
221  {
222  EXPECT_NEAR(0.0f,converged_pos[i],0.001f);
223  EXPECT_NEAR(0.0f,converged_vel[i],0.001f);
224  EXPECT_NEAR(0.0f,converged_accel_bias[i],0.001f);
225  EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f);
226  }
227 }
228 
230 {
231  // GIVEN: initialized EKF with default IMU, baro and mag input for 2s
232  // WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec
233 
234  _fuse_gps = true;
235  update_with_const_sensors(11000000,Vector3f{0.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}); // for further 3s
236 
237  // THEN: EKF should fuse GPS, but no other position sensor
238  filter_control_status_u control_status;
239  _ekf.get_control_mode(&control_status.value);
240  EXPECT_EQ(1, (int) control_status.flags.tilt_align);
241  EXPECT_EQ(1, (int) control_status.flags.yaw_align);
242  EXPECT_EQ(1, (int) control_status.flags.gps);
243  EXPECT_EQ(0, (int) control_status.flags.opt_flow);
244  EXPECT_EQ(1, (int) control_status.flags.mag_hdg);
245  EXPECT_EQ(0, (int) control_status.flags.mag_3D);
246  EXPECT_EQ(0, (int) control_status.flags.mag_dec);
247  EXPECT_EQ(0, (int) control_status.flags.in_air);
248  EXPECT_EQ(0, (int) control_status.flags.wind);
249  EXPECT_EQ(1, (int) control_status.flags.baro_hgt);
250  EXPECT_EQ(0, (int) control_status.flags.rng_hgt);
251  EXPECT_EQ(0, (int) control_status.flags.gps_hgt);
252  EXPECT_EQ(0, (int) control_status.flags.ev_pos);
253  EXPECT_EQ(0, (int) control_status.flags.ev_yaw);
254  EXPECT_EQ(0, (int) control_status.flags.ev_hgt);
255  EXPECT_EQ(0, (int) control_status.flags.fuse_beta);
256  EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed);
257  EXPECT_EQ(0, (int) control_status.flags.fixed_wing);
258  EXPECT_EQ(0, (int) control_status.flags.mag_fault);
259  EXPECT_EQ(0, (int) control_status.flags.gnd_effect);
260  EXPECT_EQ(0, (int) control_status.flags.rng_stuck);
261  EXPECT_EQ(0, (int) control_status.flags.gps_yaw);
262  EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight);
263  EXPECT_EQ(0, (int) control_status.flags.ev_vel);
264  EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
265 }
266 
267 TEST_F(EkfInitializationTest, accleBiasEstimation)
268 {
269  // GIVEN: initialized EKF with default IMU, baro and mag input for 2s
270  // WHEN: Added more sensor measurements with accel bias and gps measurements
271  Vector3f accel_bias = {0.0f,0.0f,0.1f};
272 
273  _fuse_gps = true;
274  update_with_const_sensors(10000000,Vector3f{0.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}+accel_bias); // for further 10s
275 
276  float converged_pos[3];
277  float converged_vel[3];
278  float converged_accel_bias[3];
279  float converged_gyro_bias[3];
280  _ekf.get_position(converged_pos);
281  _ekf.get_velocity(converged_vel);
282  _ekf.get_accel_bias(converged_accel_bias);
283  _ekf.get_gyro_bias(converged_gyro_bias);
284 
285  // THEN: EKF should estimate bias correctelly
286  for(int i=0; i<3; ++i)
287  {
288  EXPECT_NEAR(0.0f,converged_pos[i],0.001f) << "i: " << i;
289  EXPECT_NEAR(0.0f,converged_vel[i],0.001f) << "i: " << i;
290  EXPECT_NEAR(accel_bias(i),converged_accel_bias[i],0.001f) << "i: " << i;
291  EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f) << "i: " << i;
292  }
293 }
294 
295 // TODO: Add sampling tests
const uint32_t _imu_dt_us
void setIMUData(const imuSample &imu_sample)
const uint32_t _baro_dt_us
void setMagData(uint64_t time_usec, float(&data)[3])
struct estimator::filter_control_status_u::@60 flags
float eph
GPS horizontal position accuracy in m.
Definition: common.h:65
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
Definition: common.h:107
float yaw_offset
Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET.
Definition: common.h:63
uint32_t gps_yaw
22 - true when yaw (not ground course) data from a GPS receiver is being fused
Definition: common.h:463
float epv
GPS vertical position accuracy in m.
Definition: common.h:66
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
Definition: common.h:442
uint8_t nsats
number of satellites used
Definition: common.h:71
uint64_t time_usec
Definition: common.h:58
float vel_m_s
GPS ground speed (m/sec)
Definition: common.h:68
const uint32_t _gps_dt_us
uint32_t fixed_wing
17 - true when the vehicle is operating as a fixed wing vehicle
Definition: common.h:458
void get_gyro_bias(float bias[3]) override
Definition: ekf_helper.cpp:948
uint32_t mag_hdg
4 - true if a simple magnetic yaw heading is being fused
Definition: common.h:445
uint32_t mag_fault
18 - true when the magnetometer has been declared faulty and is no longer being used ...
Definition: common.h:459
float sacc
GPS speed accuracy in m/s.
Definition: common.h:67
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
Definition: common.h:451
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
uint32_t mag_field_disturbed
16 - true when the mag field does not match the expected strength
Definition: common.h:457
void update_with_const_sensors(uint32_t duration_us, Vector3f ang_vel=Vector3f{0.0f, 0.0f, 0.0f}, Vector3f accel=Vector3f{0.0f, 0.0f,-CONSTANTS_ONE_G}, Vector3f mag_data=Vector3f{0.2f, 0.0f, 0.4f}, float baro_data=122.2f)
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
Definition: common.h:450
const uint32_t _mag_dt_us
float pdop
position dilution of precision
Definition: common.h:72
void get_velocity(float *vel)
void setGpsData(uint64_t time_usec, const gps_message &gps)
float vel_ned[3]
GPS ground speed NED.
Definition: common.h:69
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
void get_control_mode(uint32_t *val)
uint32_t synthetic_mag_z
25 - true when we are using a synthesized measurement for the magnetometer Z component ...
Definition: common.h:466
TEST_F(EkfInitializationTest, tiltAlign)
void get_accel_bias(float bias[3]) override
Definition: ekf_helper.cpp:938
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
Definition: common.h:465
uint32_t gnd_effect
20 - true when protection from ground effect induced static pressure rise is active ...
Definition: common.h:461
bool vel_ned_valid
GPS ground speed is valid.
Definition: common.h:70
void setBaroData(uint64_t time_usec, float data)
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:111
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
int32_t lat
Latitude in 1E-7 degrees.
Definition: common.h:59
void TearDown() override
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
Definition: common.h:455
uint32_t ev_yaw
13 - true when yaw data from external vision measurements is being fused
Definition: common.h:454
uint32_t mag_aligned_in_flight
23 - true when the in-flight mag field alignment has been completed
Definition: common.h:464
uint32_t mag_dec
6 - true if synthetic magnetic declination measurements are being fused
Definition: common.h:447
Vector3< float > Vector3f
Definition: Vector3.hpp:136
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:62
uint32_t opt_flow
3 - true if optical flow measurements are being fused
Definition: common.h:444
Definition: ekf.h:47
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Definition: common.h:446
uint32_t in_air
7 - true when the vehicle is airborne
Definition: common.h:448
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
Definition: common.h:61
uint32_t wind
8 - true when wind velocity is being estimated
Definition: common.h:449
uint32_t ev_pos
12 - true when local position data from external vision is being fused
Definition: common.h:453
uint32_t fuse_beta
15 - true when synthetic sideslip measurements are being fused
Definition: common.h:456
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
Definition: common.h:452
uint32_t rng_stuck
21 - true when rng data wasn&#39;t ready for more than 10s and new rng values haven&#39;t changed enough ...
Definition: common.h:462
uint32_t gps
2 - true if GPS measurements are being fused
Definition: common.h:443
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
Definition: common.h:441
bool init(uint64_t timestamp) override
Definition: ekf.cpp:47
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
Definition: common.h:108
const uint32_t _init_duration_us
float delta_ang_dt
delta angle integration period (sec)
Definition: common.h:109
Class for core functions for ekf attitude and position estimator.
bool update() override
Definition: ekf.cpp:117
float delta_vel_dt
delta velocity integration period (sec)
Definition: common.h:110
int32_t lon
Longitude in 1E-7 degrees.
Definition: common.h:60
void get_position(float *pos)