# Improved Adaptive Federated Kalman Filtering for INS/GNSS/VNS Integrated Navigation Algorithm
## 2 Multi-Source Fusion Model
### Measurement Equation of System  
After establishing the error state equation of the integrated navigation system, a measurement equation is required to update the attitude angle, velocity, and position of the integrated navigation system. GNSS can observe the velocity and position information of the unmanned vehicle, while VNS can observe the attitude angle of the unmanned vehicle. The velocity and position information of INS and GNSS were selected as the measurement  
Figure 1: The structure of the INS/GNSS/VNS integrated navigation system. vector to establish the sub-filter 1 of INS/GNSS, and the attitude angle information of INS and VNS was selected as the measurement vector to establish the sub-filter 2 of INS/VNS. For the measurement equation, we usually use the following discrete time expressions:  
\[\mathbf{Z}_{k}=\mathbf{H}_{k}\mathbf{X}_{k}+\mathbf{V}_{k} \tag{3}\]  
where \(\mathbf{Z}_{k}\) is the measurement vector, \(\mathbf{H}_{k}\) is the measurement matrix, and \(\mathbf{V}_{k}\) is the measurement noise vector, which is typically modeled as zero-mean Gaussian white noise. In the INS/GNSS integrated navigation, the GNSS provides measurements of the position and velocity. The measurement equation for the INS/GNSS integration is as follows:  
\[\mathbf{Z}_{GNSS,k} =\mathbf{H}_{GNSS,k}\mathbf{X}_{k}+\mathbf{V}_{GNSS,k} \tag{4}\] \[=\]  
where \(\mathbf{V}_{GNSS,k}\) represents the measurement noise vector of GNSS, which is modeled as independent zero-mean Gaussian white noise, and \(\mathbf{H}_{GNSS,k}\) represents the measurement matrix of GNSS, which can be expressed as:  
\[\mathbf{H}_{GNSS,k}= \begin{bmatrix}0_{3\times 3}&0_{3\times 3}&diag[(R_{M}+h)&(R _{N}+h)\cos L&1]&0_{3\times 6}\\ 0_{3\times 3}&diag[1&1&1]&0_{3\times 3}&0_{3\times 6}\end{bmatrix}  \tag{5}\]  
In the INS/VNS integrated navigation system, the visual navigation system provides measurements of the attitude. The measurement equation for the INS/VNS integration is as follows:  
\[\mathbf{Z}_{VNS,k}=\mathbf{H}_{VNS,k}\mathbf{X}_{k}+\mathbf{V}_{VNS,k}= \begin{bmatrix}\varphi_{FS}^{INS}-\varphi_{FS}^{VNS}\\ \varphi_{NLS}^{INS}-\varphi_{NLS}^{VNS}\\ \varphi_{UL}^{INS}-\varphi_{UL}^{INS}\end{bmatrix}  \tag{6}\]  
where \(\mathbf{V}_{VNS,k}\) represents the attitude measurement noise vector of VNS, which is modeled as independent zero-mean Gaussian white noise, and \(\mathbf{H}_{VNS,k}\) represents the measurement matrix of VNS, which can be expressed as:  
\[\mathbf{H}_{V,k}= \begin{bmatrix}diag[1&1&1]&0_{3\times 12}\end{bmatrix}  \tag{7}\]  
To clearly represent the information processing process of each sub-filter in the federated Kalman filter, it can be assumed that the model of the sub-filter is as follows:  
\[\mathbf{x}_{i,k}=\mathbf{\Phi}_{i,k}\mathbf{x}  
\[\mathbf{P}_{k/k-1}=\mathbf{\Phi}_{k/k-1}\mathbf{P}_{k-1}\mathbf{\Phi}_{k/k-1}^{T}+\mathbf{\Gamma}_{k,k -1}\mathbf{Q}_{k-1}\mathbf{\Gamma}_{k,k-1}^{T} \tag{11}\]  
where \(\mathbf{Q}_{k-1}\) represents the covariance matrix of the input system noise vector \(\mathbf{\omega}_{k-1}\), \(\mathbf{P}_{k-1}\) represents the error covariance matrix at time \(k-1\), and \(\mathbf{P}_{k/k-1}\) represents the one-step prediction error covariance matrix from time \(k-1\) to \(k\). Measurement update: The optimal Kalman gain for the sub-filter is as follows. \[K_{k}=\mathbf{P}_{k/k-1}\mathbf{H}_{k}{}^{T}(\mathbf{H}_{k}\mathbf{P}_{k/k-1}\mathbf{H}_{k}^{T}+\bm {R}_{k})^{-1} \tag{12}\]  
The covariance matrix of the measurement noise vector \(\mathbf{v}_{k}\) is \(\mathbf{R}_{k}\).