Task $1$: Kalman Filter Design

You will need to design an integrated GPS$/$IMU$/$Air$-$data navigation system by constructing a navigation model with $12$ states $($position$,$ velocity, attitude and wind components$),6$ inputs and $12$ measurements, and adapt your scalar extended Kalman filter $($EKF$)$ or iterated extended Kalman filter $($IEKF$)$ algorithm from the lab. The choice between EKF and IEKF is at your own discretion. If you are familiar with other estimation schemes, e$.$g$.$ unscented Kalman filter, feel free to use them as well.

For this task you will work with dataTask$1.$ mat which contains input measurements $c_k,$ the outputs $d_k$ and the time variables $t$ and $dt$ in seconds. $c_k$ and $d_k$ are matrices with rows the recordings of different time instances and the columns the variables are in the same ordering of $c_m$ and $d_m$ presented earlier.

You will realize that in comparison to your lab exercise, the expected value for the initial condition $|)=E\{x(t_0)\}$ and the error covariance $|)$ are not explicitly given. You could use engineering judgment to configure this.

For state variables that can be directly measured $(x_E,y_E,z_E,,,),$ simply assume the initial value to be the same as the noisy measurements at the first time instance.

The airspeed body components $(u,v,w)$ we do not directly measure but we know the airspeed $V_{\text{T}\text{A}\text{S}}$ is measured and we know the airspeed will be mostly in the forward travelling direction. So it is reasonable to configure the initial guess of $(u,v,w)$ to be $(V_{\text{T}\text{A}\text{S}},0,0).$ As for the wind speeds, we can simply guess them to be zero. The key point of making these engineering guesses is that you need to tell the scheme through $|)$ that these guesses are far from being accurate, by making the variance$/$standard deviation suitably large.

Task $1\mathrm{.}1$: For the use of the Kalman filter, you will need to derive the Jacobian matrices for our nonlinear system. Obtain the expressions for your derived Jacobian matrices $F=\frac{\text{d}\text{e}\text{l}\text{f}}{\text{d}\text{e}\text{l}\text{x}},G=\frac{\text{d}\text{e}\text{l}\text{g}}{\text{d}\text{e}\text{l}}$ and $H=\frac{\text{d}\text{e}\text{l}\text{h}}{\text{d}\text{e}\text{l}\text{x}}$ with $f$ and $g$ from $(1)$ and $h$ from $(2).$ Present the expressions for the following elements in your report:

The $5$th row, $4$th column of the $F$ matrix,

The $6$th row, $7$ th column of the $F$ matrix,

The $4$th row, $6$th column of the $G$ matrix,

The $9$th row, $5$th column of the $G$ matrix,

The $7$th row, $7$th column of the $H$ matrix,

The $11$th row, $4$th column of the $H$ matrix.

Hints: substitute IMU measurement equations into the kinematic equations, then group different terms appropriately into $f(x(t),c(t),t)$ and $g(x(t),(t)).$ Equations for the kinematic model and observation model can be found in usefulEquations.m$,$ and you can use Matlab's Symbolic Math Toolbox to help you compute the required matrices $($code developed in Lab B and $C$ may be helpful$).$

Task $1\mathrm{.}2$: Complete the Kalman Filter design for the integrated navigation system $($code developed in Lab C may be helpful$).$ In clear figures, plot the trajectories of the estimated $12$ states and comment on your findings.

SUBMISSION TEMPLATE

function

$]$

$[$ instance, $Ao{A}_f{?}_i$nstance

$\%\%$ Note: Bename the function in the format of SID $+$ vour student ID as on Blackboard $($e$.$g$.$ if xeur ID is $21010000,$ name the function as SID$21010000$ and submit it as$.$ SID$21010000.$m$)$

$$\%$

$\%$ Input Variables

load$(\text{'}$dataTask$1.$mat'$)$

$\%$ c k: input measurements, a N x $6$ matrix with rows the recordings of different

$\%$ d k: output measurements, a N x $12$ matrix with rows the recordings of different W GPS m$,$ Phi GPS m$,$ theta GPS m$,$ RSi GPS m$,$ V TAS m$,$ alpha m$,$ beta m$]$

$\%$ t: time vecter

$\%$ dt: uniform time step size

$\%$ qutput Vaciables

$\%$ x est: estimated state trajectory, a N x $12$ matrix with rows the recordings of psi, $[$:$v_{-}\{wxE\},v_{-}\{wxE\},v_{-}\{wzE\}\}$

$\%$ best: estimated state trajectory, a N x $6$ matrix with rows the recordings of

$\%$ Ax $f$