Sensor Fusion

• overcomve limitations of individual sensors
• estimate quantities that are not directly measured

• describe the orientation by roll ($\phi$), pitch ($\theta$) and yaw ($\psi$) angles w.r.t a fixed coordinate system
• the measurement of gyroscopes (p, q, r) are not equal to ($\dot{\phi}$, $\dot{\theta}$, $\dot{\psi}$)

• measure accelerations of all axis: $a = [a_{x}, a_{y}, a_{z}]~ (m/s^{2})$
• Accelerameter model

• If the accelerometer is at rest, we can get $\phi$ and $\theta$ from:

  • $a_{x} = g * \sin(\theta)$
  • $a_{y} = -g * \sin(\phi) * \cos(\theta)$
  • $a_{z} = -g * \cos(\phi) * \cos(\theta)$

• and get:

  • $\hat{\phi}_{acc} = \tan^{-1}(\frac{a_y}{a_z})$
  • $\hat{\theta}_{acc} = \sin^{-1}(\frac{a_x}{g})$

• Practical considerations

  1. only close to true at rest
  2. noise term: HF noise and need low-pass filter to measurements
  3. Time-varying bias: how to estimate? initial calibration?

• Measures angular rate of rotation around each axis: $\omega_b = [p, q, r]~ (rad/s)$
• Gyroscope model

• Need to transfrom body rates to Eulaer rates:

  • $\dot{\phi} = p + \tan(\theta) * (q * \sin(\phi) + r * \cos(\phi))$
  • $\dot{\theta} = q * \cos(\phi) - r * \sin(\phi)$
  • $\dot{\psi} = \frac{q * \sin(\phi) + r * \cos(\phi)}{\cos(\theta)}$

• Practical considerations

  • fixed sampling time T
  • we need to know $\phi$ and $\theta$ to get $\dot{\phi}$ and $\dot{\theta}$
  • Can’t simply get them by integrating $\dot{\phi}$ and $\dot{\theta}$ from accelerometer measurements, due to time-varying bias, which leads to gyro drift
  • need low-pass filter and optional very low cutoff high-pass filter

• estimation error is defined by: $\tilde x = x - \hat x$ (true - eseimate)
• error covariance is defined by: $P = E[\tilde x * \tilde x^T]$

Basic idea: The Kalman filter choose the Kalman gain K, such that the error covariance matrix P is minimized