# BDS/IMU Integrated Auto-Navigation System of Orchard Spraying Robot
## 2 Materials and Methods
### Overall Design of the System
#### 2.2.2 BDS/IMU Integrated Navigation Model
The system noise vector can be expressed as follows:  
\[W(t)=\left[\omega_{gx}^{b}&\omega_{gy}^{b}&\omega_{gz}^{b}&\
u_{ax}^{b}&\
u_ {ay}^{b}&\
u_{az}^{b}\right]^{T} \tag{6}\]  
where, \(\left[\omega_{gx}^{b}&\omega_{gy}^{b}&\omega_{gz}^{b}\right]\) are the white noise of the three-axis gyroscope of the spraying robot, respectively; \(\left[\upsilon_{gx}^{b}&\upsilon_{ay}^{b}&\upsilon_{az}^{b}\right]\) are the white noise of the three-axis accelerometer of the spraying robot, respectively. The noise transfer matrix can be expressed as follows:  
\[G(t)=\left[ \begin{array}{ccc}C_{b}^{n}&0_{3\times 3}\\ 0_{3\times 3}&C_{b}^{n}\\ 0_{9\times 3}&0_{9\times 3}\end{array} \right] \tag{7}\]  
where, \(C_{b}^{n}\) is the attitude transfer matrix. The measurement equation explains the nature of the integrated navigation system and relates the navigation information to the error equation of the navigation system. The position and heading angle accuracy of the dual-antenna BDS F-V11 receiver can meet the integrated navigation requirements, so the heading angle and position were used as the observed quantities. The measured value in the measurement equation was the difference of the heading angle and position output by IMU and F-V111 receiver, respectively, and the system measurement equation can be expressed as follows:  
\[Z(t)=\left[ \begin{array}{c}yaw_{1}-yaw_{B}\\ L_{I}-L_{B}\\ \lambda_{I}-\lambda_{B}\end{array} \right]=HX(t)+V(t) \tag{8}\]where, \( \begin{bmatrix}yaw_{I}&L_{I}&\lambda_{I}\end{bmatrix} \) are the heading angle, latitude and longitude output by IMU; \( \begin{bmatrix}yaw_{B}&L_{B}&\lambda_{B}\end{bmatrix} \) are the heading angle, latitude and longitude output by F-V111; \(V(t)\) is the noise of the measurement system. The measurement matrix in system measurement equation can be expressed as follows:  
\[H= \begin{bmatrix}0_{3\times 2}&diag\begin{bmatrix}1&1&1\end{bmatrix}&0_{3 \times 10}\end{bmatrix}  \tag{9}\]
2. Discrete error Kalman filter equation for BDS/IMU integrated navigation;  
To meet the computer computing requirements, the continuous-time error Kalman filter equation was transformed into a discrete-error Kalman filter equation. \[ \begin{cases}X_{k}=AX_{k-1}+\omega_{k-1}\\ Z_{k}=HX_{k}+v_{k}\end{cases}  \tag{10}\]  
where, \(X_{k}\), \(X_{k-1}\) are the states of the system at moments \(k\) and \(k-1\); \(A\) is the system state transition matrix; \(\omega_{k-1}\) is the Gaussian noise during state transfer at moment \(k-1\); \(Z_{k}\) is the measured value of the system at moment \(k\); \(H\) is the system parameters of the measurement system; \(v_{k}\) is the Gaussian noise of the measurement system at moment \(k\). 3. The error Kalman filter equation; \[\left\{ \begin{array}{l}\hat{X}_{k/k-1}=A\hat{X}_{k-1}\\ \hat{X}_{k/k}=\hat{X}_{k/k-1}+K_{k}\big{(}Z_{k}-H\hat{X}_{k/k-1}\big{)}\\ K_{k}=P_{k/k-1}H^{T}\big{(}HP_{k/k-1}H^{T}+R_{k}\big{)}^{-1}\\ P_{k/k-1}=AP_{k-1/k-1}A^{T}+Q_{k-1}\\ P_{k/k}=(I-K_{k}H)P_{k/k-1}\end{array} \right.\] (11)  
where, \(\hat{X}_{k/k-1}\) is the predicted value of the state at moment \(k-1\) for moment \(k\); \(\hat{X}_{k/k}\) is the state estimate at moment \(k\); \(K_{k}\) is the filtering gain at moment \(k\); \(Q_{k-1}\) is the system noise variance matrix at moment \(k-1\); \(P_{k/k-1}\) is the covariance matrix of the prediction error estimates at moment \(k-1\) for moment \(k\); \(P_{k/k}\) is the covariance matrix of the error estimate at moment \(k\); \(R_{k}\) is the measurement noise error matrix at moment \(k\).