Attitude and velocity estimation of a projectile using low cost magnetometers and accelerometers
Christophe Combettes, French-German Research Institute of Saint-Louis (ISL), France
This paper concerns the description and the validation of a projectile navigation algorithm. Attitude and velocity estimation are performed by a six-degree of freedom projectile modeling combined with MEMS accelerometers and magnetometers into three nonlinear observers. The present algorithm provide angular velocity, attitude, center of gravity linear acceleration and linear velocity. Then, the Attitude/Velocity algorithm is tested and validated in both simulation and experimental setup.
Low cost projectile navigation is a challenging issue due to the critical constraints of a free flight: high accelerations and velocities make the use of gyro difficult even impossible along the principle axis for high spinning projectiles. In order to give an idea of these constraints, linear velocity can reach 1200m/s, linear acceleration is at the beginning around 100 000m/s² and the angular velocity can reach 60 000 °/s according to the projectile type.
Even if sensors must be calibrated, three-axis accelerometers and magnetometers are sufficient to provide linear acceleration, angular rate, and attitude (velocity partially). For an outside free flight, the local magnetic field measured from magnetometers is the local earth magnetic field projected into the body frame. This measurement compare to the knowledge of the direction of the local Earth magnetic field is not sufficient to resolve the Wahba’s problem. But by adding some informations such as projectile characteristics and initial pitch/yaw conditions, the roll angle and the axial angular rate are observables from the radial magnetometers. Then axial magnetometer gives an information about the projectile pitching. Accelerometers give specific forces combined with accelerations issued from non-centralized accelerometers positions with respect to the center of gravity. Specific forces are depending on incidence angles, and the training acceleration is depending on the angular rate. Then the accelerometers combined with a six-degree of freedom projectile model make the incidence and the radial angular rate observables.
Description of the algorithm
Before algorithmic part a calibration phase is needed to compensate the sensors errors. Accelerometers errors are modelled with a bias and a matrix which contain respectively offset and, non-orthogonalities, scale factor, misalignment between sensors and body frames. The calibration is performed with a test bed. For magnetometers, errors such as soft and bias iron are estimated too, thanks to a Helmholtz coil.
The algorithm is divided into three nonlinear observers, which are three extended Kalman filters, Figure 1:
- A first observer to estimate the roll angle and the axial angular rate. The axial angular evolution is given by the projectile modelling of rolling moments. The axial angular rate is approximately the roll derivative. These considerations construct the propagation model. In the flight conditions, the local magnetic field is the Earth’s magnetic field. Then if the pitch and the yaw angular positions are known, the roll position can be estimated without precise initialization. The axial angular rate is observable as soon as the local magnetic field is constant. The estimation of these two values is crucial to get a navigation system.
- A second observer is designed to compute the incidence angles and their derivatives. The incidence angles describe the orientation of the projectile velocity (with respect to the airflow) with respect to the body frame. In other words, the incidence angles give the direction of the linear acceleration and the velocity of the projectile. However, equations which describe the velocity/angular rate temporal evolution, named respectively Newton and Euler equations, are nonlinear. In this observer a linearization, with the assumption that incidence angles remain close too few degrees, simplify the projectile model. Finally incidence angles temporal evolution is described by a 2nd order differential equations system. The Radial accelerometers measurements combined with the 6dof (degree of freedom) and the knowledge of the lever arm, give an observations of the incidence angles. Then the radial angular rates are directly estimated from incidence angles.
- The third observer is based on the axial magnetometer in order to estimate the pitch and yaw angles. As like as the roll observer, this observer is based on the earth magnetic field measurement. The temporal evolution of pitch/yaw is given by the kinematics equations of the rotation, with angular rates as system entries.
With the three described observers, the attitude, the angular rate and the incidence angles are estimated. These values combined with the 6-dof projectile model give an estimation of the velocity and finally a position.
Experiments and simulations:
The algorithm is first validated on simulations. A reliable simulation is used to compute reference trajectory from a complete initialization. The algorithm performed the different trajectories such as angular rates, incidence angles…, from accelerometers/magnetometers measurements, a minimum of initial features and a generic 6 dof model.
Secondly, the algorithm is tested with real measurements from free flight tests. In this case, the validation become trickier because there are no direct measurements of the estimated values. However, in some cases the validation is achieved with the embedded gyros: the angular rates estimated are compared with the gyros measurements and the match is very good, Figure 2.
What is new? :
A generic algorithm is developed to perform the navigation function for an instrumented projectile. This algorithm is designed in order to achieve a real time/embedded computation. This algorithm uses a minimum of low cost / small-size sensors informations from a 3-axis accelerometer and magnetometer. A new simplified incidence model was developed and validated with simulations and experiments.