José Fernandes Vasconcelos

A Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame

Abstract In this work a new algorithm is derived for the onboard calibration of three-axis strapdown magnetometers. The proposed calibration method is written in the sensor frame, and compensates for the combined effect of all linear time-invariant distortions, namely soft iron, hard iron, sensor non-orthogonality, bias, among others. A Maximum Likelihood Estimator (MLE) is formulated to iteratively find the optimal calibration parameters that best fit to the onboard sensor readings, without requiring external attitude references. It is shown that the proposed calibration technique is equivalent to the estimation of an ellipsoidal surface, and that the sensor alignment matrix is given by the solution of the orthogonal Procrustes problem. Good initial conditions for the iterative algorithm are obtained by a suboptimal batch least squares computation. Simulation and experimental results with low-cost sensors data are presented, supporting the application of the algorithm to autonomous vehicles and other robotic platforms.

Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame

In this work a new algorithm is derived for the onboard calibration of three-axis strapdown magnetometers. The proposed calibration method is written in the sensor frame, and compensates for the combined effect of all linear time-invariant distortions, namely soft iron, hard iron, sensor nonorthogonality, and bias, among others. A maximum likelihood estimator (MLE) is formulated to iteratively find the optimal calibration parameters that best fit to the onboard sensor readings, without requiring external attitude references. It is shown that the proposed calibration technique is equivalent to the estimation of a rotation, scaling and translation transformation, and that the sensor alignment matrix is given by the solution of the orthogonal Procrustes problem. Good initial conditions for the iterative algorithm are obtained by a suboptimal batch least squares computation. Simulation and experimental results with low-cost sensors data are presented and discussed, supporting the application of the algorithm to autonomous vehicles and other robotic platforms.

A nonlinear position and attitude observer on SE(3) using landmark measurements

This paper addresses the problem of position and attitude estimation, based on landmark readings and velocity measurements. A derivation of a nonlinear observer on SE(3) is presented, using a Lyapunov function conveniently expressed as a function of the difference between the estimated and the measured landmark coordinates. The resulting feedback laws are explicit functions of the landmark measurements and velocity readings, exploiting the sensor information directly in the observer. The proposed observer yields almost global asymptotic stabilization of the position and attitude errors and exponential convergence in any closed ball inside the region of attraction. Also, it is shown that the asymptotic convergence of the estimation error trajectories is shaped by the landmark geometry and observer design parameters. The problem of non-ideal velocity readings is also considered, and the observer is augmented to compensate for bias in the angular and linear velocity measurements. The resulting position, attitude, and bias estimation errors are shown to converge exponentially fast to the desired equilibrium points, for bounded initial estimation errors. Simulation results are presented to illustrate the stability and convergence properties of the observer.

A Nonlinear GPS/IMU based observer for rigid body attitude and position estimation

This work proposes a position and attitude nonlinear observer based on inertial measurements and GPS pseudorange readings. The observation problem is formulated on SE(3), and the solution yields exponential convergence of the attitude and position estimates. The GPS pseudorange measurements and inertial sensor readings are exploited directly in the observer, and the integration of vector readings in the observer is discussed. The proposed observer dynamics compensate for the bias in the angular velocity sensor and the clock offset in GPS pseudorange measurements. The stability of the position and velocity estimates in the presence of bounded accelerometer noise is also analyzed. The properties of the GPS/IMU based observer are illustrated in simulation for a rigid body describing a challenging trajectory.

Landmark based nonlinear observer for rigid body attitude and position estimation

This work proposes a nonlinear observer for position and attitude estimation on SE(3). Using a Lyapunov function based on the landmark measurement error, almost global exponential stability (GES) of the desired attitude and position equilibrium points is obtained. It is shown that the derived feedback law is an explicit function of the landmark measurements and velocity readings, and that the landmark geometry characterizes the asymptotic convergence of the closed loop system solution. Almost global exponential stabilization in the presence of biased velocity readings is also achieved. Simulation results for trajectories described by time-varying linear and angular velocities and for distinct initial conditions on SE(3) are presented to illustrate the stability and convergence properties of the observer.

A Nonlinear Observer for Rigid Body Attitude Estimation using Vector Observations

