1 of 53

Chapter 8

State Estimation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 1

2 of 53

Architecture

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 2

3 of 53

Why estimate states?

State information needed to control aircraft
We measure many things

accelerations, angular rates, pressure altitude, dynamic pressure (airspeed), magnetic heading (sometimes), GPS position

Don’t have direct measurements of everything we need

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 3

4 of 53

Benchmark Maneuver

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 4

5 of 53

Inverting Sensor Measurement Model

Several states can be estimated by inverting sensor measurement model

Angular rates, altitude, airspeed
LPF denotes low pass filter

Mathematical Model

State Estimate

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 5

6 of 53

The alpha-filter

Suppose that your sensor gives noisy data as shown on the right.

The objective is to process this data to smooth out the noise.

The standard method is to use an alpha-filter:

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 6

7 of 53

The alpha-filter

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 7

8 of 53

The alpha-filter

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 8

9 of 53

The alpha-filter: a deeper look

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 9

10 of 53

The alpha-filter: a deeper look

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 10

11 of 53

The alpha-filter: �Python Implementation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 11

\begin{lstlisting}

class AlphaFilter:

# alpha filter implements a simple low pass filter

# y[k] = alpha * y[k-1] + (1-alpha) * u[k]

def __init__(self, alpha=0.5, y0=0.0):

self.alpha = alpha # filter parameter

self.y = y0 # initial condition

�

def update(self, u):

self.y = self.alpha * self.y + (1-self.alpha) * u

return self.y

�

# instantiation

self.lpf_gyro_x = AlphaFilter(alpha=0.7, y0=initial_measurements.gyro_x)

self.lpf_abs = AlphaFilter(alpha=0.9, y0=initial_measurements.abs_pressure)

self.lpf_diff = AlphaFilter(alpha=0.7, y0=initial_measurements.diff_pressure)

�

# update

# estimates for p, q, r are low pass filter of gyro minus bias estimate

self.estimated_state.p = self.lpf_gyro_x.update(measurement.gyro_x) \

- self.estimated_state.bx

abs_pressure = self.lpf_abs.update(measurement.abs_pressure)

diff_pressure = self.lpf_diff.update(measurement.diff_pressure)

�

\end{lstlisting}

12 of 53

Inverting Sensor Measurement Model

Several states can be estimated by inverting sensor measurement model

Angular rates, altitude, airspeed
LPF denotes low pass filter

Mathematical Model

State Estimate

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 12

13 of 53

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 13

14 of 53

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 14

15 of 53

Inverting Sensor Measurement Model

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 15

\noindent Assuming steady, level flight (no acceleration), we can also invert accelerometer measurements to determine pitch and roll. Accelerometer outputs are

\begin{align*}

y_{\text{accel,x}} &= \dot{u} + qw - rv + g\sin\theta +

\eta_{\text{accel,x}} \\

y_{\text{accel,y}} &= \dot{v} + ru - pw

-g\cos\theta\sin\phi +

\eta_{\text{accel,y}} \\

y_{\text{accel,z}} &= \dot{w} + pv - qu -

g\cos\theta\cos\phi + \eta_{\text{accel,z}}

\end{align*}

In unaccelerated flight, $\dot{u}=\dot{v}=\dot{w}=p=q=r=0$, and

\begin{align*}

\mathit{LPF}(y_{\text{accel,x}}) &= g\sin\theta \\

\mathit{LPF}(y_{\text{accel,y}}) &= -g\cos\theta\sin\phi\\

\mathit{LPF}(y_{\text{accel,z}}) &= -g\cos\theta\cos\phi

\end{align*}

Solving for $\phi$ and $\theta$:

\begin{align*}

\hat{\phi}_{\text{accel}} &= \tan^{-1}\left(

\frac{\mathit{LPF}(y_{\text{accel,y}})} {\mathit{LPF}(y_{\text{accel,z}})} \right) \\

\hat{\theta}_{\text{accel}} &= \sin^{-1}\left( \frac{\mathit{LPF}(y_{\text{accel,x}})}{g} \right)

\end{align*}

16 of 53

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 16

17 of 53

Inverting Sensor Measurement Model

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 17

18 of 53

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 18

19 of 53

Dynamic Observer Theory

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 19

20 of 53

Predictor-Corrector Structure

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 20

21 of 53

Predictor-Corrector Structure

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 21

22 of 53

Kalman Filter

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 22

23 of 53

Quadratic Forms

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 23

24 of 53

Multivariate Gaussian

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 24

25 of 53

Kalman Filter

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 25

26 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 26

27 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 27

28 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 28

\par\noindent{\bf Between Measurements (prediction):}

�

Compute the evolution of the error covariance $P$:

\begin{align*}

\dot{P} &= \frac{d}{dt} E\{\tilde{x}\tilde{x}^{\top}\}

