State estimation for distributed drive agricultural electric vehicles in complex mountain environments

1 Introduction

With the widespread application of advanced sensing and intelligent control technologies in agricultural equipment, modern agricultural machinery is continually evolving towards greater efficiency and intelligence (Jin et al., 2025). Against this backdrop, ensuring the dynamic stability and operational safety of agricultural vehicles under complex working conditions has become a critical research topic in contemporary agricultural mechanisation.

Regarding the estimation of vehicle motion states, extensive research has been conducted by scholars, primarily establishing an algorithmic framework centered on particle filter (PF), extended Kalman filter (EKF), unscented Kalman filter (UKF), and cubature Kalman filter (CKF). In addressing non-linear and non-Gaussian noisy systems, particle filtering has garnered significant attention due to its unique resampling mechanism. To address particle depletion and diversity deficiencies, Xu et al. and Zhao et al. respectively introduced the enhanced Northern Hawk algorithm and adaptive genetic algorithm to optimize resampling weights, effectively enhancing state observation accuracy for distributed electric vehicles and semitrailer trains under non-Gaussian noise conditions (Xu et al., 2025; Zhao et al., 2025). Wang et al. further achieved high-precision estimation of steer-by-wire chassis dynamics by constructing a dual-tracking particle filter framework (Wang et al., 2024). However, the substantial computational demands of Particle Filtering constrain its real-time application within vehicle controllers.

As the most mature methods in engineering applications, the extended Kalman filter and unscented Kalman filter hold significant importance in vehicle state estimation. Ahmadi Jeyed et al. utilized an extended Kalman filter to effectively estimate the non-linear dynamics of articulated heavy vehicles (Ahmadi Jeyed and Ghaffari, 2019). Building upon this, Reina et al. further developed the Augmented extended Kalman filter, incorporating road gradient and adhesion coefficient into the state vector, thereby effectively resolving environmental parameter identification challenges in complex terrain (Reina and Messina, 2019). Wang et al. established a longitudinal model incorporating suspension dynamics and employed the EKF to design a pitch angle estimator, thereby validating its reliability in residual range prediction (Wang et al., 2025). Addressing model uncertainty and noise interference, Prakash et al. constructed an adaptive robust EKF based on maximum likelihood principles, effectively mitigating performance degradation caused by random noise (Prakash and Dheer, 2023). Zha et al. enhanced system robustness through adaptive weight fusion of Radial Basis Function Neural Networks (RBFNNs) and the EKF (Zha et al., 2022). To overcome the limitations of EKF’s linearised truncation error, Hannes et al. and Wu et al. respectively proposed an adaptive UKF observer considering road surface friction coefficients and an STKF-UKF hybrid estimation architecture, significantly improving estimation stability for four-wheel-drive vehicles across different lateral acceleration ranges (Hannes et al., 2020; Wu et al., 2023). Xu et al. further optimized parameter adaptation capabilities in high-speed scenarios by integrating adaptive UKF using interactive multi-model theory (Xu et al., 2022). Chebbi et al. investigated Kalman-filter-based state estimation and control in nonlinear engineering systems and further reviewed simultaneous state and parameter estimation methods, highlighting the importance of model accuracy, parameter uncertainty, and filter tuning for reliable estimation (Chebbi et al., 2024; Chebbi et al., 2025).

Cubature Kalman filter emerged as a research focus in recent years due to its superior numerical stability and accuracy over UKF in nonlinear systems, finding application in agricultural machinery. Successful implementation by Gao et al. in aerospace validated CKF’s efficacy for high-dimensional strongly nonlinear systems, paving the way for its adoption in ground vehicle and agricultural machinery research (Gao et al., 2022). To further address complex time-varying conditions, the integration of Interacting Multiple Model with CKF has emerged as a mainstream trend. Wan et al. proposed an IMM algorithm based on the square-root cubature Kalman filter, which effectively enhances estimation accuracy under severe manoeuvring conditions through a model soft-switching mechanism (Wan et al., 2021). Hou et al. and Zhang et al. employed the IMM-CKF framework to incorporate model switching mechanisms and adaptive noise statistics, achieving high-precision tracking of sideslip angle and tire forces (Hou et al., 2019; Zhang and Li, 2017). Additionally, addressing robustness in adverse environments, Chen et al. and Qi et al. proposed adaptive square-root cubature Kalman filter and Maximum Correntropy SRCKF respectively, demonstrating the square-root filtering structure’s significant advantages in suppressing non-Gaussian noise and enhancing numerical stability (Chen et al., 2022; Qi et al., 2023). Building upon this foundation, Liu et al. have recently proposed an enhanced Maximum Correntropy SRCKF algorithm (Liu et al., 2025). This approach further addresses the convergence challenges posed by extreme measurement noise through an adaptive kernel bandwidth adjustment mechanism. Gao et al. integrated robust CKF with a forgetting factor recursive least squares method, successfully resolving the challenge of joint mass and state estimation under variable gradient conditions (Gao et al., 2025).