Abstract This work proposes a nonlinear observer for attitude estimation on SO(3), exploiting the information of vector observations and biased angular rate measurements. It is shown that the attitude and bias estimation errors converge exponentially fast to the origin, for arbitrary angular velocity trajectories. The proposed attitude feedback law is an explicit function of the vector measurements and observer estimates, and convergence rate bounds are obtained using recent results for parametrized linear time-varying systems. The stability and convergence properties of the estimation errors are evidenced in simulation for time-varying angular velocities.

Discrete-Time Complementary Filters for Attitude and Position Estimation: Design, Analysis and Experimental Validation

This paper develops a navigation system based on complementary filtering for position and attitude estimation, with application to autonomous surface crafts. Using strapdown inertial measurements, vector observations, and global positioning system (GPS) aiding, the proposed complementary filters provide attitude estimates in Euler angles representation and position estimates in Earth frame coordinates, while compensating for rate gyro bias. Stability and performance properties of the proposed filters under operating conditions usually found in oceanic applications are derived, and the tuning of the filter parameters in the frequency domain is emphasized. The small computational requirements of the proposed navigation system make it suitable for implementation on low-power hardware and using low-cost sensors, providing a simple yet effective multirate architecture suitable to be used in applications with autonomous vehicles. Experimental results obtained in real time with an implementation of the proposed algorithm running on-board the DELFIMx catamaran, an autonomous surface craft developed at ISR/IST for automatic marine data acquisition, are presented and discussed.

USBL/INS Tightly-Coupled Integration Technique for Underwater Vehicles

This paper presents a new ultra-short baseline (USBL) tightly-coupled integration technique to enhance error estimation in low-cost strap-down inertial navigation systems (INSs) with application to underwater vehicles. In the proposed strategy the acoustic array spatial information is directly exploited resorting to the extended Kalman filter implemented in a direct feedback structure. The determination and stochastic characterization of the round trip travel time are obtained resorting to pulse detection matched filters of acoustic signals modulated using spread-spectrum code division multiple access (CDMA). The performance of the overall navigation system is assessed in simulation and compared with a conventional loosely-coupled solution that consists of solving separately the triangulation and sensor fusion problems. From the simulation results it can be concluded that the proposed technique enhances the position, orientation, and sensors biases estimates accuracy

INS/GPS Aided by Frequency Contents of Vector Observations With Application to Autonomous Surface Crafts

This paper presents a high-accuracy, multirate inertial navigation system (INS) integrating global positioning system (GPS) measurements and advanced vector aiding techniques for precise position and attitude estimation of autonomous surface crafts (ASCs). Designed to be implemented and tested in the DELFIMx catamaran developed at ISR/IST, the navigation system comprises an advanced inertial integration algorithm to account for coning and sculling motions, combined with an extended Kalman filter (EKF) for inertial sensor error compensation. Aiding gravitational observations are optimally exploited in the EKF, by deriving a sensor integration technique that takes into account the vehicles dynamics bandwidth information to properly trace measurement disturbances and extract the relevant sensor information. The proposed aiding technique and the performance of the navigation system are assessed using experimental data obtained at seatrials with a low-cost hardware architecture installed on-board the DELFIMx platform. It is shown that the low frequency information embodied in pendular measurements improves the compensation of inertial sensor bias and noise, and consequently enhances the performance of position and attitude estimation. The overall improvements obtained with the vector aiding observations are also illustrated for the case of GPS signal outage, emphasizing the extended autonomy of the navigation system with respect to position aiding.

Embedded Vehicle Dynamics Aiding for USBL/INS Underwater Navigation System

This brief presents an embedded vehicle dynamics (VD) aiding technique to enhance position, velocity, and attitude error estimation in low-cost inertial navigation systems (INSs), with application to underwater vehicles. The model of the VD provides motion information that is complementary to the INS and, consequently, the fusion of both systems allows for a comprehensive improvement of the overall navigation system performance. In this brief, the specific VD equations of motion are directly embedded in an extended Kalman filter, as opposed to classical external vehicle models that act as secondary INSs. A tightly-coupled inverted ultrashort baseline is also adopted to enhance position and attitude estimation using measurements of relative position of a transponder located in the vehicle mission area. The improvement of the overall navigation system is assessed in simulation using a nonlinear model of the INFANTE autonomous underwater vehicle, resorting to extensive Monte Carlo runs that implement perturbed versions of the nominal dynamics. The results show that the vehicle dynamics produce relevant performance enhancements, and that the accuracy of the system is robust to modeling uncertainties.