Estimation for Quadrotors

When performing estimation, the first thing one might think of is averaging the sensor measurements. We consider averaging for the 1D case, where the range sensor value, $z_{t}$ is directly observing the state (the height of the drone), $x_{t}$, both of which are scalars. The notation $\hat{x_{t}}$ means our estimate of the true state $x_{t}$. Then the average is:

This formulation allows the designer to tune the parameter $\alpha$, which weights how much to weight the old estimate compared to the new sensor value.

None of the above formulations update the estimate based on the drone’s movements. Intuitively, if we have told the drone to go up, for instance, then our belief about the drone’s position should also go up. The Bayes Filter gives us a way to incorporate motion prediction into our state estimate. First we predict the next state, given a control input, the current state, and a model of how the system evolves over time. We do not maintain a point estimate but rather a belief or distribution of the estimate. Then, we revise our prediction with an update from an observation. The update method takes the previous estimate from prediction, and an observed sensor value. It returns a new distribution that takes into account the sensor value, using a model of how the sensor works. Below you will see pseudocode for the prediction and update steps for the filter, following Thrun et al. 2005. In typical use one would call predict after determining the control input, and update after reading a sensor value. The BayesFilter function is illustrative only; in real life, one might call predict in the sensor callback for example.

The Kalman Filter and Extended Kalman Filter make the assumption that the distributions over belief state are Gaussian, represented as a mean and covariance matrix. Compare to the Bayes’ filter, the distributions $bel$ and $\bar{bel}$ are represented as a mean and covariance matrix. The KF assumes that the transition and observation models are linear, and can be defined by a matrix. The EKF is the extension to the nonlinear case, where we take use the Jacobian matrix of the transition and observation functions to compute a point-wise linear estimate, and then do the same updates as the Kalman Filter. We define the Extended Kalman Filter (EKF) algorithm following Thrun et al. 2005. We refactor it to include separate Predict and Update methods and to use our notation. We also unify the KF and EKF algorithm pseudocode. The transition or prediction covariance is $Q_{t}$; the measurement covariance is $R_{t}$. These matrices are often taken to be constant, but also sometimes people change them over time depending on the sensor model.

The EKF and KF are closely related. For the KF, the matrix $G_{t}$ is constant every iteration of the function and does not need to be recomputed each time (except for $\Delta t$). Another way to say it is the implementation of $g^{\prime}$ ignores its state input. Similarly, for the KF, the matrix $H_{t}$ is constant every iteration of the function and does not need to be recomputed. Another way to say it is the function $h^{\prime}$ ignores its state input. For the EKF, these matrices change each iteration, because it linearizes around the current state.

The Unscented Kalman Filter [14] is similar to the EKF in that it handles nonlinear transition models $g$ and measurement models $h$. However there are no Jacobians! Instead of linearizing around the current estimate, the Unscented Kalman Filter picks magic “sigma points” which are sample points chosen according to the current state estimate and covariance. Then these sigma points are passed through the nonlinear transition or observation function, and the sample mean and sample covariance of the sigma points is used to construct a new Gaussian $\mu$ and $\sigma$. The pseudocode here follows Kandepu et al. 2008 but removes the augmentation for tracking moving prediction and measurement covariance, and uses our notation.

Sigma points are computed using the matrix $S$ which is defined from the covariance matrix, $\Sigma_{t}$. $S_{i}$ denotes the $ith$ colum of the matrix $S$.

We define the sigma points $X_{i,t}\in X_{t}$ as follows:

Note that they are defined using the mean and covariance matrix of the distributions; by picking several representative points $S_{i}$ away from the mean, we can use a relatively small set of points to represent the entire distribution. The weights when computing the sample mean, $w_{i}^{m}$ are:

The weights when computing the sample covariance, $w_{i}^{c}$ are:

The parameters are defined as:

See Kandepu et al. 2008 for tuning suggestions when implementing the filter.

The pseudocode for the Unscented Kalman Filter is given in Algorithm 3. Compared to the Bayes’ filter, we use the computed sigma points to represent the distribution $\bar{bel}$. However we use a mean and covariance to represent the distribution $bel$ at the beginning and end of each iteration of the filter.

To implement the filters on specific vehicles, we need to define the state transition function, $g$ and the measurement model, $h$. We will do this three times for increasingly more realistic models of a quadrotor. First, we define a 1D quad model, where the quadroter is moving in $z$, but not in $x$ or $y$. It has one control input, the downward pointing thrust, and a noisy range sensor. The intention is that this is identical to the 1D quad used in the controls lesson. The state is then the position, $z$ and velocity, $\dot{z}$:

We define the control input as directly setting the accelleration, $\ddot{z}$:

We define a 2D quad model where the quadrotor is operating at a fixed height $z$. It has a range sensor pointing sideways, and it can move by rotating about its center, the angle $\phi$. Additionally it can move right and left in $y$. Then we define the state $x_{t}$ as the state transition function as follows:

We define the control input as directly setting the angle, $\phi$.

The state will be position and velocity which we will estimate with the GPS. We will use the magnetometer to estimate yaw. We will represent position and velocity in the global frame, and yaw that goes from body to local in the state. We would also then use a multirate Kalman Filter [4, 12] to perform updating, with separate updates for the GPS, the magnetometer, and the IMU (accelerometer plus rate gyro).

We track yaw as the heading from magnetic north. So it is the reading one would get if one reads from a compass.

Then $u_{t}$ is the acceleration in the body frame, where $\dot{\psi}$ is global frame yaw.

We have provided equations for filtering for a quadrotor helicopter, combining information and notation from Thrun et al. 2005, Quan 2017 and other sources.