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 #include "rosflight.h"
34 
35 namespace rosflight_firmware
36 {
37 
39  RF_(_rf)
40 {}
41 
43 {
44  state_.attitude.w = 1.0f;
45  state_.attitude.x = 0.0f;
46  state_.attitude.y = 0.0f;
47  state_.attitude.z = 0.0f;
48 
49  state_.angular_velocity.x = 0.0f;
50  state_.angular_velocity.y = 0.0f;
51  state_.angular_velocity.z = 0.0f;
52 
53  state_.roll = 0.0f;
54  state_.pitch = 0.0f;
55  state_.yaw = 0.0f;
56 
57  w1_.x = 0.0f;
58  w1_.y = 0.0f;
59  w1_.z = 0.0f;
60 
61  w2_.x = 0.0f;
62  w2_.y = 0.0f;
63  w2_.z = 0.0f;
64 
65  bias_.x = 0.0f;
66  bias_.y = 0.0f;
67  bias_.z = 0.0f;
68 
69  accel_LPF_.x = 0;
70  accel_LPF_.y = 0;
71  accel_LPF_.z = -9.80665;
72 
73  gyro_LPF_.x = 0;
74  gyro_LPF_.y = 0;
75  gyro_LPF_.z = 0;
76 
78 
80 
81  // Clear the unhealthy estimator flag
83 }
84 
86 {
87  bias_.x = 0;
88  bias_.y = 0;
89  bias_.z = 0;
90 }
91 
93 {
94  last_time_ = 0;
96  reset_state();
97 }
98 
99 void Estimator::param_change_callback(uint16_t param_id)
100 {
101  (void) param_id;
102 }
103 
105 {
106  float alpha_acc = RF_.params_.get_param_float(PARAM_ACC_ALPHA);
107  const turbomath::Vector &raw_accel = RF_.sensors_.data().accel;
108  accel_LPF_.x = (1.0f-alpha_acc)*raw_accel.x + alpha_acc*accel_LPF_.x;
109  accel_LPF_.y = (1.0f-alpha_acc)*raw_accel.y + alpha_acc*accel_LPF_.y;
110  accel_LPF_.z = (1.0f-alpha_acc)*raw_accel.z + alpha_acc*accel_LPF_.z;
111 
112  float alpha_gyro_xy = RF_.params_.get_param_float(PARAM_GYRO_XY_ALPHA);
113  float alpha_gyro_z = RF_.params_.get_param_float(PARAM_GYRO_Z_ALPHA);
114  const turbomath::Vector &raw_gyro = RF_.sensors_.data().gyro;
115  gyro_LPF_.x = (1.0f-alpha_gyro_xy)*raw_gyro.x + alpha_gyro_xy*gyro_LPF_.x;
116  gyro_LPF_.y = (1.0f-alpha_gyro_xy)*raw_gyro.y + alpha_gyro_xy*gyro_LPF_.y;
117  gyro_LPF_.z = (1.0f-alpha_gyro_z)*raw_gyro.z + alpha_gyro_z*gyro_LPF_.z;
118 }
119 
121 {
123  q_correction_ = q;
124 }
125 
127 {
128  float acc_kp, ki;
129  uint64_t now_us = RF_.sensors_.data().imu_time;
130  if (last_time_ == 0)
131  {
132  last_time_ = now_us;
134  return;
135  }
136  else if (now_us < last_time_)
137  {
138  // this shouldn't happen
140  last_time_ = now_us;
141  return;
142  }
143 
144 
146 
147  float dt = (now_us - last_time_) * 1e-6f;
148  last_time_ = now_us;
149  state_.timestamp_us = now_us;
150 
151  // Crank up the gains for the first few seconds for quick convergence
152  if (now_us < static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME))*1000)
153  {
154  acc_kp = RF_.params_.get_param_float(PARAM_FILTER_KP)*10.0f;
156  }
157  else
158  {
161  }
162 
163  // Run LPF to reject a lot of noise
164  run_LPF();
165 
166  // add in accelerometer
167  float a_sqrd_norm = accel_LPF_.sqrd_norm();
168 
169  turbomath::Vector w_acc;
171  && a_sqrd_norm < 1.1f*1.1f*9.80665f*9.80665f && a_sqrd_norm > 0.9f*0.9f*9.80665f*9.80665f)
172  {
173  // Get error estimated by accelerometer measurement
174  last_acc_update_us_ = now_us;
175  // turn measurement into a unit vector
177  // Get the quaternion from accelerometer (low-frequency measure q)
178  // (Not in either paper)
179  turbomath::Quaternion q_acc_inv(g_, a);
180  // Get the error quaternion between observer and low-freq q
181  // Below Eq. 45 Mahony Paper
182  turbomath::Quaternion q_tilde = q_acc_inv * state_.attitude;
183  // Correction Term of Eq. 47a and 47b Mahony Paper
184  // w_acc = 2*s_tilde*v_tilde
185  w_acc.x = -2.0f*q_tilde.w*q_tilde.x;
186  w_acc.y = -2.0f*q_tilde.w*q_tilde.y;
187  w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer
188 
189  // integrate biases from accelerometer feedback
190  // (eq 47b Mahony Paper, using correction term w_acc found above
191  bias_.x -= ki*w_acc.x*dt;
192  bias_.y -= ki*w_acc.y*dt;
193  bias_.z = 0.0; // Don't integrate z bias, because it's unobservable
194  }
195  else
196  {
197  w_acc.x = 0.0f;
198  w_acc.y = 0.0f;
199  w_acc.z = 0.0f;
200  }
201 
203  {
206  }
207 
208 
209  // Handle Gyro Measurements
210  turbomath::Vector wbar;
212  {
213  // Quadratic Interpolation (Eq. 14 Casey Paper)
214  // this step adds 12 us on the STM32F10x chips
215  wbar = (w2_/-12.0f) + w1_*(8.0f/12.0f) + gyro_LPF_ * (5.0f/12.0f);
216  w2_ = w1_;
217  w1_ = gyro_LPF_;
218  }
219  else
220  {
221  wbar = gyro_LPF_;
222  }
223 
224  // Build the composite omega vector for kinematic propagation
225  // This the stuff inside the p function in eq. 47a - Mahony Paper
226  turbomath::Vector wfinal = wbar - bias_ + w_acc * acc_kp;
227 
228  // Propagate Dynamics (only if we've moved)
229  float sqrd_norm_w = wfinal.sqrd_norm();
230  if (sqrd_norm_w > 0.0f)
231  {
232  float p = wfinal.x;
233  float q = wfinal.y;
234  float r = wfinal.z;
235 
237  {
238  // Matrix Exponential Approximation (From Attitude Representation and Kinematic
239  // Propagation for Low-Cost UAVs by Robert T. Casey)
240  // (Eq. 12 Casey Paper)
241  // This adds 90 us on STM32F10x chips
242  float norm_w = sqrtf(sqrd_norm_w);
243  turbomath::Quaternion qhat_np1;
244  float t1 = cosf((norm_w*dt)/2.0f);
245  float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f);
246  qhat_np1.w = t1*state_.attitude.w + t2*(-p*state_.attitude.x - q*state_.attitude.y - r*state_.attitude.z);
247  qhat_np1.x = t1*state_.attitude.x + t2*(p*state_.attitude.w + r*state_.attitude.y - q*state_.attitude.z);
248  qhat_np1.y = t1*state_.attitude.y + t2*(q*state_.attitude.w - r*state_.attitude.x + p*state_.attitude.z);
249  qhat_np1.z = t1*state_.attitude.z + t2*(r*state_.attitude.w + q*state_.attitude.x - p*state_.attitude.y);
250  state_.attitude = qhat_np1.normalize();
251  }
252  else
253  {
254  // Euler Integration
255  // (Eq. 47a Mahony Paper), but this is pretty straight-forward
257  0.5f * (p*state_.attitude.w + r*state_.attitude.y - q*state_.attitude.z),
258  0.5f * (q*state_.attitude.w - r*state_.attitude.x + p*state_.attitude.z),
259  0.5f * (r*state_.attitude.w + q*state_.attitude.x - p*state_.attitude.y));
260  state_.attitude.w += qdot.w*dt;
261  state_.attitude.x += qdot.x*dt;
262  state_.attitude.y += qdot.y*dt;
263  state_.attitude.z += qdot.z*dt;
265  }
266  }
267 
268  // Extract Euler Angles for controller
270 
271  // Save off adjust gyro measurements with estimated biases for control
273 
274  // If it has been more than 0.5 seconds since the acc update ran and we are supposed to be getting them
275  // then trigger an unhealthy estimator error
278  {
280  }
281  else
282  {
284  }
285 }
286 
287 } // namespace rosflight_firmware
turbomath::Vector bias_
Definition: estimator.h:100
turbomath::Vector gyro_LPF_
Definition: estimator.h:103
turbomath::Vector accel_LPF_
Definition: estimator.h:102
float sqrd_norm() const
Definition: turbomath.cpp:52
void param_change_callback(uint16_t param_id) override
Definition: estimator.cpp:99
void get_RPY(float *roll, float *pitch, float *yaw) const
Definition: turbomath.cpp:285
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:322
void set_attitude_correction(const turbomath::Quaternion &q)
Definition: estimator.cpp:120
Vector normalized() const
Definition: turbomath.cpp:68
virtual uint64_t clock_micros()=0
turbomath::Vector w2_
Definition: estimator.h:98
void set_error(uint16_t error)
void clear_error(uint16_t error)
turbomath::Quaternion attitude
Definition: estimator.h:55
Quaternion & normalize()
Definition: turbomath.cpp:165
turbomath::Vector angular_velocity
Definition: estimator.h:54
turbomath::Vector w1_
Definition: estimator.h:97
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:312
turbomath::Quaternion q_correction_
Definition: estimator.h:108
const Data & data() const
Definition: sensors.h:169
const turbomath::Vector g_
Definition: estimator.h:89


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18