## 3 Intelligent Integrated Navigation Q-Learning FEKF Algorithm
### Design of the \(Q\)-Learning-Based FKF for Intelligent Integrated Navigation
``` Input:  Initial state estimation \(\hat{X}_{ps,0}\), error covariance matrix \(\mathbf{P}_{ps,0}\), predetermined set of the noise covariance matrix \(\hat{\mathbf{Q}}_{0}\), \(\hat{\mathbf{Q}}_{0}\), \(\hat{\mathbf{Q}}_{0}\), and learning parameter \(\alpha\), \(\gamma\), \(\varepsilon\)
1 Initialize variables, \(\hat{\mathbf{X}}_{ps,0}^{Tr(p)}=\hat{\mathbf{X}}_{ps,0}^{Ex(p)}=\hat{\mathbf{X}}_{ps,0}^{Se( p)}\leftarrow\hat{\mathbf{X}}_{ps,0}\), \(\hat{\mathbf{X}}_{ps,0}^{Tr(d)}=\hat{\mathbf{X}}_{ps,0}^{Ex(d)}=\hat{\mathbf{X}}_{ps,0}^{ Se(d)}\leftarrow\hat{\mathbf{X}}_{ps,0}\), \(\hat{\mathbf{X}}_{ps,0}^{S}\leftarrow\hat{\mathbf{X}}_{ps,0}\), \(\mathbf{P}_{ps,0}^{Tr}=\mathbf{P}_{ps,0}^{Ex}=\mathbf{P}_{ps,0}^{Se}\leftarrow\mathbf{P}_{ps,0}\), \(\mathbf{P}_{ps,0}^{T}\leftarrow\mathbf{P}_{ps,0}\)
2: Time index initialization \(k\gets 0\)
3: For the specified time of the navigation
4: Set state and action, for all construct state sets \(s\in\mathbf{S}\), the action set \(a\in\mathbf{A}\)
5: Environment state initialization \(s_{k}\gets s_{0}\), Q-value \(Q^{(p)}(s_{k},a_{k})\gets 0,R^{(p)}(s_{k},a_{k})\gets 0\), and reward initialization \(Q^{(d)}(s_{k},a_{k})\gets 0,R^{(d)}(s_{k},a_{k})\gets 0\)
6: Set the initial noise error covariance matrix \(\hat{\mathbf{Q}}_{rk}\leftarrow\hat{\mathbf{Q}}_{r0}\), \(\hat{\mathbf{Q}}_{ck}\leftarrow\hat{\mathbf{Q}}_{v0}\), and \(\hat{\mathbf{Q}}_{k}\leftarrow[\hat{\mathbf{Q}}_{rk},\hat{\mathbf{Q}}_{ck}]\);
7: \(\hat{\mathbf{R}}_{pk}\leftarrow\hat{\mathbf{R}}_{p0}\), \(\hat{\mathbf{R}}_{dk}\leftarrow\hat{\mathbf{R}}_{d0}\)
8: Generated randomly the noise error covariance matrix \(\hat{\mathbf{Q}}_{rk}^{(i)}\in\left[\bar{q}_{rk}\ast\hat{\mathbf{Q}}_{r0},\bar{q}_{rk} \ast\hat{\mathbf{Q}}_{r0}\right]\), \(\hat{\mathbf{Q}}_{cvk}^{(i)}\in\left[\bar{q}_{pk}\ast\hat{\mathbf{Q}}_{r0},\bar{q}_{rk }\ast\hat{\mathbf{Q}}_{r0}\right]\), \(\hat{\mathbf{R}}_{pk}\in\left[\bar{r}_{pk}\ast\hat{\mathbf{R}}_{p0},\bar{r}_{pk}\ast \hat{\mathbf{R}}_{p0}\right]\), \(\hat{\mathbf{R}}_{dk}\in\left[\bar{r}_{dk}\ast\hat{\mathbf{R}}_{d0},\bar{r}_{dk}\ast \hat{\mathbf{R}}_{d0}\right]\)
9: \(\varepsilon\)-greedy strategy \(a_{k}\leftarrow\varepsilon\)-greedy (\(s_{k}\), \(A\), \(Q\) (\(s_{k}\), \(a_{k}\)), \(\varepsilon\)) choose action \(a_{k}\) for state \(s_{k}\)
10: Execute action \(a_{k}\) and observe arrived the next states \(s^{\prime}_{k}\)
11: For each time step \(ltCy=1\), \(2\), \(\ldots\), \(T\) in one iteration, do
12: \(k\gets k+1\)
13: \(\left[\hat{\mathbf{X}}_{ps,k}^{Tr(p)},\mathbf{P}_{ps,k}^{Tr(p)},\bar{\mathbf{y}}_{ps,k}^ {Tr(p)}\right]\gets EKF\left(\hat{\mathbf{X}}_{ps,k-1}^{Tr(p)},\mathbf{P}_{ps,k-1}^ {Tr(p)},\mathbf{y}_{ps,pk}^{Tr(p)}\right)\)\(\times\)XP benchmark
14: \(\left[\hat{\mathbf{X}}_{ps,k}^{Tr(d)},\mathbf{P}_{ps,k}^{Tr(d)},\bar{\mathbf{y}}_{ps,k}^ {Tr(d)}\right]\gets EKF\left(\hat{\mathbf{X}}_{ps,k|k-1}^{Tr(d)},\mathbf{P}_{ps,k|k-1}^ {Tr(p)},\mathbf{y}_{ps,dk}^{Tr(d)},\hat{\mathbf{Q}}_{rk},\hat{\mathbf{R}}_{dk}\right)\)\(\times\)STD benchmark
15: \(\hat{\mathbf{Q}}_{rk}^{(i)},\hat{\mathbf{Q}}_{ck}^{(i)},\hat{\mathbf{R}}_{pk}^{(j)}\) and \(\hat{\mathbf{R}}_{dk}^{(j)}\) are determined according to the current state \(s_{k}\)
16: \(\left[\hat{\mathbf{X}}_{ps,k}^{Ex(p)},\mathbf{P}_{ps,k}^{Ex(p)},\bar{\mathbf{y}}_{ps,k}^ {Ex(p)}\right]\gets EKF\left(\hat{\mathbf{X}}_{ps,k|k-1}^{Ex(p)},\mathbf{P}_{ps,k|k-1}^ {Ex(p)},\mathbf{y}_{ps,pk}\hat{\mathbf{Q}}_{rk}^{(i)},\hat{\mathbf{Q}}_{ck}^{(i)},\hat\mathbf{{R}}_{pk}^{(j)}\right)\)\(\times\)XP searching
17: \(\left[\hat{\mathbf{X}}_{ps,k}^{Ex(d)},\mathbf{P}_{ps,k}^{Ex(d)},\bar{\mathbf{y}}_{ps,k}^ {Ex(d)}\right]\gets EKF\left(\hat{\mathbf{X}}_{ps,k|k-1}^{Ex(d)},\mathbf{P}_{ps,k|k-1}^ {Ex(d)},\mathbf{y}_{ps,dk}^{Ex(d)},\hat{\mathbf{Q}}_{rk}^{(i)},\hat{\mathbf{Q}}_{ck}^{(i)}, \hat{\mathbf{R}}_{dk}^{(j)}\right)\)\(\times\)STD searching
18: \(R^{(p)}(s_{k},a_{k})\leftarrow R^{(p)}(s_{k-1},a_{k-1})+\frac{1}{ltCy}\Bigg{\{} \bigg{[}\Big{(}\bar{\mathbf{y}}_{ps,k}^{Tr(p)}\Big{)}^{T}\Big{(}\hat{\mathbf{R}}_{pk}^ {(j)}\Big{)}^{-1}\bar{\mathbf{y}}_{ps,k}^{Ex(p)}\bigg{]}^{-1}-R^{(p)}(s_{k-1},a_{k-1 })\Bigg{\}}\)\(\times\)XP rewards
19: \(R^{(d)}(s_{k},a_{k})\leftarrow R^{(d)}(s_{k-1},a_{k-1})+\frac{1}{ltCy}\Bigg{\{} \bigg{[}\Big{(}\bar{\mathbf{y}}_{ps,k}^{Tr(d)}\Big{)}^{T}\Big{(}\hat{\mathbf{R}}_{dk}^ {(j)}\Big{)}^{-1}\bar{\mathbf{y}}_{ps,k}^{Ex(d)}\bigg{]}^{-1}-R^{(d)}(s_{k-1},a_{k- 1})\Bigg{\}}\)\(\times\)STD rewards
20: \(\mathbf{P}_{ps,k-1}^{\mathcal{S}(p)}\leftarrow\beta_{p}^{-1}\mathbf{P}_{ps,k-1}^{ \mathcal{S}}\), \(\hat{\mathbf{Q}}_{rk}^{(i)}\leftarrow\beta_{p}^{-1}\hat{\mathbf{Q}}_{rk}^{(i)}\), \(\hat{\mathbf{Q}}_{pk}^{(i)}\leftarrow\beta_{d}^{-1}\hat{\mathbf{Q}}_{pk}^{(i)}\)
21: \(\mathbf{P}_{ps,k-1}^{\mathcal{S}(p)}\leftarrow\beta_{p}^{-1}\mathbf{P}_{ps,k-1}^{ \mathcal{S}(p)},\hat{\mathbf{Q}}_{dk}^{(i)}\leftarrow\beta_{d}^{-1}\hat{\mathbf{Q}}_{ pk}^{(i)}\)\(\triangleright\)Information distribution weight
2 In Algorithm 1, the initial information includes the estimation error covariance matrix \(\hat{\mathbf{X}}_{ps,0}\), the state noise error covariance matrix \(\hat{Q}_{0}=\big{(}\hat{Q}_{r0},\hat{Q}_{r0}\big{)}\) of the system, and the observation noise error covariance matrix \(\big{(}\hat{R}_{p0},\hat{R}_{a0}\big{)}\) at the initial time.