= E\{ \dot{\tilde{x}}\tilde{x}^{\top} + \tilde{x}\dot{\tilde{x}}^{\top} \} \\

&= E\left\{ A\tilde{x}\tilde{x}^{\top} + \xi\tilde{x}^{\top} +

\tilde{x}\tilde{x}^{\top}A^{\top} + \tilde{x}\xi^{\top} \right\} \\

&= AP + PA^{\top} + E\{\xi\tilde{x}^{\top}\} + E\{\tilde{x}\xi^{\top}\},

\end{align*}

Where

\begin{align*}

E\{\tilde{x}\xi^{\top}\} &=

E\{

e^{At}\tilde{x}_0 \xi^{\top}(t)

+ \int_0^t e^{A(t-\tau)}\xi(\tau)\xi^{\top}(t)\,d\tau

\} \\

&= \int_0^t e^{A(t-\tau)}Q\delta(t-\tau)\,d\tau \\

&= \frac{1}{2} Q. \qquad \text{(only used half of area in delta function)}

\end{align*}

Therefore

\[

\dot{P} = AP + PA^{\top} + Q.

\]

29 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 29

\par\noindent{\bf At Measurements (correction):}

�

At a measurement, we have that

\begin{align*}

\tilde{x}^{+} &= x-\hat{x}^{+} \\

&= x-\hat{x}^{-} - L\left(Cx + \eta - C\hat{x}^{-}\right) \\

&= \tilde{x}^{-} - LC\tilde{x}^{-} - L\eta.

\end{align*}

We also have that

\begin{align*}

P^{+} &= E\{ \tilde{x}^{+}\tilde{x}^{+T} \} \\

&= E\left\{ \left( \tilde{x}^{-} - LC\tilde{x}^{-} - L\eta \right)

\left( \tilde{x}^{-} - LC\tilde{x}^{-} - L\eta \right)^{\top} \right\} \notag \\

&= E\Big\{ \tilde{x}^{-}\tilde{x}^{-\top} -

\tilde{x}^{-}\tilde{x}^{-\top} C^{\top} L^{\top} - \tilde{x}^{-} \eta^{\top} L^{\top}

-LC\tilde{x}^{-}\tilde{x}^{-\top} + LC

\tilde{x}^{-}\tilde{x}^{-\top} C^{\top} L^{\top} + LC\tilde{x}^{-}\eta^{\top}L^{\top}

\\ & \qquad \qquad

-L\eta\tilde{x}^{-\top} + L\eta\tilde{x}^{-\top}C^{\top}L^{\top} +

L\eta\eta^{\top}L^{\top} \Big\} \\

&= P^{-} - P^{-}C^{\top}L^{\top} - LCP^{-} + LCP^{-}C^{\top}L^{\top} + LRL^{\top} \\

&= (I - LC)P^{-}(I-LC)^\top + LRL^\top.

\end{align*}

�

\par\noindent{\bf Objective:} Pick $L$ to minimize $\text{trace}(P^+)$.

30 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 30

31 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 31

32 of 53

Covariance Update

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 32

33 of 53

Covariance Update

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 33

34 of 53

Covariance Update

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 34

35 of 53

Kalman Filter Derivation

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 35

36 of 53

Extended Kalman Filter

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 36

37 of 53

Extended Kalman Filter, cont.

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 37

38 of 53

EKF Algorithm

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 38

\begin{algorithm}

\caption{Continuous-Discrete Extended Kalman Filter}

\label{alg:est-kalman-continuous-discrete}

\begin{algorithmic}[1]

\STATE Initialize: $\hat{x} = 0$.

\STATE Pick an output sample rate $T_{out}$ which is much less than

the sample rates of the sensors.

\STATE At each sample time $T_{out}$:

\FOR[Prediction]{$i=1$ to $N$}

\STATE $T_p = T_{out}/N$

\STATE $\hat{x} \leftarrow \hat{x} + T_p f(\hat{x},u)$

\STATE $A = \frac{\partial f}{\partial x}(\hat{x},u)$

\STATE $A_d = I + A T_p + A^2 T_p^2$

\STATE $P \leftarrow A_d P A_d^\top + T_p^2 Q$

\ENDFOR

\IF[Correction]{Measurement has been received from

sensor $i$}

\STATE $C_i = \frac{\partial h_i}{\partial x}(\hat{x},u[n])$

\STATE $L_i = PC_i^{\top}(R_i+C_iPC_i^{\top})^{-1}$

\STATE $P \leftarrow (I-L_i C_i) P (I-L_i C_i)^\top + L_i R_i L_i^{\top}$

\STATE $\hat{x} \leftarrow \hat{x} + L_i\left( y_i[n] - h(\hat{x},u[n]) \right)$.

\ENDIF

\end{algorithmic}

\end{algorithm}

\noindent If $R$ is diagonal (uncorrelated sensors), then update one measurement at a time.

39 of 53

