The material in this section is based on the work of Thrun et al. in their book Proba- bilistic Robotics [111]. The problem of state estimation can generally be described as estimating some extrinsic properties of a system given (possibly noisy) measurements from multiple sources. In the first part of this chapter we looked at a some specific state estimation problems where vision was the only available sensor. However on typical robot systems this it not the case as they often feature multiple sensors. In most cases each sensor will have their own strengths and weaknesses, for example a gyro sensor is very good at measuring rotational velocity but is prone to drift over time. To address
Figure 2.14: Graphs corresponding to the different weighted least squares cost func- tions.
this a common strategy is to combine sensors to mitigate these weaknesses for example adding an accelerometer (which measures acceleration) can be used to correct for gyro drift. Another example is monocular Visual SLAM, using the techniques discussed in this chapter one can estimate the position of a camera within it’s environment but only up to some unknown scale factor. However combining the position estimates from a Vi- sual SLAM system with sensor readings from an accelerometer and gyro (which provide readings in metric units) we can estimate the metric scale factor for the Visual SLAM system. This approach is commonly referred to as sensor fusion.
The Kalman filter[50] is a Gaussian, recursive state estimator derived from the more general case Baysian filter. The state, in a Kalman filter, is modelled using a multivariate normal distribution p(x):
p(x) = det(2πΣ)−12exp −1
2(x − µ)
Tσ−1(x − µ)} (2.38)
where the state vector x is characterised by the mean µ and covariance Σ of a Gaus- sian probability density function. The Kalman Filter is described as a recursive state estimator for linear systems, meaning both the measurement and process models must be linear functions. The state vector defines the set of variables to be estimated for
example the position and velocity of a MAV:
x = (x, y, z, ˙x, ˙y, ˙z)T
The Kalman filter process can be divided into two steps, the prediction step which a motion or control model is used to predict the state of the system on time step into the future and the correction/update step where sensor data is incorporated into the state to further refine the prediction.
The prediction step defines how the state evolves from one moment to the next as a result of the process model and the control model. The process model describes how the state changes from one moment to the next regardless of the control command. For example if the state of the system is affected by an external force such as in the case of a MAV operating in the wind, we can account for this in the process model. The control model describes how the state evolves as the direct result of a control command. The input command utis a vector of control commands executed at time t for example if we
commanded our MAV to accelerate forwards with an acceleration of 1.0 m/s2: u = (0, 0, 0, 1.0, 0, 0)T
we can then generate a prediction of the state in the next moment in time: ¯
µt= At µt−1+ Bt ut (2.39)
¯
Σt= At Σt−1 ATt + Rt (2.40)
where At is a matrix describing the process model, Bt describes the control model and
Rtmodels the uncertainty introduced by the prediction step, represented by a zero mean
Gaussian.
The correction step, incorporates measurements or observations of the environment into the prediction. This is done in two parts, first we calculate the Kalman gain. Intuitively this can be described as the extent to which we trust our measurements over our prediction of the state. That is if the uncertainty of our measurements is lower than the uncertainty of our state predictions then the correction will be biased towards the measurements rather than the predicted state. The Kalman gain Kt is computed by:
Kt= ¯ΣtCtT(Ct Σ¯t CtT + Qt)−1 (2.41)
where: (i) Ct is the measurement model which relates the current state ut to the mea-
surement ztand (ii) Qtis the covariance of the zero-mean Gaussian measurement noise.
In the next part we compute the innovation which is the difference between the actual measurement and the expected measurement calculated from the measurement model Ct
gain and the innovation:
µt= ¯ut+ Kt(zt− Ct µ¯t) (2.42)
Finally the correction step completes with an update of the covariance, based on the incorporation of the new information contained in the measurement:
Σt= (I − Kt Ct) ¯Σt (2.43)
The complete Kalman filter algorithm is as follows: Algorithm 2 Kalman Filter(µt−1, Σt−1, ut, zt)
Input: The previous mean µt−1, covariance Σt−1and current control utand current
measurement zt
Output: The new mean µt and covariance Σt
1: µ¯t= At µt−1+ Bt ut 2: Σ¯t= At Σt−1 ATt + Rt 3: Kt= ¯ΣtCtT(Ct Σ¯t CtT + Qt)−1 4: µt= ¯ut+ Kt(zt− Ct µ¯t) 5: Σt= (I − KtCt) ¯Σt 6: return µt, Σt
2.5.1 Extension to non-linear systems
A key insight into the workings of the Kalman filter is the fact that any linear transfor- mation of a Gaussian results in another Gaussian [111]. Therefore, provided our system requires only linear transformations, we can ensure our state represented by a multivari- ate Gaussian distribution remains a Gaussian. However a large proportion of the state estimation problems in robotics involve non-linear functions. The Extended Kalman Filter (EKF) relaxes the linear transformation requirement, specifically we replace the process and control matrices A and B with a non-linear function g and the measure- ment model C is replaced with another non-linear function h. Additionally as there is no closed-form solution for non-linear functions it is no longer possible to calculate the exact mean and covariance of the state instead we calculate an approximation based on the first order linear approximation of the functions g and h using a Taylor series expansion.
The Taylor approximation of the function g is based on the value and slope of g0, where the slope is the partial derivative of g with respect to the current control command ut and the current mean µt:
g0(ut, xt−1) =
∂(ut, µt−1)
∂µt−1
also known as the Jacobian of the function g. An important note as the Jacobian depends on ut and µt−1 and therefore must be recomputed each time. The state prediction step
(equation 2.39 then becomes:
¯
µt= g(ut, µt−1) (2.44)
and the covariance prediction step (equation 2.40) becomes: ¯
Σt= Gt Σt−1 GTt + Rt (2.45)
Where Gtis the Jacobian matrix consisting of all the partial derivatives of g with respect
to ut and µt−1. A similar linear approximation is used for the measurement function h.
Here the linear approximation is based on the current state prediction ¯µt. The prediction
step is then altered to take this into account:
Kt= ¯ΣtHtT(Ht Σ¯t HtT + Qt)−1 (2.46)
µt= ¯ut+ Kt(zt− h(¯µt)) (2.47)
Σt= (I − Kt Ht) ¯Σt (2.48)
where H is the Jacobian matrix consisting of all the partial derivatives of h with respect to ¯µt. The full EKF algorithm is then given as follows: This allows us to apply the
Algorithm 3 ExtendedKalmanFilter(µt−1, Σt−1, ut, zt)
Input: The previous mean µt−1, covariance Σt−1and current control utand current
measurement zt
Output: The new mean µt and covariance Σt
1: µ¯t= g(ut, µt−1) 2: Σ¯t= GtΣt−1 GTt + Rt 3: Kt= ¯ΣtHtT(Ht Σ¯t HtT + Qt)−1 4: µt= ¯ut+ Kt(zt− h(¯µt)) 5: Σt= (I − KtHt) ¯Σt 6: return µt, Σt
Kalman filter algorithm to non-linear process and observation models at the expense of having to compute the Jacobians at each iteration. The drawback of the EKF approach is that the first order Taylor approximation is not always guaranteed to be the best approximation, particularly when the uncertainty in the current estimate is high, this leads to a poor linear approximation and the state estimate may become unstable. An alternative approach is the Unscented Kalman Filter (UKF) [117] which applies a deterministic sampling technique to pick a set of sigma points around the mean. These sigma points are propagated through the non-linear functions from which the mean and covariance of the estimate can then be recovered. This more accurately represents the true mean and covariance.
F1 F2 F3 F4 Fgrav τ1 τ2 τ3 τ4
Figure 2.15: Quadcopter dynamics: each motor induces a torque Ti causing the
propellers to rotate at a certain speed. Each rotating propeller accelerates the air and induces a perpendicular force Ficounteracting the force Fgrav that pulls the quadcopter
towards the earth.