## 3 Controller Synthesis
### Overview and Synthesis of Kalman Filtering
#### 3.3.2 Extended Kalman Filter Synthesis  
In the following sections, we delve into the implementation details of the extended Kalman filter for a nonlinear model of the UAV based on [16]'s theory. Initially, the Kalman filter predicts the forthcoming system state based on the current estimated state \(\hat{\eta}_{k-1/k-1}\), established upon setting up all initial conditions:  
\[\hat{\eta}_{0/0}=E(\eta_{0}) \tag{9}\]  
\[\hat{P}_{0/0}=E((\eta_{0}-\hat{\eta}_{0/0})(\eta_{0}-\hat{\eta}_{0/0})^{T}) \tag{10}\]  
As in (1), the model noise \(w_{k}\) can be expressed as a zero-mean white Gaussian noise that captures system model discrepancies or uncertainties. The mathematical format for \(w_{k}~{}\sim~{}(0,~{}Q_{k})\), where \(Q_{k}\) is the covariance matrix of the model noise. The same case applies to observation noise \(v_{k}\), which is a zero-mean white Gaussian noise that represents inaccuracies in sensor measurements and has covariance \(R_{k}\). The mathematical representation for this is \(v_{k}~{}\sim~{}(0,~{}R_{k})\).  
We denote the state vector \(\eta_{k}\) at time step \(k\) as follows:  
\[\eta_{k}=\begin{pmatrix}x_{k}&y_{k}&z_{k}&\varphi_{k}&\theta_{k}&\psi_{k}\\ \end{pmatrix}^{T} \tag{11}\]  
The physical model representation in (1) can be assimilated to:  
\[\begin{cases}\eta_{\text{k}+1}=f(\eta_{\text{k}},u_{k})+w_{k}\\ \zeta_{k+1}=h(\eta_{\text{k}})+v_{k}\\ \end{cases} \tag{12}\]  
Here, \(f\) represents the nonlinear system dynamics function, \(u_{k}\) is the control input, and \(h\) is the measurement function. \(w_{k}\) and \(v_{k}\) are mutually uncorrelated with zero means  
Figure 3: Enhanced navigation through Kalman filtering.  
\(\mathrm{E}\big{[}w_{k}\,v_{k}^{T}\big{]}=0\) for all values of \(k\), where \(\mathrm{E}[\cdot]\) stands for the expectation operator. Figure 4 summarizes the EKF approach in the Parrot Mambo's flight control system (FCS). We then introduce the predicted state \(\widetilde{\eta}_{k/k-1}^{\sim}\).  
\[\widetilde{\eta}_{k/k-1}=F(\eta_{k-1/k-1},u_{k}) \tag{13}\]  
Furthermore, the level of uncertainty associated with the prediction is as follows:  
\[\widetilde{P}_{k/k-1}=E((\eta_{k/k-1}-\widetilde{\eta}_{k/k-1})(\eta_{k/k-1}- \widetilde{\eta}_{k/k-1})^{T}).\]  
\[\widetilde{P}_{k/k-1}=F_{k-1}\,\,\hat{P}_{k-1/k-1}\,\,F_{k-1}^{T}\,\,+\,\,Q_{k -1} \tag{14}\]  
Here, \(Q_{k-1}\) represents the process noise covariance matrix at \(k-1\), and \(F_{k-1}\) is the Jacobian matrix that is given by:  
\[F_{k}=\frac{\partial F(\eta_{k},u_{k})}{\partial\eta_{k}^{T}}|_{\eta_{k}= \bar{\eta}(k/(k-1))} \tag{15}\]  
where \(F_{k}\) is the Jacobian matrix of partial derivatives of F with respect to \(\eta_{k}\).  
In the correction step, we incorporate sensor measurements to update the state estimate based on the prediction. First, the optimization of the Kalman filter gain is determined by:  
\[\frac{d(Tr(\widetilde{P}_{k/k-1}^{\sim}))}{dK_{k}}=0 \tag{16}\]  
\(K_{k}\) is the Kalman gain matrix, which can be expressed as follows:  
\[K_{k}=\widetilde{P}_{k/k-1}\,\,H_{k}^{T}\,\,(H_{k}\,\,\widetilde{P}_{k/k-1}\, \,H_{k}\,\,+\,\,R_{k})^{-1} \tag{17}\]  
where \(H_{k}\) is the Jacobian matrix of the measurement function evaluated at \(k\) given by:  
\[H_{k}\!\!=\!\frac{\partial\bar{u}(\eta_{k})}{\partial\eta_{k}^{T}}|_{\eta_{k} =\widetilde{\eta}(k/k)} \tag{18}\]  
Subsequently, the Kalman filter refines the prediction to \(\widetilde{\eta}_{k/k-1}\) and adjusts the level of uncertainty, \(\widetilde{P}_{k/k-1}\), based on the current state (step k) after receiving measurements \(\zeta_{k}\), yielding:  
Figure 4: Schematic diagram of multi-sensor fusion with EKF approach.  
\[\hat{\eta}_{k/k}=\overset{\sim}{\eta}_{k/k-1}+K_{k}(\xi_{k}-h(\overset{\sim}{\eta}_ {k/k-1})) \tag{19}\]  
We may then update the covariance matrix to reflect the incorporation of sensor measurements. The covariance update equation is given by:  
\[\hat{P}_{k/k}=(I-K_{k}H_{k})\overset{\sim}{P}_{k/k-1} \tag{20}\]