Although the aforementioned algorithms have achieved significant results in the field of vehicle state estimation, they still exhibit limitations when confronted with the specific operational environment of mountainous agricultural machinery. On the one hand, extended Kalman filters may suffer from reduced estimation accuracy or even divergence when handling highly non-linear vehicle dynamics due to linearisation truncation errors. While unscented Kalman filters avoid linearisation, they are prone to numerical instability arising from non-positive definite covariance matrices in high-dimensional systems. On the other hand, while the standard cubature Kalman filter outperforms the aforementioned algorithms in non-linear approximation capabilities, it remains highly susceptible to computational error accumulation under the rugged terrain and strong noise interference commonly encountered by agricultural vehicles. This leads to the loss of positivity and symmetry in the error covariance matrix, ultimately causing filter termination. Furthermore, existing research predominantly focuses on passenger or commercial vehicles on structured roads, with limited studies addressing highly robust state estimation for the dynamics of mountainous agricultural machinery in complex, unstructured terrain.

In light of this, a state estimation algorithm for agricultural vehicles based on the square-root cubature Kalman filter is proposed in this study. By directly propagating the covariance square-root factors through QR decomposition, while using Cholesky factorization to obtain the square-root factors of the initial and noise covariance matrices, the SRCKF avoids explicit propagation of the full covariance matrix and improves numerical stability. Taking mountainous agricultural electric vehicles as the research subject, a three-degree-of-freedom vehicle dynamics model is established. Utilising the CarSim-Simulink co-simulation platform, typical mountainous operation scenarios, namely, chassis twist and discrete impacts, are constructed. Through simulation experiments, the proposed algorithm’s estimation accuracy and reliability in complex dynamic environments are verified.

2 Vehicle parameters and research operating conditions

2.1 Vehicle parameters

CarSim is employed to construct a vehicle-level simulation model and establish a motor model within the MATLAB/Simulink environment. As the primary focus of this study is placed on the parameter state estimation for mountainous agricultural electric vehicles under varying operating conditions, a motor model with minimal complexity is required. Consequently, modelling details of the motor’s internal structure are omitted during development, concentrating solely on its input and output characteristics to achieve model simplification. The current motor and vehicle speed as input variables are utilized in the motor model, with the actual motor output torque as the output variable. Its functional modelling is implemented through the MATLAB/Simulink platform. This modelling approach effectively reflects the real-time dynamic performance of the motor during vehicle operation, meeting the requirements for control strategy design and validation.

The physical image of the vehicle is depicted in Figure 1. The relevant parameters for the mountainous terrain agricultural electric vehicle and motor are shown in Table 1.

TABLE 1. Parameters of mountainous agricultural electric vehicles and motors.

2.1.1 Chassis twist conditions

In mountainous orchard environments, agricultural vehicles frequently encounter diagonal crossing of irrigation segments or non-standard ridges. This scenario is simulated using a chassis twist condition, where the vehicle’s frame undergoes extreme torsional deformation. As a wheel climbs a ridge while the opposite wheel remains in a depression, the wheel-plane non-coplanarity poses a severe challenge to the traction and posture estimation of the electric drive system. The simulation is set on a straight eastward path with sinusoidal vertical deformations (5 cycles). The target vehicle speed is set to 6 km/h. For the chassis-twist road dataset in CarSim, the road-surface friction coefficient and tire rolling-resistance coefficient are set to 0.85 and 1.0, respectively. A schematic diagram of the chassis twist conditions is shown in Figure 2.

2.1.2 Discrete impact conditions

This condition replicates the traversal of scattered rocks, exposed tree roots, or localized road obstacles commonly encountered on unpaved mountain tracks. The discrete-impact road is defined using an S–L grid in CarSim, with the road elevation reconstructed by two-dimensional linear interpolation and extrapolation. Four smooth bumps with an identical maximum incremental elevation of 0.30 m are arranged along the longitudinal direction. The bumps are located approximately within the station ranges of 10–15 m, 30–35 m, 90–95 m, and 98–103 m, respectively, corresponding to a longitudinal length of approximately 5 m for each bump. Their center-to-center spacings are approximately 20 m, 60 m, and 8 m. The elevated profile spans the main road region from approximately −3.5 m to +3.5 m in the lateral direction. The vehicle maintains a speed of 15 km/h, consistent with typical rapid transit on rough farmland, to test the filter’s ability to suppress high-frequency noise and resolve real-time body states. For the discrete-impact road dataset in CarSim, the road-surface friction coefficient and tire rolling-resistance coefficient are also set to 0.85 and 1.0, respectively. A schematic diagram of the discrete impact conditions is shown in Figure 3.

