estimator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "estimator.h"
33 
34 #include "rosflight.h"
35 
36 namespace rosflight_firmware
37 {
38 Estimator::Estimator(ROSflight& _rf) : RF_(_rf) {}
39 
41 {
42  state_.attitude.w = 1.0f;
43  state_.attitude.x = 0.0f;
44  state_.attitude.y = 0.0f;
45  state_.attitude.z = 0.0f;
46 
47  state_.angular_velocity.x = 0.0f;
48  state_.angular_velocity.y = 0.0f;
49  state_.angular_velocity.z = 0.0f;
50 
51  state_.roll = 0.0f;
52  state_.pitch = 0.0f;
53  state_.yaw = 0.0f;
54 
55  w1_.x = 0.0f;
56  w1_.y = 0.0f;
57  w1_.z = 0.0f;
58 
59  w2_.x = 0.0f;
60  w2_.y = 0.0f;
61  w2_.z = 0.0f;
62 
63  bias_.x = 0.0f;
64  bias_.y = 0.0f;
65  bias_.z = 0.0f;
66 
67  accel_LPF_.x = 0;
68  accel_LPF_.y = 0;
69  accel_LPF_.z = -9.80665;
70 
71  gyro_LPF_.x = 0;
72  gyro_LPF_.y = 0;
73  gyro_LPF_.z = 0;
74 
76 
78 
79  // Clear the unhealthy estimator flag
81 }
82 
84 {
85  bias_.x = 0;
86  bias_.y = 0;
87  bias_.z = 0;
88 }
89 
91 {
92  last_time_ = 0;
95  reset_state();
96 }
97 
98 void Estimator::param_change_callback(uint16_t param_id)
99 {
100  (void)param_id;
101 }
102 
104 {
105  float alpha_acc = RF_.params_.get_param_float(PARAM_ACC_ALPHA);
106  const turbomath::Vector& raw_accel = RF_.sensors_.data().accel;
107  accel_LPF_.x = (1.0f - alpha_acc) * raw_accel.x + alpha_acc * accel_LPF_.x;
108  accel_LPF_.y = (1.0f - alpha_acc) * raw_accel.y + alpha_acc * accel_LPF_.y;
109  accel_LPF_.z = (1.0f - alpha_acc) * raw_accel.z + alpha_acc * accel_LPF_.z;
110 
111  float alpha_gyro_xy = RF_.params_.get_param_float(PARAM_GYRO_XY_ALPHA);
112  float alpha_gyro_z = RF_.params_.get_param_float(PARAM_GYRO_Z_ALPHA);
113  const turbomath::Vector& raw_gyro = RF_.sensors_.data().gyro;
114  gyro_LPF_.x = (1.0f - alpha_gyro_xy) * raw_gyro.x + alpha_gyro_xy * gyro_LPF_.x;
115  gyro_LPF_.y = (1.0f - alpha_gyro_xy) * raw_gyro.y + alpha_gyro_xy * gyro_LPF_.y;
116  gyro_LPF_.z = (1.0f - alpha_gyro_z) * raw_gyro.z + alpha_gyro_z * gyro_LPF_.z;
117 }
118 
120 {
122  q_extatt_ = q;
123 }
124 
126 {
127  //
128  // Timing Setup
129  //
130 
131  const uint64_t now_us = RF_.sensors_.data().imu_time;
132  if (last_time_ == 0)
133  {
134  last_time_ = now_us;
135  last_acc_update_us_ = now_us;
136  last_extatt_update_us_ = now_us;
137  return;
138  }
139  else if (now_us < last_time_)
140  {
141  // this shouldn't happen
143  last_time_ = now_us;
144  return;
145  }
146 
148 
149  float dt = (now_us - last_time_) * 1e-6f;
150  last_time_ = now_us;
151  state_.timestamp_us = now_us;
152 
153  // Low-pass filter accel and gyro measurements
154  run_LPF();
155 
156  //
157  // Gyro Correction Term (werr)
158  //
159 
160  float kp = 0.0f;
162 
163  turbomath::Vector w_err;
164 
165  if (can_use_accel())
166  {
167  // Get error estimated by accelerometer measurement
168  w_err = accel_correction();
170 
171  last_acc_update_us_ = now_us;
172  }
173 
174  if (can_use_extatt())
175  {
176  // Get error estimated by external attitude measurement. Overwrite any
177  // correction based on the accelerometer (assumption: extatt is better).
178  w_err = extatt_correction();
180 
181  // the angular rate correction from external attitude updates occur at a
182  // different rate than IMU updates, so it needs to be integrated with a
183  // different dt. The following scales the correction term by the timestep
184  // ratio so that it is integrated correctly.
185  const float extAttDt = (now_us - last_extatt_update_us_) * 1e-6f;
186  const float scaleDt = (dt > 0) ? (extAttDt / dt) : 0.0f;
187  w_err *= scaleDt;
188 
189  last_extatt_update_us_ = now_us;
190  extatt_update_next_run_ = false;
191  }
192 
193  // Crank up the gains for the first few seconds for quick convergence
194  if (now_us < static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000)
195  {
198  }
199 
200  //
201  // Composite Bias-Free Angular Rate (wfinal)
202  //
203 
204  // Integrate biases driven by measured angular error
205  // eq 47b Mahony Paper, using correction term w_err found above
206  bias_ -= ki * w_err * dt;
207 
208  // Build the composite omega vector for kinematic propagation
209  // This the stuff inside the p function in eq. 47a - Mahony Paper
211  turbomath::Vector wfinal = wbar - bias_ + kp * w_err;
212 
213  //
214  // Propagate Dynamics
215  //
216 
218 
219  //
220  // Post-Processing
221  //
222 
223  // Extract Euler Angles for controller
225 
226  // Save off adjust gyro measurements with estimated biases for control
228 
229  // If it has been more than 0.5 seconds since the accel update ran and we
230  // are supposed to be getting them then trigger an unhealthy estimator error.
233  {
235  }
236  else
237  {
239  }
240 }
241 
243 {
244  // if we are not using accel, just bail
246  return false;
247 
248  // current magnitude of LPF'd accelerometer
249  const float a_sqrd_norm = accel_LPF_.sqrd_norm();
250 
251  // Ideally, gyros would never drift and we would never have to use the accelerometer.
252  // Since gyros do drift, we can use the accelerometer (in a non-accelerated state) as
253  // another estimate of roll/pitch angles and to make gyro biases observable (except r).
254  // Since there is noise, we give some margin to what a "non-accelerated state" means.
255  // Establish allowed acceleration deviation from 1g (i.e., non-accelerated flight).
257  const float lowerbound = (1.0f - margin) * (1.0f - margin) * 9.80665f * 9.80665f;
258  const float upperbound = (1.0f + margin) * (1.0f + margin) * 9.80665f * 9.80665f;
259 
260  // if the magnitude of the accel measurement is close to 1g, we can use the
261  // accelerometer to correct roll and pitch and estimate gyro biases.
262  return (lowerbound < a_sqrd_norm && a_sqrd_norm < upperbound);
263 }
264 
266 {
268 }
269 
271 {
272  // turn measurement into a unit vector
274 
275  // Get the quaternion from accelerometer (low-frequency measure q)
276  // (Not in either paper)
277  turbomath::Quaternion q_acc_inv(g_, a);
278 
279  // Get the error quaternion between observer and low-freq q
280  // Below Eq. 45 Mahony Paper
281  turbomath::Quaternion q_tilde = q_acc_inv * state_.attitude;
282 
283  // Correction Term of Eq. 47a and 47b Mahony Paper
284  // w_acc = 2*s_tilde*v_tilde
285  turbomath::Vector w_acc;
286  w_acc.x = -2.0f * q_tilde.w * q_tilde.x;
287  w_acc.y = -2.0f * q_tilde.w * q_tilde.y;
288  w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer
289 
290  return w_acc;
291 }
292 
294 {
295  // DCM rows of attitude estimate and external measurement (world w.r.t body).
296  // These are the world axes from the perspective of the body frame.
297  // Note: If we extracted cols it would be body w.r.t world.
298  turbomath::Vector xhat_BW, yhat_BW, zhat_BW;
299  turbomath::Vector xext_BW, yext_BW, zext_BW;
300 
301  // extract rows of rotation matrix from quaternion attitude estimate
302  quaternion_to_dcm(state_.attitude, xhat_BW, yhat_BW, zhat_BW);
303 
304  // extract rows of rotation matrix from quaternion external attitude
305  quaternion_to_dcm(q_extatt_, xext_BW, yext_BW, zext_BW);
306 
307  // calculate cross products of corresponding axes as an error metric. For example, if vehicle were
308  // level but extatt had a different yaw angle than the internal estimate, xext_BW.cross(xhat_BW)
309  // would be a measure of how the filter needs to update in order to the internal estimate's yaw
310  // to closer to the extatt measurement. This is done for each axis.
311  turbomath::Vector w_ext = xext_BW.cross(xhat_BW) + yext_BW.cross(yhat_BW) + zext_BW.cross(zhat_BW);
312 
313  return w_ext;
314 }
315 
317 {
318  turbomath::Vector wbar;
320  {
321  // Quadratic Interpolation (Eq. 14 Casey Paper)
322  // this step adds 12 us on the STM32F10x chips
323  wbar = (w2_ / -12.0f) + w1_ * (8.0f / 12.0f) + gyro_LPF_ * (5.0f / 12.0f);
324  w2_ = w1_;
325  w1_ = gyro_LPF_;
326  }
327  else
328  {
329  wbar = gyro_LPF_;
330  }
331 
332  return wbar;
333 }
334 
336  const turbomath::Vector& omega,
337  const float dt) const
338 {
339  // only propagate if we've moved
340  // TODO[PCL]: Will this ever be true? We should add a margin to this
341  const float sqrd_norm_w = omega.sqrd_norm();
342  if (sqrd_norm_w == 0.0f)
343  return;
344 
345  // for convenience
346  const float &p = omega.x, &q = omega.y, &r = omega.z;
347 
349  {
350  // Matrix Exponential Approximation (From Attitude Representation and Kinematic
351  // Propagation for Low-Cost UAVs by Robert T. Casey)
352  // (Eq. 12 Casey Paper)
353  // This adds 90 us on STM32F10x chips
354  float norm_w = sqrtf(sqrd_norm_w);
355  float t1 = cosf((norm_w * dt) / 2.0f);
356  float t2 = 1.0f / norm_w * sinf((norm_w * dt) / 2.0f);
357  quat.w = t1 * quat.w + t2 * (-p * quat.x - q * quat.y - r * quat.z);
358  quat.x = t1 * quat.x + t2 * (p * quat.w + r * quat.y - q * quat.z);
359  quat.y = t1 * quat.y + t2 * (q * quat.w - r * quat.x + p * quat.z);
360  quat.z = t1 * quat.z + t2 * (r * quat.w + q * quat.x - p * quat.y);
361  quat.normalize();
362  }
363  else
364  {
365  // Euler Integration
366  // (Eq. 47a Mahony Paper)
368  0.5f * (-p * quat.x - q * quat.y - r * quat.z), 0.5f * (p * quat.w + r * quat.y - q * quat.z),
369  0.5f * (q * quat.w - r * quat.x + p * quat.z), 0.5f * (r * quat.w + q * quat.x - p * quat.y));
370  quat.w += qdot.w * dt;
371  quat.x += qdot.x * dt;
372  quat.y += qdot.y * dt;
373  quat.z += qdot.z * dt;
374  quat.normalize();
375  }
376 }
377 
381  turbomath::Vector& Z) const
382 {
383  // R(q) = [X.x X.y X.z]
384  // [Y.x Y.y Y.z]
385  // [Z.x Z.y Z.z]
386 
387  const float &w = q.w, &x = q.x, &y = q.y, &z = q.z;
388  X.x = 1.0f - 2.0f * (y * y + z * z);
389  X.y = 2.0f * (x * y - z * w);
390  X.z = 2.0f * (x * z + y * w);
391  Y.x = 2.0f * (x * y + z * w);
392  Y.y = 1.0f - 2.0f * (x * x + z * z);
393  Y.z = 2.0f * (y * z - x * w);
394  Z.x = 2.0f * (x * z - y * w);
395  Z.y = 2.0f * (y * z + x * w);
396  Z.z = 1.0f - 2.0f * (x * x + y * y);
397 }
398 
399 } // namespace rosflight_firmware
void quaternion_to_dcm(const turbomath::Quaternion &q, turbomath::Vector &X, turbomath::Vector &Y, turbomath::Vector &Z) const
Definition: estimator.cpp:378
void integrate_angular_rate(turbomath::Quaternion &quat, const turbomath::Vector &omega, const float dt) const
Definition: estimator.cpp:335
turbomath::Vector bias_
Definition: estimator.h:90
Vector cross(const Vector &v) const
Definition: turbomath.cpp:125
turbomath::Vector gyro_LPF_
Definition: estimator.h:93
turbomath::Vector accel_LPF_
Definition: estimator.h:92
turbomath::Vector smoothed_gyro_measurement()
Definition: estimator.cpp:316
float sqrd_norm() const
Definition: turbomath.cpp:47
void param_change_callback(uint16_t param_id) override
Definition: estimator.cpp:98
turbomath::Vector accel_correction() const
Definition: estimator.cpp:270
turbomath::Vector extatt_correction() const
Definition: estimator.cpp:293
void get_RPY(float *roll, float *pitch, float *yaw) const
Definition: turbomath.cpp:263
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
Vector normalized() const
Definition: turbomath.cpp:61
void set_external_attitude_update(const turbomath::Quaternion &q)
Definition: estimator.cpp:119
virtual uint64_t clock_micros()=0
turbomath::Quaternion q_extatt_
Definition: estimator.h:98
turbomath::Vector w2_
Definition: estimator.h:88
void set_error(uint16_t error)
void clear_error(uint16_t error)
turbomath::Quaternion attitude
Definition: estimator.h:53
Quaternion & normalize()
Definition: turbomath.cpp:144
turbomath::Vector angular_velocity
Definition: estimator.h:52
turbomath::Vector w1_
Definition: estimator.h:87
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:319
const Data & data() const
Definition: sensors.h:167
const turbomath::Vector g_
Definition: estimator.h:78


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Fri Oct 9 2020 03:17:15