# 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 IMU has accumulated errors, and the integrated navigation using BDS positioning information improved the accuracy of long-term operation. The algorithm flow for estimating the error of the IMU using the error Kalman filter algorithm and correcting it by feedback is shown in Figure 4.  
Figure 4: Flow chart of BDS/IMU integrated navigation based on error Kalman filter. Figure 3: The IMU solution process. Note: \(L\) is latitude, \(\lambda\) is longitude, (\({}^{\circ}\)); \(h\) is altitude, m; \(\Phi\), \(\theta\), \(\gamma\) is pitch angle, heading angle and roll angle, rad; \(C_{b}^{n}(0)\) is the attitude matrix of spraying robot at initial stage; \(f_{ib}^{h}\) is the specific force projection of the coordinate system \(b\) (sprayer coordinate system) with reference to the coordinate system \(i\) (geocentric inertial coordinate system) under the coordinate system \(b\), m\(\cdot\)s\({}^{-2}\); \(V_{cb}^{n}\) is the velocity projection of the coordinate system \(b\) with reference to the coordinate system \(e\) (earth centered earth fixed) under the coordinate system \(n\) (east-north-sky navigation coordinate system), m\(\cdot\)s\({}^{-1}\); \(C\) is the directional cosine matrix, \(\omega\) is the angular velocity, rad\(\cdot\)s\({}^{-1}\), the upper corner indicates the coordinate system of the variable, the lower corner indicates the transformation coordinate system. Continuous-time error Kalman filter equation for BDS/IMU integrated navigation;  
When combining navigation systems for information fusion, we first established state equations that reflect the dynamic characteristics of the system state vectors. Due to the existence of navigation system errors and inertial sensor random errors, a suitable error propagation model was established to improve the navigation accuracy by using the external observation of optimal estimation errors. Considering the estimation accuracy and application environment, the IMU navigation parameter errors and inertial sensor drift errors (attitude error, velocity error, position error and device error) were selected as the state vector of the filter. The complexity of the filter is reduced while the accuracy of the navigation system is better satisfied. The state equation of integrated navigation system can be expressed as follows:  
\[\dot{X}(t)=F(t)X(t)+G(t)W(t) \tag{4}\]  
The state vector of system state equation can be expressed as follows:  
\[X(t)=\left[ \begin{array}{cccccccccccc}\mathbf{\sigma}_{E}&\mathbf{\sigma}_{N}&\mathbf{ \sigma}_{U}&\delta L&\delta\lambda&\delta h&\delta V_{E}&\delta V_{N}&\delta V_{U }&\epsilon_{gx}^{b}&\epsilon_{gy}^{b}&\epsilon_{gz}^{b}&\
abla_{ax}^{b}&\
abla _{ay}^{b}&\
abla_{az}^{b}\end{array} \right]^{T} \tag{5}\]  
where, \(\left[\mathbf{\sigma}_{E}&\mathbf{\sigma}_{N}&\mathbf{\sigma}_{U}\right]\) are the attitude angle errors of the east, north and sky axes in the spraying robot coordinate system, respectively, rad; \(\left[\delta L&\delta\lambda&\delta h\right]\) are the position errors of the spraying robot (latitude, longitude and height); \(\left[\delta V_{E}&\delta V_{N}&\delta V_{U}\right]\) are the velocity errors of the east, north and sky axes in the spraying robot coordinate system, respectively, m/s; \(\left[\epsilon_{gx}^{b}&\epsilon_{gy}^{b}&\epsilon_{gz}^{b}\right]\) are the three-axis gyro drift of the spraying robot, respectively, rad/s; \(\left[\
abla_{ax}^{b}&\
abla_{ay}^{b}&\
abla_{az}^{b}\right]\) are the three-axis accelerometer drift of the spraying robot, respectively, m/s2. The transfer matrix \(F(t)\) in the system state equation was obtained by decomposing the error vector equation of the IMU navigation system.