3 System modeling and enhanced SRCKF algorithm design

3.1 Estimation algorithm architecture

The proposed estimation architecture mainly consists of signal acquisition, signal preprocessing, and vehicle-state estimation. In the CarSim–Simulink co-simulation framework, vehicle-level and wheel-level information, including steering angle, longitudinal and lateral accelerations, yaw rate, and tire-force-related signals, is obtained from CarSim or the corresponding force-estimation modules. It should be noted that these signals do not all directly participate in the SRCKF measurement update. In the current SRCKF implementation, the measurement vector consists of the yaw rate, longitudinal acceleration, and lateral acceleration. These three signals can be obtained from an onboard inertial measurement unit (IMU) in practical vehicle applications. The tire-force-related signals shown in the overall co-simulation architecture are provided by CarSim or the corresponding force-estimation modules and are not treated as directly measurable sensor inputs of the current SRCKF. After appropriate preprocessing, the measurement signals are incorporated into the SRCKF to estimate the longitudinal velocity, lateral velocity, yaw rate, longitudinal acceleration, and lateral acceleration of the vehicle. The overall estimation architecture is illustrated in Figure 4.

3.2 Nonlinear vehicle state-space model

To achieve precise simulation and estimation of the dynamic state of agricultural machinery during operation, a three-degree-of-freedom vehicle dynamics model is introduced in this study, which represents a simplified formulation of vehicle dynamics. The three-degree-of-freedom vehicle dynamics model treats the vehicle as a single mass point rotating about an axis connected to its center within a plane. It accounts for lateral and longitudinal motion within this plane, alongside angular oscillation about a rotational axis linked to the vehicle’s center. It facilitates the investigation of vehicle dynamic characteristics such as stability, manoeuvrability, and ride comfort. The model is matched to algorithm design and required input/output data, with the coordinate origin positioned at the vehicle’s center of gravity. The model represents the vehicle body as a rigid body, connected by four independently controlled wheels, as illustrated in Figure 5.

Its governing equations are given by Equations 1–3. A body-fixed coordinate system is adopted, with the x-axis pointing forward along the longitudin al direction of the vehicle, the y-axis pointing to the left, and the z-axis pointing upward. Positive yaw motion follows the right-hand rule about the z-axis. where, a _x denotes the vehicle’s longitudinal acceleration, a _y denotes the vehicle’s lateral acceleration, u denotes the longitudinal velocity, v denotes the lateral velocity, r denotes the yaw rate, I _z denotes the moment of inertia about the z-axis, and M _z denotes the yaw moment.

u ˙ = a x + v r (1)

v ˙ = a y − u r (2)

r ˙ = 1 I z M z (3)

The longitudinal acceleration, lateral acceleration, and yawing moment of the vehicle may be expressed by Equations 4–6. where, F _{x_fl,} F _{x_fr}, F _{x_rl} and F _{x_rr} denote the longitudinal forces acting on the vehicle’s four wheels respectively, F _{y_fl}, F _{y_fr}, F _{y_rl} and F _{y_rr} represent the lateral forces on the four wheels, δ _fl, δ _fr, δ _rl and δ _rr denote the steering angles of the four wheels, m is the vehicle’s total mass.where, a and b denote the distances from the vehicle’s center of gravity to the front and rear axles respectively, B ₁ and B ₂ denote the track widths of the front and rear wheels respectively. The four-wheel distributed-drive configuration is reflected in the vehicle-dynamics model through the independent representation of the wheel-level forces. In particular, the longitudinal forces of the four wheels are retained separately, allowing the effects of asymmetric wheel forces on the longitudinal and yaw dynamics to be considered. It should be noted that the distributed-drive configuration characterizes the vehicle platform, whereas the robustness-enhancement mechanisms of the SRCKF itself are not dependent on the drivetrain topology.

