## 3 QR Code Navigation and Construction of a Template Matching Library
### Extended Kalman Filter Fusion Data  
The estimated position of the QR code navigation AGV sweep data and the AMCL algorithm can be fused by a fusion method to obtain a more accurate position estimation [27,28]. In the fusion process, it is first necessary to convert the QR code navigation AGV scanning data into positional information, which is usually obtained by decoding the QR code to obtain the position and attitude information of the QR code. Then, this position information is fused with the estimated position of the AMCL algorithm. The fusion method is as follows:  
Figure 16: Improved AMCL algorithm localization process for QR code navigation fusion.  
Figure 15: Building a simulation environment: (**a**) synchronize environment on Rviz; (**b**) synchronize environment on Gazebo; (**c**) simulation model.  
Prediction steps.  
Establishing the equation of state:  
\[X_{k+1}=\begin{bmatrix}x_{k}\\ y_{k}\\ v_{k}\\ \theta_{k}\\ \omega_{k}\end{bmatrix}+\begin{bmatrix}v_{k}\cdot\cos(\theta)\cdot\Delta t\\ v_{k}\cdot\sin(\theta)\cdot\Delta t\\ 0\\ \omega\cdot\Delta t\\ 0\end{bmatrix}+\begin{bmatrix}\frac{1}{2}\cdot\vec{x}_{\omega,k}\cdot\cos( \theta)\cdot\Delta t^{2}\\ \frac{1}{2}\cdot\vec{x}_{\omega,k}\cdot\sin(\theta)\cdot\Delta t^{2}\\ \alpha_{\omega,k}\cdot\Delta t\\ \frac{1}{2}\cdot\vec{x}_{\omega,k}\cdot\Delta t^{2}\\ \alpha_{\omega,k}\cdot\Delta t^{2}\end{bmatrix},\ \omega_{k}\
eq 0 \tag{18}\]  
where \(x\) is the \(x\)-direction coordinate, \(y\) is the \(y\)-direction coordinate, \(v\) is the radial velocity, \(\theta\) is the yaw angle, \(\omega\) is the yaw angular velocity, \(\vec{x}_{\upsilon,k}\) is the acceleration of the radial velocity \(v\), and \(\vec{\alpha}_{\omega,k}\) is the acceleration of the yaw angular velocity \(\omega\).  
Condition prediction:  
\[\hat{x}_{k+1|k}=\left[f\left(\hat{x}_{k|k}\right)+F_{k}\left(x_{k}-\hat{x}_{k|k }\right)+\omega_{k}\right] \tag{19}\]  
where \(f\left(\hat{x}_{k|k}\right)\) is the state estimate, \(x_{k}\) is the state vector, and \(F_{k}\) is the higher order term derivation.  
From Equations (18) and (19) the predicted covariance prediction can be derived as  
\[P_{k+1|k}=\left[F_{k}\left(x_{k}-\hat{x}_{k|k}\cdot\cos(\theta)\Delta t\right) +\alpha_{\upsilon,k}\cdot\Delta t\right]\cdot\left[F_{k}\left(x_{k}+\hat{x}_{ k|k}\cdot\sin(\theta)\Delta t\right)-\alpha_{\omega,k}\cdot\Delta t^{2}\right] \tag{20}\]  
2. Update steps.  
Convert the QR code navigation AGV scanning data into an observation model, i.e., the position and attitude information of the QR code is converted into a positional observation. Compare the observation model with the predicted position and attitude, and calculate the observation residuals:  
\[\hat{y}=z_{k}-H_{k}\hat{x}_{k|k+1} \tag{21}\]  
\(z_{k}\) is the state estimation transfer equation and \(H_{k}\) is the observation matrix.  
\[\left\{\begin{aligned} z_{k}&=H_{k}\cdot x_{k}+v_{k}\\ x_{k}&=F_{k}x_{k-1}+B_{k}u_{k}+\omega_{k}\end{aligned}\right. \tag{22}\]  
\(v_{k}\) is the observation noise and \(B_{k}\) is the input control model acting on the controller vector \(u_{k}\).  
Calculate the Kalman gain:  
\[K_{k}=\hat{P}_{k}\cdot H^{T}\cdot\left(H\cdot\hat{P}_{k}H^{T}+R\right)^{-1} \tag{23}\]  
\(R\) is the measurement noise covariance matrix and \(H\) is the measurement matrix.  
Based on the Kalman gain and observation residuals, the updated bit position estimate is  
\[h\left(x^{\prime}\right)=\begin{bmatrix}\rho\\ \psi\end{bmatrix}=\begin{bmatrix}\sqrt{P_{x}^{2}+P_{y}^{2}}\\ \arctan\left(\frac{P_{y}}{P_{x}}\right)\\ \frac{P_{y}V_{x}+P_{y}V_{y}}{\sqrt{P_{x}^{2}+P_{y}^{2}}}\end{bmatrix} \tag{24}\]  
where \(P_{x}\), \(P_{y}\), \(V_{x}\), and \(V_{y}\) are the positions and velocities of the fused a priori estimates.  
Simulation result diagram.  
The two were fused for simulation to test the correlation relationship.  
Figure 17 shows the accuracy test of the EKF fusion data. Simulations using EKF fused data can yield close to the true values and can be used with this fusion method.