No Arabic abstract
Time-equispaced inertial measurements are practically used as inputs for motion determination. Polynomial interpolation is a common technique of recovering the gyroscope signal but is subject to a fundamentally numerical stability problem due to the Runge effect on equispaced samples. This paper reviews the theoretical results of Runge phenomenon in related areas and proposes a straightforward borrowing-and-cutting (BAC) strategy to depress it. It employs the neighboring samples for higher-order polynomial interpolation but only uses the middle polynomial segment in the actual time interval. The BAC strategy has been incorporated into attitude computation by functional iteration, leading to accuracy benefit of several orders of magnitude under the classical coning motion. It would potentially bring significant benefits to the inertial navigation computation under sustained dynamic motions.
This paper proposes a dynamic analytical initialization method for spacecraft attitude estimators. In the proposed method, the desired attitude matrix is decomposed into two parts: one is the constant attitude matrix at the very start and the other encodes the attitude changes of the body frame from its initial state. The latter one can be calculated recursively using the gyroscope outputs and the constant attitude matrix can be determined using constructed vector observations at different time. Compared with traditional initialization methods, the proposed method does not necessitate the spacecraft being static or more than two non-collinear vector observations at the same time. Therefore, the proposed method can promote increased spacecraft autonomy by autonomous initialization of attitude estimators. The effectiveness and prospect of the proposed method in spacecraft attitude estimation applications have been validated through numerical simulations.
Routing strategies for traffics and vehicles have been historically studied. However, in the absence of considering drivers preferences, current route planning algorithms are developed under ideal situations where all drivers are expected to behave rationally and properly. Especially, for jumbled urban road networks, drivers actual routing strategies deteriorated to a series of empirical and selfish decisions that result in congestion. Self-evidently, if minimum mobility can be kept, traffic congestion is avoidable by traffic load dispersing. In this paper, we establish a novel dynamic routing method catering drivers preferences and retaining maximum traffic mobility simultaneously through multi-agent systems (MAS). Modeling human-drivers behavior through agents dynamics, MAS can analyze the global behavior of the entire traffic flow. Therefore, regarding agents as particles in smoothed particles hydrodynamics (SPH), we can enforce the traffic flow to behave like a real flow. Thereby, with the characteristic of distributing itself uniformly in road networks, our dynamic routing method realizes traffic load balancing without violating the individual time-saving motivation. Moreover, as a discrete control mechanism, our method is robust to chaos meaning drivers disobedience can be tolerated. As controlled by SPH based density, the only intelligent transportation system (ITS) we require is the location-based service (LBS). A mathematical proof is accomplished to scrutinize the stability of the proposed control law. Also, multiple testing cases are built to verify the effectiveness of the proposed dynamic routing algorithm.
This work deals with error models for trident quaternion framework proposed in the companion paper (Part I) and further uses them to investigate the odometer-aided static/in-motion inertial navigation attitude alignment for land vehicles. By linearizing the trident quaternion kinematic equation, the left and right trident quaternion error models are obtained, which are found to be equivalent to those derived from profound group affine. The two error models are used to design their corresponding extended Kalman filters (EKF), namely, the left-quaternion EKF (LQEKF) and the right-quaternion EKF (RQEKF). Simulations and field tests are conducted to evaluate their actual performances. Owing to the high estimation consistency, the L/RQEKF converge much faster in the static alignment than the traditional error model-based EKF, even under arbitrary large heading initialization. For the in-motion alignment, the L/RQEKF possess much larger convergence region than the traditional EKF does, although they still require the aid of attitude initialization so as to avoid large initial attitude errors.
State estimation problems without absolute position measurements routinely arise in navigation of unmanned aerial vehicles, autonomous ground vehicles, etc., whose proper operation relies on accurate state estimates and reliable covariances. Unaware of absolute positions, these problems have immanent unobservable directions. Traditional causal estimators, however, usually gain spurious information on the unobservable directions, leading to over-confident covariance inconsistent with actual estimator errors. The consistency problem of fixed-lag smoothers (FLSs) has only been attacked by the first estimate Jacobian (FEJ) technique because of the complexity to analyze their observability property. But the FEJ has several drawbacks hampering its wide adoption. To ensure the consistency of a FLS, this paper introduces the right invariant error formulation into the FLS framework. To our knowledge, we are the first to analyze the observability of a FLS with the right invariant error. Our main contributions are twofold. As the first novelty, to bypass the complexity of analysis with the classic observability matrix, we show that observability analysis of FLSs can be done equivalently on the linearized system. Second, we prove that the inconsistency issue in the traditional FLS can be elegantly solved by the right invariant error formulation without artificially correcting Jacobians. By applying the proposed FLS to the monocular visual inertial simultaneous localization and mapping (SLAM) problem, we confirm that the method consistently estimates covariance similarly to a batch smoother in simulation and that our method achieved comparable accuracy as traditional FLSs on real data.
This paper presents a novel approach using sensitivity analysis for generalizing Differential Dynamic Programming (DDP) to systems characterized by implicit dynamics, such as those modelled via inverse dynamics and variational or implicit integrators. It leads to a more general formulation of DDP, enabling for example the use of the faster recursive Newton-Euler inverse dynamics. We leverage the implicit formulation for precise and exact contact modelling in DDP, where we focus on two contributions: (1) Contact dynamics in acceleration level that enables high-order integration schemes; (2) Formulation using an invertible contact model in the forward pass and a closed form solution in the backward pass to improve the numerical resolution of contacts. The performance of the proposed framework is validated (1) by comparing implicit versus explicit DDP for the swing-up of a double pendulum, and (2) by planning motions for two tasks using a single leg model making multi-body contacts with the environment: standing up from ground, where a priori contact enumeration is challenging, and maintaining balance under an external perturbation.