a x = 1 m ( F x _ f l ⁡ cos ⁡ δ f l − F y _ f l ⁡ sin ⁡ δ f l + F x _ f r ⁡ cos ⁡ δ f r − F y _ f r ⁡ sin ⁡ δ f r + F x _ r l + F x _ r r (4)

a y = 1 m ( F x f l ⁡ sin ⁡ δ f l + F y f l ⁡ cos ⁡ δ f l + F x f r ⁡ sin ⁡ δ f r + F y _ f r ⁡ cos ⁡ δ f r + F y _ r l + F y _ r r (5)

M z = a F x _ f l ⁡ sin ⁡ δ f l + F y _ f l ⁡ cos ⁡ δ f l − B 1 2 F x _ f l ⁡ cos ⁡ δ f l − F y _ f l ⁡ sin ⁡ δ f l + a F x _ f r ⁡ sin ⁡ δ f r + F y _ f r ⁡ cos ⁡ δ f r + B 1 2 F x _ f r ⁡ cos ⁡ δ f r − F y _ f r ⁡ sin ⁡ δ f r − b F y _ r l + F y _ r r − B 2 2 F x _ r l − F x _ r r (6)

For the state-estimation implementation, the filter state vector and measurement vector are defined in Equations 7, 8, respectively.where V _x = u and V _y = v correspond to the longitudinal and lateral velocities in the vehicle-dynamics model, respectively. The measurement vector is defined as

X k = V x , k , V y , k , r k , a x , k , a y , k T (7)

z k = r k , a x , k , a y , k T (8)

No independent control-input vector is introduced in the current SRCKF implementation. The state propagation is based on the previous state estimate and the measurement-related quantities described above. In the actual implementation, the state-transition function f(⋅) adopts a measurement-assisted kinematic propagation scheme. The longitudinal velocity is propagated using the previous longitudinal acceleration, while the lateral velocity is reconstructed from the sideslip-angle evolution determined by lateral acceleration and yaw rate. The yaw rate, longitudinal acceleration, and lateral acceleration are propagated using first-order weighted updates based on their previous estimates and the corresponding measurements. The observation function directly maps the yaw rate, longitudinal acceleration, and lateral acceleration from the state vector to the measurement space.

Accordingly, the nonlinear state-space model, observation function, and derived sideslip-angle calculation are summarized in Equations 9–12. where f(⋅) and h(⋅) denote the nonlinear state-transition and observation functions, respectively, and W _k-1 and V _k denote the process and measurement noises. For the present implementation, the observation function is

X k = f X k − 1 + W k − 1 (9)

z k = h X k + V k (10)

h X k = r k a x , k a y , k (11)

The vehicle sideslip angle, expressed in radians, is subsequently calculated from the estimated velocity components asand is therefore treated as a derived quantity rather than an independent filter state.

β k = atan ⁡ 2 V y , k , V x , k (12)

Based on the above vehicle-dynamics and state-space formulations, the vehicle model and the square-root cubature Kalman filter estimator are implemented in the CarSim–Simulink co-simulation framework. Figures 6–8 illustrate the corresponding vehicle-state estimation architecture.

3.3 Standard square-root cubature Kalman filter

To improve the numerical stability of nonlinear state estimation, the square-root cubature Kalman filter (SRCKF) directly propagates the square-root factor of the error covariance matrix instead of explicitly updating the full covariance matrix. For the nonlinear discrete-time system defined in Section 3.2, the process noise and measurement noise are assumed to satisfy:where Q and R denote the noise assumptions and corresponding square-root covariance representations are given in Equations Equations 13–15.

w k ∼ N 0 , Q , v k ∼ N 0 , R (13)

Q = S Q S Q T , R = S R S R T (14)

In the SRCKF implementation, the process and measurement noises are assumed to be zero-mean random variables with covariance matrices Q and R, respectively. The sampling interval, initial state, initial covariance, process noise covariance, and measurement noise covariance used in the simulations are summarized in Table 2. No additional synthetic outlier sequence or prescribed non-Gaussian probability distribution is artificially introduced. The abnormal measurement disturbances investigated in this study arise from the vehicle responses to the specified CarSim road excitations and enter the estimator through the simulated measurement channels.

TABLE 2. Main simulation and filter parameters used in the SRCKF implementation.

Similarly, the state-error covariance matrix is represented as:where S _k|k denotes the square-root factor of the posterior error covariance matrix.

P k | k = S k | k S k | k T (15)

3.3.1 Cubature-point generation and time update

For an n-dimensional state vector, N _*c* = 2n third-degree spherical-radial the complete cubature-point generation and time-update procedure is summarized in Equations 16–23. where e _i denotes the ith unit vector. The corresponding weight of each cubature point is

ξ i = n e i , i = 1 , 2 , … , n (16)

ξ i + n = − n e i , i = 1 , 2 , … , n (17)

ω i = 1 2 n , i = 1 , … , 2 n (18)

Based on the posterior state estimate and square-root covariance factor at time k−1, the cubature points are generated as

X i , k − 1 k − 1 = X ^ k − 1 k − 1 + S k − 1 k − 1 ξ i (19)

The generated cubature points are propagated through the nonlinear state-transition function:

X i , k | k − 1 * = f X i , k − 1 k − 1 (20)

The predicted state mean is then calculated as

X ^ k | k − 1 = 1 2 n ∑ i = 1 2 n X i , k | k − 1 * (21)

To directly propagate the square-root covariance, the normalized centered cubature-point matrix is constructed as

x k | k − 1 = 1 2 n X 1 , k | k − 1 * − X ^ k | k − 1 , … , X 2 n , k | k − 1 * − X ^ k | k − 1 (22)

Rather than explicitly calculating the predicted covariance matrix, the predicted square-root covariance factor is obtained using QR decomposition:where Tria(⋅) denotes the triangular square-root factor obtained through QR decomposition.

S k | k − 1 = T r i a x k | k − 1 , S Q (23)

3.3.2 Measurement update

Using the predicted state estimate and square-root covariance factor, a new set of cubature points is generated. The measurement-update and posterior square-root covariance-update procedures are summarized in Equations 24–32:

X i , k | k − 1 = X ^ k | k − 1 + S k | k − 1 ξ i (24)

The cubature points are then propagated through the nonlinear measurement function:

Z i , k | k − 1 = h X i , k | k − 1 (25)

The predicted measurement mean is calculated as

Z ^ k | k − 1 = 1 2 n ∑ i = 1 2 n Z i , k | k − 1 (26)

The normalized centered measurement matrix is defined as

Z k | k − 1 = 1 2 n Z 1 , k | k − 1 − z ^ k | k − 1 , … , Z 2 n , k | k − 1 − z ^ k | k − 1 (27)

Accordingly, the square-root factor of the innovation covariance is calculated directly using QR decomposition:

S z z , k = T r i a Z k | k − 1 , S R (28)

The state-measurement cross-covariance matrix is calculated as

P x z , k = 1 2 n ∑ i = 1 2 n X i , k | k − 1 Z i , k | k − 1 − z ^ k | k − 1 T (29)

The Kalman gain is subsequently obtained as

K k = P x z , k S z z , k S z z , k T − 1 (30)

In numerical implementation, triangular matrix solution can be employed instead of directly calculating the matrix inverse to improve computational stability.

The posterior state estimate is updated according to

X ^ k | k = X ^ k | k − 1 + K k z k − z ^ k | k − 1 (31)

3.3.3 Square-root covariance update

Unlike the conventional CKF, the SRCKF does not explicitly calculate the posterior covariance matrix. The posterior square-root covariance factor is directly updated using QR decomposition as

S k | k = Tria X k | k − 1 − K k Z k | k − 1 , K k S R (32)

The dominant computational cost of the standard SRCKF arises from QR decomposition and matrix operations associated with covariance square-root propagation, with a computational complexity on the order of O(n ³). The additional robustness mechanisms introduced in this study mainly involve state projection and rate limiting O(n), fixed Kalman-gain attenuation O(nm), and measurement preprocessing and innovation saturation O(m), where n and m denote the state and measurement dimensions, respectively. Therefore, these additional operations do not alter the dominant computational complexity order of the SRCKF, although they introduce a limited constant computational overhead. For the present estimator, n = 5 and m = 3, and the additional protection operations therefore involve only low-dimensional element-wise calculations.

3.4 Robustness enhancement mechanisms of the SRCKF

Traditional nonlinear filtering algorithms are typically developed under idealized assumptions regarding system modeling and noise characteristics. However, under mountainous agricultural operating conditions, severe road-surface excitations and high-frequency measurement disturbances may amplify estimation errors and increase the risk of numerical instability or state divergence. To address these engineering challenges, a multi-layer robustness-enhancement framework is introduced by integrating physically guided state constraints, fixed gain attenuation, and dual-layer abnormal-measurement protection.

3.4.1 Physically guided state constraint mechanism

To prevent filtered estimates from deviating beyond the physically feasible domain under strong disturbances, a forced constraint function is introduced following the prediction and update phases in this study. This mechanism constructs a dual hard-constraint model incorporating both amplitude boundaries and dynamic change rates.

Based on the physical limitations of agricultural machinery, the feasible domain Ω _x for the state vector x _k is defined. Here, the longitudinal velocity v _x is strictly constrained within the interval [0.1, 15] m/s to prevent zero-speed singularities and overspeed divergence; The sideslip angle β is dynamically constrained within a predefined physically admissible sideslip angle bound of ±0.349 rad(equivalent to ±20°), with boundary violations corrected through the corresponding lateral velocity projection; Corresponding physical thresholds are also established for yaw rate r and lateral acceleration a _y. The state-projection rule is given in Equation 33:where proj_Ωx(·) denotes the feasible region projection operator. In the current implementation, the hard projection acts only on the state estimate, while the square-root covariance factor generated by the SRCKF covariance update is retained without an additional projection-based transformation. Therefore, the constraint is implemented as an engineering state-protection mechanism rather than as a covariance-consistent constrained-filter update.

x ^ k = proj Ω x x ^ k − = x max , if x ^ k − > x max x ^ k − , if x min ≤ x ^ k − ≤ x max x min , if x ^ k − < x min (33)

To simulate the substantial physical inertia of the vehicle and suppress state jumps caused by sudden sensor changes, the algorithm incorporates a rate-of-change limiter based on the sampling interval Δt. For the velocity states v _*x* and v _*y*, the deviation between the estimated value at time k and the previous time step must satisfy the inertia constraint:

v i , k − v i , k − 1 ≤ λ i Δ t , i ∈ x , y (34)

In Equation 34, λ _i denotes the maximum allowable rate of change of the velocity state along the i-axis, equivalently an acceleration bound, with units of m/s². For the physically guided state constraints, these thresholds are specifically tuned to λ _x = 1.0 m/s² and λ _y = 0.5 m/s². The two rate limits were selected according to the physically feasible acceleration ranges of the investigated low-speed agricultural vehicle and preliminary numerical tuning. They are used as engineering protection bounds rather than globally optimized parameters. This constraint physically smooths the state trajectory and eliminates non-physical high-frequency oscillations.

3.4.2 Fixed gain attenuation in the measurement update

The Kalman gain matrix K _k of the standard SRCKF is entirely derived from the error covariance, rendering it highly sensitive to instantaneous measurement noise. To address the issue of low signal-to-noise ratio in accelerometer signals under the bumpy operating conditions of agricultural machinery, an element-level gain attenuation strategy is proposed in this study.

After calculating the theoretical gain K _theory, the weighted matrix W is introduced to perform the Hadamard product correction. In this fixed gain attenuation strategy, the fixed attenuation weights matrix W in Equation 35 applies scaling factors of 0.8, 0.6 and 0.6 for the yaw rate, longitudinal acceleration, and lateral acceleration update gains. The attenuation factors of 0.8, 0.6, and 0.6 were empirically selected according to the relative measurement sensitivity of the three channels and remain fixed throughout the simulations; therefore, they are treated as engineering tuning parameters rather than adaptive or globally optimal values.

K k = K theory ∘ W (35)

To further purify sensor inputs, a dual-protection system comprising measurement preprocessing and innovation sequence saturation is established in this study:

• Measurement preprocessing. The proposed estimation framework incorporates a preprocessing function before the filter update phase. This function calculates the deviation between the raw sensor measurement z _k and the predicted mapping value h x ∧ k k − 1 of the prior state. If the deviation exceeds the threshold τ _pre set based on physical limits, the measurement is deemed an outlier sensor value and subjected to truncation processing:

z i , k new = z i , k , z i , k − h x ∧ i ≤ τ i h x ∧ i + sgn Δ z · τ i , otherwise (36)

• 2. Innovation sequence saturation. During the update process, a strict saturation constraint is imposed upon the innovation vector z ^ k. An innovation threshold is established to forcibly truncate the amplitude of the residual when it becomes excessively large. This effectively prevents a single substantial disturbance from propagating through the gain matrix to the entire state vector, thereby ensuring the smoothness of the estimation results. Furthermore, in the dual-layer outlier rejection mechanism, the measurement preprocessing thresholds τ _pre in Equation 36 are restricted to 45.84 deg/s, 2.0 m/s², and 1.5 m/s² for the yaw-rate, longitudinal-acceleration, and lateral-acceleration channels, while the maximum innovation saturation vector is rigidly bounded at [0.5, 1.0, 0.8]^*T* . These thresholds were selected with reference to the physically admissible signal ranges and disturbance magnitudes under the investigated operating conditions and are used as engineering protection parameters rather than optimized values.

4 State estimation validation

The comparison baseline used in this study is the standard SRCKF, which retains the conventional square-root cubature filtering structure but excludes the additional physical constraints, fixed gain attenuation, and dual-layer outlier-handling mechanisms introduced in the proposed method.

4.1 Chassis twist conditions

The Chassis Twist conditions primarily test the suspension’s impact resistance and the chassis dynamic response on rugged off-road terrains. In Figures 9a,b present the longitudinal and lateral velocity estimates, respectively; Figures 9c,d show the longitudinal and lateral acceleration estimates; and Figures 9e,f present the yaw rate and sideslip angle estimates, respectively. The vehicle travels at a low speed of approximately 1.6 m/s (nearly 6 km/h) accompanied by severe vertical bumps. This physical environment of low speed and high oscillation results in extremely small effective components in the sensor signals, while noise interference is drastically amplified. This severely challenges the robustness of the filtering algorithms. The estimation results for longitudinal velocity and lateral velocity show that the standard SRCKF algorithm suffers from severe integration divergence under strong noise interference. Specifically in the lateral velocity estimation, while the true value remains around 0 m/s, the standard SRCKF accumulates estimation errors over time, with the lateral-velocity estimate exceeding 4.0 m/s near the end of the simulation and deviating substantially from the CarSim reference. In contrast, the enhanced SRCKF maintains bounded longitudinal and lateral velocity estimates throughout the investigated condition. The longitudinal velocity curve of the enhanced SRCKF closely follows the true value fluctuating between 1.5 m/s and 1.9 m/s, and the lateral velocity estimation converges smoothly near the zero axis, showing reduced estimation drift. Regarding the tracking performance of accelerations, tires rolling over bump obstacles trigger high-frequency physical impacts. The true Ax oscillates violently between ±1.5 m/s². The standard SRCKF is more directly affected by high-frequency road-induced measurement disturbances, resulting in large transient fluctuations in the estimated accelerations, with amplitudes reaching approximately ±3.0 m/s² and ±8.0 m/s². Compared with the standard SRCKF, the enhanced SRCKF exhibits smaller high-frequency fluctuations in the acceleration estimates while retaining the main transient variation of the CarSim reference signals. In the estimation of the sideslip angle and yaw rate, since the vehicle speed is extremely low with no large steering maneuvers, the true β and r exhibit extremely weak fluctuation characteristics (the true fluctuation amplitude of r is only ±0.05 deg/s). This extremely low signal-to-noise ratio environment causes the standard SRCKF algorithm to exhibit substantial tracking deviation, with the β estimation erroneously drifting above 0.25 rad. The enhanced SRCKF demonstrates an improved tracking performance to extract weak signals. Its estimation curves maintain high consistency with the CarSim true outputs in both phase and amplitude. Although β exhibits a very minor steady-state offset near the end of the simulation, no divergence trend occurs globally. Based on the overall results, the standard SRCKF algorithm exhibits substantial estimation degradation under the severe Chassis Twist conditions. Overall, the enhanced SRCKF maintains more stable and bounded estimates than the standard SRCKF under the investigated chassis-twist condition.

To further quantify the estimation accuracy and anti-noise robustness of the proposed algorithm under severe chassis twist scenarios, the Root Mean Square Error (RMSE) of the evaluated quantities is calculated and presented in Table 3. The estimation accuracy is quantified using the root mean square error (RMSE), calculated from the synchronized CarSim reference and estimated state sequences as the RMSE is calculated according to Equation 37.where N denotes the number of valid paired samples, x ∧ k is the estimated state, and x k r e f is the corresponding CarSim reference value. Invalid samples, if any, are excluded before the calculation.

R M S E = 1 N ∑ k = 1 N x ^ k − x k r e f 2 (37)

TABLE 3. Root Mean Square Error (RMSE) of evaluated quantities under chassis twist conditions.

As shown in Table 3, the standard SRCKF exhibits substantial estimation errors, particularly in lateral velocity (1.8536 m/s) and lateral acceleration (2.2092 m/s²), due to severe noise amplification at low speeds. In contrast, the enhanced SRCKF achieves lower RMSE values for all evaluated quantities. Notably, the lateral velocity RMSE is reduced to 0.0112 m/s under this condition.

4.2 Discrete impact conditions

The discrete-impact condition primarily evaluates the disturbance-rejection and state-estimation performance of the vehicle state-estimation system under sudden, high-intensity road-induced transient disturbances. In Figure 10, Figures 10a,b show the longitudinal and lateral velocity estimates, Figures 10c,d present the longitudinal and lateral acceleration estimates, and Figures 10e,f show the yaw rate and sideslip angle estimates, respectively. The vehicle maintains a straight path and sequentially runs over discrete obstacles at specific timestamps (such as around 5s, 12s, and 25s), which triggers severe transient vertical and longitudinal chassis shock oscillations. In the estimation curves of longitudinal velocity and lateral velocity, the standard SRCKF algorithm exhibits prominent cumulative drift characteristics. As the simulation progresses, the V _x estimate of the standard SRCKF gradually deviates upward from the baseline, while V _y estimate becomes severely distorted and drifts far away from the realistic physical boundaries. In contrast, the enhanced SRCKF maintains bounded longitudinal- and lateral-velocity estimates and follows the overall variation of the CarSim reference more closely under the discrete-impact condition. Throughout the entire simulation cycle, the V _x trajectory of the enhanced SRCKF tightly matches the CarSim true value, and V _y estimate remains close to the reference value of 0 m/s, with relatively small tracking errors. Regarding the tracking performance of accelerations, the instant the tires run over the obstacles transmits high-frequency shockwaves to the chassis. The true longitudinal acceleration A _x generates instantaneous oscillations at the impact points, while the true lateral acceleration A _y generally stays around 0 due to straight-line driving. At these moments, the standard SRCKF algorithm is more directly affected by the sensor measurements, which causes its estimation curves to be filled with relatively large high-frequency fluctuations, resulting in increased estimation errors. During the discrete impacts, the standard SRCKF exhibits relatively large high-frequency fluctuations in the acceleration estimates. In comparison, the enhanced SRCKF produces smoother acceleration estimates while retaining the main transient characteristics of the CarSim reference signals. In the estimation of the sideslip angle and yaw rate, which are critical metrics for evaluating vehicle handling stability, the true values of β and r constantly hover near zero because the vehicle executes no steering maneuvers. However, the standard SRCKF algorithm is strongly affected by the discrete impacts. It exhibits sharp, needle-like spikes with amplitudes up to ±0.15 deg/s in the yaw rate r estimation, and its sideslip angle β demonstrates a step-like divergence trend. During the discrete impacts, the enhanced SRCKF maintains bounded yaw-rate and sideslip-angle estimates with smaller transient deviations than the standard SRCKF. The blue enhanced SRCKF curves overlap closely and remain close to the CarSim reference values during both the flat road cruising and the severe impact instances. Overall, the standard SRCKF exhibits substantial estimation degradation under the severe road-induced transient disturbances of the discrete-impact condition. In comparison, the enhanced SRCKF maintains more stable and bounded estimates under the investigated discrete-impact condition.

Similarly, the statistical RMSE results for the discrete impact conditions are summarized in Table 4, which distinctly demonstrates the algorithm’s capability to suppress high-frequency transient disturbances.

TABLE 4. Root Mean Square Error (RMSE) of evaluated quantities under discrete impact conditions.

Quantitative results show that the enhanced SRCKF achieves lower RMSE values than the standard SRCKF baseline under the discrete-impact condition. Specifically, the RMSEs of longitudinal and lateral velocities are 0.0876 m/s and 7.169 × 10⁻⁶ m/s, respectively. These results indicate that the integrated enhanced SRCKF framework achieves lower estimation errors than the standard SRCKF baseline under the investigated discrete-impact condition.

5 Conclusion

To address the challenges of accurate state estimation for large agricultural machinery in complex operational environments, an enhanced square-root cubature Kalman filter framework is proposed in this study. The main contribution of this study is the vehicle-physics-guided integration of a physically guided state constraint mechanism, fixed gain attenuation, and dual-layer abnormal-measurement protection within the SRCKF framework. These mechanisms help maintain the state estimates within physically feasible ranges and suppress integration drift and numerical instability under severe terrain excitations and abnormal measurement disturbances. Systematic evaluations were conducted using a CarSim-Simulink joint simulation platform across two typical mountainous agricultural scenarios: chassis twist and discrete impacts. The results demonstrate that:

• The enhanced SRCKF provides simultaneous estimation of longitudinal velocity, lateral velocity, yaw rate, longitudinal acceleration, and lateral acceleration, while the sideslip angle is derived from the estimated velocity components.
• Under the investigated chassis-twist and discrete-impact conditions, the enhanced SRCKF maintains bounded estimates and lower overall estimation errors than the standard SRCKF baseline.
• The integrated physical-state constraints and measurement-protection framework improves estimation stability under severe road-induced disturbances within the investigated CarSim–Simulink simulation conditions.

The present results demonstrate the performance of the integrated enhancement framework under the investigated simulation conditions. However, the current study is limited to CarSim–Simulink co-simulation, and systematic ablation analysis, multi-realization statistical evaluation, parameter-sensitivity analysis, vehicle-parameter uncertainty tests, broader robust-filter benchmarking, and experimental validation have not yet been conducted. In practical vehicle implementation, additional issues such as sensor bias, signal synchronization, communication delay, vehicle-parameter variations, and ECU computational constraints will also need to be considered. Future work will therefore focus on systematic robustness evaluation, hardware-in-the-loop testing, and full-vehicle experiments using independent reference measurements.

Generative AI statement

The author(s) declared that generative AI was not used in the creation of this manuscript.

Any alternative text (alt text) provided alongside figures in this article has been generated by Frontiers with the support of artificial intelligence and reasonable efforts have been made to ensure accuracy, including review by the authors wherever possible. If you identify any issues, please contact us.

Publisher’s note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

Footnotes

Edited by: Xiaojie Sun, Shanghai Jiao Tong University, China

Reviewed by: Amal Chebbi, Fitila PLLC, United States

Guangfei Xu, Liaocheng University, China