Attitude Estimation Using EKF

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 39

40 of 53

Attitude Estimation Using EKF

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 40

\noindent Assume that $\dot{u}=\dot{v}=\dot{w}\approx 0$

\noindent Recall that

\[

\begin{pmatrix} u \\ v \\ w \end{pmatrix} \approx V_a

\begin{pmatrix} \cos\alpha\cos\beta \\ \sin\beta \\

\sin\alpha\cos\beta \end{pmatrix}.

\]

Assuming that $\alpha\approx\theta$ and $\beta\approx 0$ gives

\[

\begin{pmatrix} u \\ v \\ w \end{pmatrix} \approx V_a

\begin{pmatrix} \cos\theta \\ 0 \\

\sin\theta \end{pmatrix}

\]

Substituting into original output equation

\begin{equation*}

y_{\text{accel}} =

\begin{pmatrix}

\dot{u} + qw - rv + g\sin\theta \\

\dot{v}+ru-pw - g\cos\theta\sin\phi \\

\dot{w}+pv-qu - g\cos\theta\cos\phi

\end{pmatrix}

+ \eta_{\text{accel}}

\end{equation*}

gives

\[

y_{\text{accel}} =

\begin{pmatrix}

qV_a\sin\theta + g\sin\theta \\

rV_a\cos\theta - pV_a\sin\theta - g\cos\theta\sin\phi \\

-qV_a\cos\theta - g\cos\theta\cos\phi

\end{pmatrix}

+ \eta_{\text{accel}}

\]

41 of 53

Attitude Estimation Using EKF

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 41

42 of 53

Attitude Estimation Using EKF

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 42

43 of 53

Attitude Estimation Results

Not perfect, but significantly better!

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 43

44 of 53

GPS Smoothing

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 44

45 of 53

GPS Smoothing

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 45

46 of 53

GPS Smoothing

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 46

47 of 53

GPS Smoothing

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 47

\noindent

Define: \\

$x=(p_n,p_e,V_g,\chi,w_n,w_e,\psi)^{\top}$ \\

$u=(V_a, q, r, \phi, \theta)^{\top}$ \\

\\

Jacobian of $f$:

\[

\pde{f}{x} =

\begin{pmatrix}

0 & 0 & \cos\chi & -V_g\sin\chi & 0 & 0 & 0 \\

0 & 0 & \sin\chi & V_g\cos\chi & 0 & 0 & 0 \\

0 & 0 & -\frac{\dot{V}_g}{V_g} & 0 & -\dot{\psi}V_a\sin\psi & \dot{\psi}V_a\cos\psi & \frac{\partial \dot{V}_g}{\partial \psi} \\

0 & 0 & \frac{\partial \dot{\chi}}{\partial V_g} & \frac{\partial \dot{\chi}}{\partial \chi} & 0 & 0 & \frac{\partial \dot{\chi}}{\partial \psi} \\

0 & 0 & 0 & 0 & 0 & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 0 & 0

\end{pmatrix}

\]

where

\begin{align*}

\frac{\partial \dot{V}_g}{\partial \psi} &= \frac{-\dot{\psi}V_a(w_n\cos\psi+w_e\sin\psi)}{V_g} \\

\frac{\partial \dot{\chi}}{\partial V_g} &= -\frac{g}{V_g^2}\tan\phi\cos(\chi-\psi) \\

\frac{\partial \dot{\chi}}{\partial \chi} &= -\frac{g}{V_g}\tan\phi\sin(\chi-\psi) \\

\frac{\partial \dot{\chi}}{\partial \psi} &= \frac{g}{V_g}\tan\phi\sin(\chi-\psi) \\

\end{align*}

48 of 53

GPS Smoothing

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 48

49 of 53

GPS Smoothing

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 49

50 of 53

GPS Smoothing Results

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 50

51 of 53

Measurement Gating

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 51

52 of 53

Measurement Gating

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 52

53 of 53

Suggestions for Tuning Your EKF

Implement EKF in steps

Attitude estimation
GPS smoother

Test and tune each component independently and thoroughly

Attitude estimator first
GPS smoother second

As a first step, make sure your filter estimates track the states when sensors are perfect (no sensor error). This will expose coding errors and show the limits of performance of your filter.
Keep the wind set to zero until you are confident that your filter is well tuned.
We know R pretty well typically. Tune filter by changing Q.
Tune filter by focusing on individual states. Give inputs to those states while keeping other states “quiet”. Adjust Q value corresponding to state of interest. Tune by trial and error (use your brain to make a good guess!).
Don’t put extreme inputs into the system when tuning the filter (e.g., large steps). Your filter will have its own dynamic limits – don’t make the problem impossibly difficult.
Tune your controllers so that the response of the aircraft to typical inputs is smooth and graceful. Abrupt and jerky state dynamics are difficult to estimate.
Check your equations AGAIN!

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 8, Slide 53