A Robust Inner and Outer Loop Control Method for Trajectory Tracking of a Quadrotor

. 2017 Sep 19;17(9):2147. doi: 10.3390/s17092147

Algorithm 1: Extended Kalman Filter

Given the initial state

x_{0}

and initial covariance matrix

P_{0}

, update the state estimation as follows
Compute the predicted state:

{\hat{x}}_{k | k - 1} = f (x_{k - 1}, w_{k - 1})

Compute the process model Jacobian matrix :

F_{k - 1} = \frac{\partial f}{\partial x} |_{x_{k - 1}}

G_{k - 1} = \frac{\partial f}{\partial w} |_{x_{k - 1}}

Compute the predicted covariance matrix :

P_{k | k - 1} = F_{k - 1} P_{k - 1} F_{k - 1}^{T} + G_{k - 1} Q_{k - 1} G_{k - 1}^{T}

Compute the Kalam gain:

K_{k} = P_{k | k - 1} H^{T} {(H P_{k | k - 1} H^{T} + R_{k})}^{- 1}

Update the state estimation:

x_{k} = {\hat{x}}_{k | k - 1} + K_{k} (z_{k} - h ({\hat{x}}_{k | k - 1}))

Update the covariance matrix:

P_{k} = P_{k | k - 1} - K_{k} H P_{k | k - 1}