**Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System' Errors due to the Inertial Sensors**

Teodor Lucian Grigorie and Ruxandra Mihaela Botez

Additional information is available at the end of the chapter

http://dx.doi.org/10.5772/57583

### **1. Introduction**

Inertial navigation is a dead reckoning positioning method based on the measurement and mathematical processing of the vehicle absolute acceleration and angular speed in order to estimate its attitude, speed and position related to different reference. Due to the specific operation principle, the positioning errors for this method result from the imperfection of the initial conditions knowledge, from the errors due to the numerical calculation in the inertial system, and from the accelerometers and gyros errors. Therefore, the inertial sensors perform‐ ances play a main role in the establishment of the navigation system precision, and should be considered in its design phase frames (Bekir, 2007; Farrell, 2008; Grewal et al., 2013; Grigorie, 2007; Salychev, 1998; Titterton and Weston, 2004).

Amazing evolution of physics and manufacturing technologies to improve the optical an electronic fields have made possible the development of opto-electronic rotation and transla‐ tion sensors in parallel with the mechanical sensors. The Ring Laser Gyros (RLG) have entered the market only in 1980's even if in 1963 was first demonstrated in a square configuration. Mechanical gyroscopes dominated the market and the RLG were required in military appli‐ cations, because these are ideal systems for high dynamics strap-down inertial navigation, used in extreme environments. The RLG has excellent scale-factor stability and linearity, negligible sensitivity to acceleration, digital output, fast turn-on, excellent stability and repeatability across the range, and no moving parts. Present day RLG's (Ring Laser Gyros) is considered a matured technology and its development efforts are to reduce costs more than

© 2014 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

to increase its performance (Barbour & Schmidt, 2001; Barbour et al., 2010; Barbour, 2010; Edu et al., 2011; Hopkins, 2010; Kraft, 2000; Lawrence, 1998).

acceleration and rotation sensors, used especially in the inertial navigation systems. The use of such miniaturized sensors creates the premises to have redundant strap-down inertial navigation systems through the miscellaneous dedicated architectures and at the low-costs comparatively with the case of non-miniaturized and very precise inertial sensors use. On the other way, the use of these miniaturization technologies for the inertial sensors allows the implementation of the entire inertial navigation system in a single chip, including here the sensors and all circuits for the signals conditioning (Bose, 2008; Grewal et al., 2013; Grigorie,

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

From the other point of view, these miniaturized sensors have some disadvantages due to the performances decrease with the miniaturization degree increase. They are quite noises, because at the great majority of the acceleration sensors the noise density is between 100 μg/Hz1/2 and a few hundreds of μg/Hz1/2, for the bandwidths between 100Hz and 2500Hz,

bandwidths between 50Hz and 100Hz. Also, for the same type of sensors the noise density can vary from one sensor to the other with 20% of the catalogue value. The filtering of the noise it is not recommended because it is possible to be altered the useful signal and, so, the sensor output doesn't reflect exactly the signal applied at the input of the sensor. Beside the noise increase, through miniaturization appear negative influences on the stability and value of bias, on the scale factor calibration, on the cross-axis sensitivity for the accelerom‐ eters and on the sensitivity at the accelerations applied along any given axis for the gyros. For all of these the data sheets of the MEMS and NEMS products stipulate maximal values relatively high, without be able to specify exactly their value to be corrected (Grigorie et

To test the influences of the sensors errors on the solution of navigation of strap-down inertial navigators we realized Matlab/Simulink models for the acceleration and rotation sensors based on the sensors data sheets and on the IEEE equivalent models for the inertial sensors (Grigorie et al., 2010 a; Grigorie et al., 2010 b). For example, for the accelerometers was obtained the

the cross-axis acceleration *ac*, and as output the perturbed acceleration *a* (Grigorie et al., 2010

n

*N* is sensitivity axis misalignment (in radians), *B*-bias (expressed in percent of span), *kc*-crossaxis sensitivity (expressed in percent of *ac*), ν-sensor noise (given by its density ν*<sup>d</sup>* expressed in μg/Hz1/2, *K-*scale factor (expressed in mV/*g*), and Δ*K-*scale factor error (percents of *K*), and

covers theirs main errors: bias, scale factor error, sensitivity axis misalignment, cross axis

. The model was built for few miniaturized acceleration sensors and

( )(1 / ); *i i cc a a Na B k a K K* = + + + + +D

/s)/Hz1/2 and 0.1 (o

/s)/Hz1/2, for the pass

http://dx.doi.org/10.5772/57583

307

, applied along of the sensitive axis, and

(1)

2006; Grigorie et al., 2012 a; Titterton and Weston, 2004).

and at the gyro sensors it is between 0.001 (o

model in Figure 1. He has as inputs the acceleration *ai*

al., 2010 a; Grigorie et al., 2010 b).

a). The analytic form of the model is:

, *ac* expressed in m/s2

sensitivity and noise.

*a*, *ai*

Fiber optic gyros (FOG) are also a mature technology and were originally designed as a lowcost alternative to the RLG. Surprisingly, today they compete RLG's both in terms of manu‐ facturing costs, as well as that of performance, gaining prominence in a series of military and commercial applications. The studies provide that the developments in solid-state optics and fiber technology could lead to 0.001-deg/h performance in miniature design. Research in the field of fiber optic gyros, similarly to those of RLG, aimed at decreasing the size and manu‐ facturing costs at an approximately constant level of performance, if not better. Development of miniaturized FOGs was based on the technology achievements brought by the telecommu‐ nications industry. An important innovation was the discovery of photonic crystal fibers (PCF Fibers crystal photon) that have been a very important step towards the next generation of IFOG instruments, the PC-IFOG. The introduction of PCFs in IFOG applications brings significant advantages to this field, such as the significant reduction of bend losses and fiber size compared to the conventional optical fiber, minimizing the fiber optic coil diameter, the possibility of incorporating a dispersion compensation in the existing PCF, with the effect of reducing the spectral distortion, guiding light through this type of fiber allows the use of a mid-infrared optical wavelength (Barbour & Schmidt, 2001; Divakaruni & Sanders, 2006; Edu et al., 2011; KVH Industries Inc., 2007; Pavlath, 2006; Tawney et al., 2006).

In the 1980s, the Hemispherical Resonant Gyro was developed, a vibratory high performance gyro; the inertial sensing element is a fused-silica hemispherical shell coated with a thin film of metal. HRG advantages are related to the fact that it is very light, compact, operates in vacuum and has no moving parts. Its life cycle is limited only by the electronic components, which are redundant (Barbour & Schmidt, 2001; Barbour, 2010; Edu et al., 2011).

Besides the above mentioned technologies, another technology, very promising in terms of inertial detection, based on atomic interferometry (cold atom inertial sensors) is developing very fast. Atomic interferometry is a sensor-based inertial sensing that uses the atom interfer‐ ometry, using cold atoms, atoms that are a millionth of a degree above absolute zero, created and then trapped using laser technology. With the researches in optical precision spectroscopy (Nobel Prize 2005) today it is possible to have precise control on the internal and external of freedom of atomic matter. Those huge progresses led to application of ultra-cold matter in fields such as precision measurements, matter wave interferometry and applications in quantum information processing. The atom interferometers are very similar in their basic principle with the optical interferometers. The difference is that the optical wave is replaced with the matter-wave represented by the atoms. The current state-of-art of atom interferome‐ try: the atom interferometers obtained and proof-of-concept. Although, gyros and accelerom‐ eters are yet too voluminous, the miniaturization seems feasible in the near future and is developing (Dumke & Mueller, 2010; Edu et al., 2011; Hopkins, 2010; Schmidt, 2010).

The aerospace industry tendencies to realize unmanned aircraft (UAV), micro and nanosatellites, easy to launch in space and with the performances analogous with the actual satellites, imposed a nimble rhythm to the expansion of the NEMS (Nano-Electro-Mechanical-Systems) and MEMS (Micro-Electro-Mechanical-Systems) technologies in the domain of the acceleration and rotation sensors, used especially in the inertial navigation systems. The use of such miniaturized sensors creates the premises to have redundant strap-down inertial navigation systems through the miscellaneous dedicated architectures and at the low-costs comparatively with the case of non-miniaturized and very precise inertial sensors use. On the other way, the use of these miniaturization technologies for the inertial sensors allows the implementation of the entire inertial navigation system in a single chip, including here the sensors and all circuits for the signals conditioning (Bose, 2008; Grewal et al., 2013; Grigorie, 2006; Grigorie et al., 2012 a; Titterton and Weston, 2004).

to increase its performance (Barbour & Schmidt, 2001; Barbour et al., 2010; Barbour, 2010; Edu

Fiber optic gyros (FOG) are also a mature technology and were originally designed as a lowcost alternative to the RLG. Surprisingly, today they compete RLG's both in terms of manu‐ facturing costs, as well as that of performance, gaining prominence in a series of military and commercial applications. The studies provide that the developments in solid-state optics and fiber technology could lead to 0.001-deg/h performance in miniature design. Research in the field of fiber optic gyros, similarly to those of RLG, aimed at decreasing the size and manu‐ facturing costs at an approximately constant level of performance, if not better. Development of miniaturized FOGs was based on the technology achievements brought by the telecommu‐ nications industry. An important innovation was the discovery of photonic crystal fibers (PCF Fibers crystal photon) that have been a very important step towards the next generation of IFOG instruments, the PC-IFOG. The introduction of PCFs in IFOG applications brings significant advantages to this field, such as the significant reduction of bend losses and fiber size compared to the conventional optical fiber, minimizing the fiber optic coil diameter, the possibility of incorporating a dispersion compensation in the existing PCF, with the effect of reducing the spectral distortion, guiding light through this type of fiber allows the use of a mid-infrared optical wavelength (Barbour & Schmidt, 2001; Divakaruni & Sanders, 2006; Edu

et al., 2011; Hopkins, 2010; Kraft, 2000; Lawrence, 1998).

306 MATLAB Applications for the Practical Engineer

et al., 2011; KVH Industries Inc., 2007; Pavlath, 2006; Tawney et al., 2006).

which are redundant (Barbour & Schmidt, 2001; Barbour, 2010; Edu et al., 2011).

developing (Dumke & Mueller, 2010; Edu et al., 2011; Hopkins, 2010; Schmidt, 2010).

The aerospace industry tendencies to realize unmanned aircraft (UAV), micro and nanosatellites, easy to launch in space and with the performances analogous with the actual satellites, imposed a nimble rhythm to the expansion of the NEMS (Nano-Electro-Mechanical-Systems) and MEMS (Micro-Electro-Mechanical-Systems) technologies in the domain of the

In the 1980s, the Hemispherical Resonant Gyro was developed, a vibratory high performance gyro; the inertial sensing element is a fused-silica hemispherical shell coated with a thin film of metal. HRG advantages are related to the fact that it is very light, compact, operates in vacuum and has no moving parts. Its life cycle is limited only by the electronic components,

Besides the above mentioned technologies, another technology, very promising in terms of inertial detection, based on atomic interferometry (cold atom inertial sensors) is developing very fast. Atomic interferometry is a sensor-based inertial sensing that uses the atom interfer‐ ometry, using cold atoms, atoms that are a millionth of a degree above absolute zero, created and then trapped using laser technology. With the researches in optical precision spectroscopy (Nobel Prize 2005) today it is possible to have precise control on the internal and external of freedom of atomic matter. Those huge progresses led to application of ultra-cold matter in fields such as precision measurements, matter wave interferometry and applications in quantum information processing. The atom interferometers are very similar in their basic principle with the optical interferometers. The difference is that the optical wave is replaced with the matter-wave represented by the atoms. The current state-of-art of atom interferome‐ try: the atom interferometers obtained and proof-of-concept. Although, gyros and accelerom‐ eters are yet too voluminous, the miniaturization seems feasible in the near future and is

From the other point of view, these miniaturized sensors have some disadvantages due to the performances decrease with the miniaturization degree increase. They are quite noises, because at the great majority of the acceleration sensors the noise density is between 100 μg/Hz1/2 and a few hundreds of μg/Hz1/2, for the bandwidths between 100Hz and 2500Hz, and at the gyro sensors it is between 0.001 (o /s)/Hz1/2 and 0.1 (o /s)/Hz1/2, for the pass bandwidths between 50Hz and 100Hz. Also, for the same type of sensors the noise density can vary from one sensor to the other with 20% of the catalogue value. The filtering of the noise it is not recommended because it is possible to be altered the useful signal and, so, the sensor output doesn't reflect exactly the signal applied at the input of the sensor. Beside the noise increase, through miniaturization appear negative influences on the stability and value of bias, on the scale factor calibration, on the cross-axis sensitivity for the accelerom‐ eters and on the sensitivity at the accelerations applied along any given axis for the gyros. For all of these the data sheets of the MEMS and NEMS products stipulate maximal values relatively high, without be able to specify exactly their value to be corrected (Grigorie et al., 2010 a; Grigorie et al., 2010 b).

To test the influences of the sensors errors on the solution of navigation of strap-down inertial navigators we realized Matlab/Simulink models for the acceleration and rotation sensors based on the sensors data sheets and on the IEEE equivalent models for the inertial sensors (Grigorie et al., 2010 a; Grigorie et al., 2010 b). For example, for the accelerometers was obtained the model in Figure 1. He has as inputs the acceleration *ai* , applied along of the sensitive axis, and the cross-axis acceleration *ac*, and as output the perturbed acceleration *a* (Grigorie et al., 2010 a). The analytic form of the model is:

$$a = (a\_i + Na\_i + B + k\_c a\_c + \nu)(1 + \Lambda K / K);\tag{1}$$

*N* is sensitivity axis misalignment (in radians), *B*-bias (expressed in percent of span), *kc*-crossaxis sensitivity (expressed in percent of *ac*), ν-sensor noise (given by its density ν*<sup>d</sup>* expressed in μg/Hz1/2, *K-*scale factor (expressed in mV/*g*), and Δ*K-*scale factor error (percents of *K*), and *a*, *ai* , *ac* expressed in m/s2 . The model was built for few miniaturized acceleration sensors and covers theirs main errors: bias, scale factor error, sensitivity axis misalignment, cross axis sensitivity and noise.

**Figure 1.** Accelerometers Matlab/Simulink model and its interface.

The model related to the gyro sensors was implemented in Matlab/Simulink (Figure 2) starting from the equation (Grigorie et al., 2010 b):

$$\alpha o = (\alpha\_i + \mathbf{S} \cdot \mathbf{a}\_r + \mathbf{B} + \nu)(1 + \Lambda \mathbf{K} / \, \mathbf{K});\tag{2}$$

coordinates (latitude, longitude and altitude). For the presented algorithm is developed an error model that highlights the dependencies of the vehicle positioning, velocity and attitude errors by the strap-down inertial sensor errors used to detect acceleration and angular speed. In the development of the error model the small perturbation technique is used. Following is conducted a study of the dependence of the inertial navigator outputs errors by the errors of the used inertial sensors based on the Matlab/Simulink models built for acceleration and gyro

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

<sup>→</sup> of an accelerometer is influenced by the gravitational field, it being a combination

*rv r*

<sup>r</sup> r rr <sup>r</sup> r r <sup>r</sup> <sup>r</sup> r rr r r (4)

In literature, *f* is very well known as specific force (Farrell, 2008; Titterton and Weston, 2004).

d d , d d *I P*

= + W´ = + W´

( ) d d <sup>d</sup> d dd ; dd d *<sup>I</sup>* d dd *<sup>I</sup> I II <sup>I</sup>*

*a v r v r*

= = + W´ = + W´ = + W´ + W´ W´ ê ú é ù ë û ê ú ë û

<sup>→</sup> and the gravitational acceleration*g*

r r r r rr r (3)

<sup>→</sup> , i.e. *f* <sup>→</sup> <sup>=</sup>*<sup>a</sup>* <sup>→</sup> − *g* → .

http://dx.doi.org/10.5772/57583

309

sensors.

**Figure 2.** Gyros Matlab/Simulink model and its interface.

between the vehicle kinematic acceleration *a*

On the other way, according to the Coriolis formula we have:

*r r*

*t t*

*r v rv*

*tt t t tt*

**2. Navigation algorithm**

The output *f*

from where it results:

é ù

ω-sensors output angular speed (disturbed signal) expressed in <sup>o</sup> /s, ω*<sup>i</sup>* -applied angular speed ( o /s), *S*-sensitivity to the acceleration *ar* applied on an arbitrary direction ((<sup>o</sup> /s)/*g*), *B*-bias (expressed in percents of span), ν-sensor noise (given by its density ν*d* expressed in (<sup>o</sup> /s)/Hz1/2), *K-*scale factor (expressed in mV/(<sup>o</sup> /s)), Δ*K-*scale factor error (percents of *K*).

For both models, the change of the sensor type that will be used in simulations is made using the associated interfaces. In addition, the interfaces allow the setting of the models, by the user, in custom variants. The models have the advantages to work independent with each of the sensor errors and to study in this way their influence on the inertial navigator positioning solution. Although sensors data sheets specifications are not related to the components of noise, for a more detailed study of the navigators' errors, the sensors' models can be completed with some noise terms starting from theirs Allan variance definitions. Allan's variance results are related to the seven noise terms. Five noise terms are basic terms: angle random walk, rate random walk, bias instability, quantization noise and drift rate ramp, while the other two are the sinusoidal noise and exponentially correlated (Markov) noise (Grigorie et al., 2010 c; Grigorie et al., 2012 b).

This chapter deals with solving of a navigation problem relative to terrestrial non-inertial reference frames by using attitude matrices to calculate the vehicle attitude. Once it is high‐ lighted the general equation of inertial navigation, a numerical algorithm is developed for determining the position and speed of the vehicle based on this equation. The algorithm provides position and vehicle speed in horizontal local reference frame (ENU) and its global coordinates (latitude, longitude and altitude). For the presented algorithm is developed an error model that highlights the dependencies of the vehicle positioning, velocity and attitude errors by the strap-down inertial sensor errors used to detect acceleration and angular speed. In the development of the error model the small perturbation technique is used. Following is conducted a study of the dependence of the inertial navigator outputs errors by the errors of the used inertial sensors based on the Matlab/Simulink models built for acceleration and gyro sensors.

**Figure 2.** Gyros Matlab/Simulink model and its interface.

#### **2. Navigation algorithm**

**Figure 1.** Accelerometers Matlab/Simulink model and its interface.

ww

ω-sensors output angular speed (disturbed signal) expressed in <sup>o</sup>

from the equation (Grigorie et al., 2010 b):

308 MATLAB Applications for the Practical Engineer

*K-*scale factor (expressed in mV/(<sup>o</sup>

Grigorie et al., 2012 b).

( o

The model related to the gyro sensors was implemented in Matlab/Simulink (Figure 2) starting

( )(1 / ); *i r*

/s), *S*-sensitivity to the acceleration *ar* applied on an arbitrary direction ((<sup>o</sup>

(expressed in percents of span), ν-sensor noise (given by its density ν*d* expressed in (<sup>o</sup>

 n

For both models, the change of the sensor type that will be used in simulations is made using the associated interfaces. In addition, the interfaces allow the setting of the models, by the user, in custom variants. The models have the advantages to work independent with each of the sensor errors and to study in this way their influence on the inertial navigator positioning solution. Although sensors data sheets specifications are not related to the components of noise, for a more detailed study of the navigators' errors, the sensors' models can be completed with some noise terms starting from theirs Allan variance definitions. Allan's variance results are related to the seven noise terms. Five noise terms are basic terms: angle random walk, rate random walk, bias instability, quantization noise and drift rate ramp, while the other two are the sinusoidal noise and exponentially correlated (Markov) noise (Grigorie et al., 2010 c;

This chapter deals with solving of a navigation problem relative to terrestrial non-inertial reference frames by using attitude matrices to calculate the vehicle attitude. Once it is high‐ lighted the general equation of inertial navigation, a numerical algorithm is developed for determining the position and speed of the vehicle based on this equation. The algorithm provides position and vehicle speed in horizontal local reference frame (ENU) and its global

= + × + + +D *Sa B K K* (2)

/s)), Δ*K-*scale factor error (percents of *K*).

/s, ω*<sup>i</sup>*


/s)/*g*), *B*-bias

/s)/Hz1/2),

The output *f* <sup>→</sup> of an accelerometer is influenced by the gravitational field, it being a combination between the vehicle kinematic acceleration *a* <sup>→</sup> and the gravitational acceleration*g* <sup>→</sup> , i.e. *f* <sup>→</sup> <sup>=</sup>*<sup>a</sup>* <sup>→</sup> − *g* → . In literature, *f* is very well known as specific force (Farrell, 2008; Titterton and Weston, 2004).

On the other way, according to the Coriolis formula we have:

$$\left. \frac{\mathbf{d} \, \vec{r}}{\mathbf{d}t} \right|\_{\mathbf{l}} = \left. \frac{\mathbf{d} \, \vec{r}}{\mathbf{d}t} \right|\_{\mathbf{l}} + \vec{\Omega} \times \vec{r} = \vec{v} + \vec{\Omega} \times \vec{r} \,\tag{3}$$

from where it results:

$$\vec{a} = \frac{\mathbf{d}}{\mathbf{d}t} \left[ \frac{\mathbf{d}\vec{r}}{\mathbf{d}t} \bigg|\_{I} \right]\_{I} = \frac{\mathbf{d}}{\mathbf{d}t} \left[ \vec{v} + \vec{\Omega} \times \vec{r} \right]\_{I} = \frac{\mathbf{d}\vec{v}}{\mathbf{d}t} \bigg|\_{I} + \vec{\Omega} \times \frac{\mathbf{d}\vec{r}}{\mathbf{d}t} \bigg|\_{I} = \frac{\mathbf{d}\vec{v}}{\mathbf{d}t} \bigg|\_{I} + \vec{\Omega} \times \vec{v} + \vec{\Omega} \times \left( \vec{\Omega} \times \vec{r} \right); \tag{4}$$

*r* <sup>→</sup> is the position vector of the monitored vehicle in inertial frame *I*, *v* <sup>→</sup> -the vehicle speed relative to the ECEF (Earth Centered Earth Fixed) reference frame (denoted with *P*), and *Ω* <sup>→</sup>-Earth rotation speed around its axis.

Denoting with *ω* → *<sup>N</sup>* the absolute angular speed of the navigation reference frame *N*, then the Coriolis formula applied to the (d *v* <sup>→</sup> / d *t*)| *<sup>I</sup>* term implies:

$$\left. \frac{\mathbf{d} \ \vec{v}}{\mathbf{d} \mathbf{t}} \right|\_{\mathbf{l}} = \frac{\mathbf{d} \ \vec{v}}{\mathbf{d} \ \mathbf{t}} \Bigg|\_{\mathbf{N}} + \vec{o}\_{\mathbf{N}} \times \vec{v}\_{\mathbf{v}} \tag{5}$$

frame is made by using the rotation matrix describing the vehicle attitude relative to the

By choosing as navigation frame the local horizontal frame ENU (East-North-Up) it results

<sup>d</sup> , <sup>d</sup>

*xl yl zl zl yl yl zl zl yl axl*

*<sup>v</sup> f v v v vg <sup>t</sup>*

= + - +W -W -

 w

 w

 w

components of the vehicle speed relative to ECEF frame in ENU frame; *ωxl*

*yl xl zl zl xl xl zl zl xl ayl*

*f v v v vg <sup>t</sup>*

= - + -W +W -

*<sup>v</sup> f v v v vg <sup>t</sup>*

= + - +W -W -

*zl xl yl yl xl xl yl yl xl azl*

2 6 0, 0, 9,7803 0,0519 sin 3,08 10 . *xl yl zl ggg*

é ù W =W W W = W W é ù é ù

ë û ë û ë û

[, , ] , cos , tg sin ,

*h* is the altitude relative to the reference ellipsoid, *Rϕ*şi *Rλ*-principal radii of curvature of the

2 2 3/2 2 2 1/2 <sup>1</sup> , , (1 sin ) (1 sin )

 l

*v v v R hR h R h*

<sup>r</sup> (12)

*T yl xl xl*

 l

é ù é ù ë û <sup>=</sup> = -ê ú + W + W

With these considerations we have (Farrell, 2008; Grigorie, 2007; Radix, 1993):

*xl*

w

w

w

, *gazl*

*xl yl zl l*

2

*e a R a <sup>R</sup> e e*

reference ellipsoid (Farrell, 2008; Grigorie, 2007; Radix, 1993):

*yl*

*v*

*zl*

nents of the ENU frame absolute angular speed *ω*

, *gayl*

ENU frame (Farrell, 2008; Grigorie, 2007; Radix, 1993):

<sup>d</sup> , <sup>d</sup>

<sup>d</sup> , <sup>d</sup>

are the components of the specific force in ENU frame; *vxl*

on its own axes; *Ωxl*

*h* - @ @ @ + × -×× (10)

> 0 cos sin , *T T*

> > l

 


++ + ë û

<sup>r</sup> (11)


 

→ *l*

(index *l* denotes the ENU frame). The scalar components of the eq. (8) in this frame

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

(9)

311

, *vyl* , *vzl* -



, *ωzl*

, *ωyl*

http://dx.doi.org/10.5772/57583

, *Ωzl*

*T*

, *Ωyl*

navigation frame.

*ω* → *<sup>N</sup>* =*ω* → *l*

are:

where *f xl*

of *Ω*

, *f yl* , *f zl*

<sup>→</sup> in ENU frame; *gaxl*

*l xl yl zl l*

www

w

where (d *v* <sup>→</sup> / d *t*)*<sup>N</sup>* is the derivative of *v* <sup>→</sup> relative to the navigation frame. Therefore, the accel‐ eration *a* <sup>→</sup> becomes:

$$\vec{a} = \frac{\mathbf{d} \cdot \vec{v}}{\mathbf{d}t}\bigg|\_{N} + \vec{o}\_{N} \times \vec{v} + \vec{\Omega} \times \vec{v} + \vec{\Omega} \times \left(\vec{\Omega} \times \vec{r}\right) \tag{6}$$

and the specific force can be rewritten as:

$$\vec{f} = \frac{\mathbf{d} \, \vec{v}}{\mathbf{d}t}\bigg|\_{N} + \vec{o}\_{N} \times \vec{v} + \vec{\Omega} \times \vec{v} + \vec{\Omega} \times \left(\vec{\Omega} \times \vec{r}\right) - \vec{g}.\tag{7}$$

Considering the expression *g* → *<sup>a</sup>* = *g* <sup>→</sup> −*Ω* <sup>→</sup> ×(*<sup>Ω</sup>* <sup>→</sup> <sup>×</sup>*<sup>r</sup>* <sup>→</sup> ) for the apparent gravitational acceleration, eq. (7) implies:

$$\vec{f} = \frac{\mathbf{d} \cdot \vec{v}}{\mathbf{d}t}\bigg|\_{N} + \vec{a}\vec{o}\_{N} \times \vec{v} + \vec{\Omega} \times \vec{v} - \vec{g}\_{a'} \tag{8}$$

which is known as general equation of the inertial navigation.

The position and the speed of a vehicle may be obtained by the numerical integration of the eq. (8) relative to the navigation frame (Farrell, 2008; Salychev, 1998; Titterton and Weston, 2004). In the inertial navigation systems with stable platform, the axes of the acceleration sensors are kept parallel with the navigation frame axes, and, as a consequence, the compo‐ nents of the specific force are obtained directly in this frame. If a strap-down architecture is used for the inertial measurement unit (IMU), then the components of the specific force in the navigation frame should be calculated starting from the specific force components in the vehicle frame (SV); the acceleration sensors in IMU are fixed directly on the vehicle rigid structure. In this situation the coordinate change between the vehicle frame and navigation frame is made by using the rotation matrix describing the vehicle attitude relative to the navigation frame.

*r*

rotation speed around its axis.

310 MATLAB Applications for the Practical Engineer

<sup>→</sup> becomes:

Considering the expression *g*

→

Coriolis formula applied to the (d *v*

<sup>→</sup> / d *t*)*<sup>N</sup>* is the derivative of *v*

and the specific force can be rewritten as:

Denoting with *ω*

where (d *v*

(7) implies:

eration *a*

<sup>→</sup> is the position vector of the monitored vehicle in inertial frame *I*, *v*

to the ECEF (Earth Centered Earth Fixed) reference frame (denoted with *P*), and *Ω*

<sup>→</sup> / d *t*)| *<sup>I</sup>* term implies:

d d , d d *<sup>N</sup> I N*

= +´ w

( ) <sup>d</sup> , <sup>d</sup> *<sup>N</sup>*

( ) <sup>d</sup> . <sup>d</sup> *<sup>N</sup>*

<sup>d</sup> , <sup>d</sup> *N a*

The position and the speed of a vehicle may be obtained by the numerical integration of the eq. (8) relative to the navigation frame (Farrell, 2008; Salychev, 1998; Titterton and Weston, 2004). In the inertial navigation systems with stable platform, the axes of the acceleration sensors are kept parallel with the navigation frame axes, and, as a consequence, the compo‐ nents of the specific force are obtained directly in this frame. If a strap-down architecture is used for the inertial measurement unit (IMU), then the components of the specific force in the navigation frame should be calculated starting from the specific force components in the vehicle frame (SV); the acceleration sensors in IMU are fixed directly on the vehicle rigid structure. In this situation the coordinate change between the vehicle frame and navigation

*<sup>v</sup> <sup>f</sup> v vg <sup>t</sup>* = + ´ + W´ w

<sup>r</sup> <sup>r</sup> r r rr <sup>r</sup>

*<sup>v</sup> <sup>f</sup> v v rg <sup>t</sup>* = + ´ + W´ + W´ W´ -

<sup>r</sup> <sup>r</sup> r r r rr r rr

*a vv r*

= + ´ + W´ + W´ W´

<sup>r</sup> r rr r r r rr

*v v*

*t t*

*N*

w

*v*

*t*

*N*

which is known as general equation of the inertial navigation.

→ *<sup>a</sup>* = *g* <sup>→</sup> −*Ω* <sup>→</sup> ×(*<sup>Ω</sup>* <sup>→</sup> <sup>×</sup>*<sup>r</sup>*

w

*N*

*<sup>N</sup>* the absolute angular speed of the navigation reference frame *N*, then the

r r r r (5)

<sup>→</sup> relative to the navigation frame. Therefore, the accel‐

<sup>→</sup> ) for the apparent gravitational acceleration, eq.

*v*

<sup>→</sup> -the vehicle speed relative

<sup>→</sup>-Earth

(6)

(7)

(8)

By choosing as navigation frame the local horizontal frame ENU (East-North-Up) it results *ω* → *<sup>N</sup>* =*ω* → *l* (index *l* denotes the ENU frame). The scalar components of the eq. (8) in this frame are:

$$f\_{xl} = \frac{\mathbf{d}\upsilon\_{xl}}{\mathbf{d}t} + \alpha\_{yl}\upsilon\_{zl} - \alpha\_{zl}\upsilon\_{yl} + \Omega\_{yl}\upsilon\_{zl} - \Omega\_{zl}\upsilon\_{yl} - \mathbf{g}\_{axl}\prime$$

$$f\_{yl} = \frac{\mathbf{d}\upsilon\_{yl}}{\mathbf{d}t} - \alpha\_{xl}\upsilon\_{zl} + \alpha\_{zl}\upsilon\_{xl} - \Omega\_{xl}\upsilon\_{zl} + \Omega\_{zl}\upsilon\_{xl} - \mathbf{g}\_{ayl}\prime \tag{9}$$

$$f\_{zl} = \frac{\mathbf{d}\upsilon\_{zl}}{\mathbf{d}t} + \alpha\_{xl}\upsilon\_{yl} - \alpha\_{yl}\upsilon\_{xl} + \Omega\_{xl}\upsilon\_{yl} - \Omega\_{yl}\upsilon\_{xl} - \mathbf{g}\_{axl}\prime$$

where *f xl* , *f yl* , *f zl* are the components of the specific force in ENU frame; *vxl* , *vyl* , *vzl* components of the vehicle speed relative to ECEF frame in ENU frame; *ωxl* , *ωyl* , *ωzl* -compo‐ nents of the ENU frame absolute angular speed *ω* → *l* on its own axes; *Ωxl* , *Ωyl* , *Ωzl* -components of *Ω* <sup>→</sup> in ENU frame; *gaxl* , *gayl* , *gazl* -components of the apparent gravitational acceleration in ENU frame (Farrell, 2008; Grigorie, 2007; Radix, 1993):

$$\mathbf{g}\_{\rm xl} \equiv \mathbf{0}, \; \mathbf{g}\_{\rm yl} \equiv \mathbf{0}, \; \mathbf{g}\_{\rm zl} \equiv \mathbf{9}, 780 \mathbf{3} + \mathbf{0}, 0519 \cdot \sin^2 \phi - \mathbf{3}, 08 \cdot 10^{-6} \cdot h. \tag{10}$$

With these considerations we have (Farrell, 2008; Grigorie, 2007; Radix, 1993):

$$\mathbb{E}\left[\bar{\boldsymbol{\Omega}}\right]\_l = \begin{bmatrix} \boldsymbol{\Omega}\_{\boldsymbol{z}l} & \boldsymbol{\Omega}\_{\boldsymbol{y}l} & \boldsymbol{\Omega}\_{\boldsymbol{z}l} \end{bmatrix}^T = \begin{bmatrix} 0 & \boldsymbol{\Omega}\cos\phi & \boldsymbol{\Omega}\sin\phi \end{bmatrix}^T,\tag{11}$$

$$\begin{aligned} \left[\begin{bmatrix} \vec{\alpha} \\ \end{bmatrix} \right]\_l &= \begin{bmatrix} \alpha \boldsymbol{\nu}\_{\boldsymbol{x}\boldsymbol{l}\boldsymbol{\prime}} & \alpha \boldsymbol{\nu}\_{\boldsymbol{y}\boldsymbol{l}\boldsymbol{\prime}} & \alpha \boldsymbol{\underline{\boldsymbol{z}}} \end{bmatrix}^T = \begin{bmatrix} -\frac{\boldsymbol{\mathcal{V}}\_{\boldsymbol{y}\boldsymbol{l}}}{R\_{\boldsymbol{\phi}} + \boldsymbol{h}} & \frac{\boldsymbol{\mathcal{V}}\_{\boldsymbol{x}\boldsymbol{l}}}{R\_{\boldsymbol{\lambda}} + \boldsymbol{h}} + \Omega \cos\phi & \frac{\boldsymbol{\mathcal{V}}\_{\boldsymbol{x}\boldsymbol{l}}}{R\_{\boldsymbol{\lambda}} + \boldsymbol{h}} \text{tg}\boldsymbol{\phi} + \Omega \sin\phi \end{bmatrix}^T \end{aligned} \tag{12}$$

*h* is the altitude relative to the reference ellipsoid, *Rϕ*şi *Rλ*-principal radii of curvature of the reference ellipsoid (Farrell, 2008; Grigorie, 2007; Radix, 1993):

$$R\_{\phi} = a \frac{1 - e^2}{(1 - e^2 \sin^2 \phi)^{3/2}}, \ R\_{\lambda} = \frac{a}{(1 - e^2 \sin^2 \phi)^{1/2}},\tag{13}$$

*λ* and *ϕ* are the longitude and the latitude. The angular speed*ω* → *<sup>r</sup>*, relative to the ECEF reference frame, has in ENU frame the next components:

$$\left[\begin{array}{cc} \overrightarrow{\boldsymbol{\alpha}}\_{r} \end{array}\right]\_{l} = \left[\begin{array}{cc} \boldsymbol{\alpha}\_{r\boldsymbol{x}l'} & \boldsymbol{\alpha}\_{rgl'} & \boldsymbol{\alpha}\_{r\boldsymbol{z}l} \end{array}\right]^{T} = \left[\begin{array}{cc} \boldsymbol{v}\_{yl} \\ \boldsymbol{R}\_{\phi} + \boldsymbol{h} \end{array}\right, \ \frac{\boldsymbol{v}\_{\boldsymbol{x}l}}{\boldsymbol{R}\_{\lambda} + \boldsymbol{h}}, \ \frac{\boldsymbol{v}\_{\boldsymbol{x}l}}{\boldsymbol{R}\_{\lambda} + \boldsymbol{h}} \mathbf{t} \mathbf{g} \boldsymbol{\phi} \right]^{T}. \tag{14}$$

0 0

 ww

 w

w

 w

Can be easily observed that eq. (19) has the general form:

w

the angular speeds *ωxv*, *ωyv*, *ωzv* and *ωxl*

 ww

> ww

*Δϕxv*, *Δϕyv*, *Δϕzv*, respectively *Δϕxl*

with *A*=*<sup>ω</sup>*˜ *<sup>v</sup>* and*<sup>B</sup>* <sup>=</sup>*<sup>ω</sup>*˜*<sup>l</sup>*

from where it is obtained:

and

with:

 w

w

*v zv xv l zl xl yv xv yl xl*

, *ωyl*

d, d, d

ww

*xv xv xv yv yv yv zv zv zv ttt*

111

+++

*nnn*

*ttt*

 

> 

value provided for the *X* matrix at the *tn*+1 time is given by:

*A B* 

> 

*nnn*

111

+++

 ww

*xl xl xl yl yl yl zl zl zl ttt*

*nnn*

*ttt*

*nnn*

, *Δϕyl*

1 0

 

*n zv xv n zl xl yv xv yl xl*

= Dê -D = D ú ê -D <sup>ú</sup> <sup>ê</sup> ú ê <sup>ú</sup> -D D -D D êë ú ê û ë úû

<sup>é</sup> -D D ù é -D D <sup>ù</sup> <sup>ê</sup> ú ê <sup>ú</sup>

, *ωzl*

*tt tt tt*

d, d, d.

measured around the roll, pitch and yaw axes, respectively the increments of the angular rotations around the ENU frame axes calculated by the navigation processor. In this way, the

*tt tt tt*

D = = DD = = DD = = D òòò (23)

D = = DD = = DD = = D òòò (22)

<sup>é</sup> - - ùé ù <sup>ê</sup> úê ú = -= - <sup>ê</sup>

ê- - êë ûë û

0 , 0. 0 0

 w

w

w

 w

. Considering that for a short period of time*Δt*, between *tn* and *tn*+1 times,

 

 

<sup>1</sup> , *X X X t X X A t B tX n nn nn n* <sup>+</sup> = + D= + D- D & (24)

<sup>1</sup> ( ) , *X X I A t B tX X A B X n n* <sup>+</sup> *n nn nn* = + D-D = - (25)

   

> 

(26)

1 , 0. 1 0

*zv yv zl yl*

% % (20)

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

 w http://dx.doi.org/10.5772/57583

313

 w

*X XA BX* = - , & (21)

are constant, we obtains:

 ww

 ww

, *Δϕzl* are the increments of the angular rotations

*zv yv zl yl*

Therefore, equations (9) become:

$$\begin{split} \frac{\mathbf{d} \, v\_{xl}}{\mathbf{d}t} &= f\_{xl} + \frac{v\_{xl} v\_{yl}}{R\_{\lambda} + h} \mathbf{t} \, \boldsymbol{\phi} + 2\Omega \sin \phi v\_{yl} - v\_{zl} \left( \frac{v\_{xl}}{R\_{\lambda} + h} + 2\Omega \cos \phi \right) + \mathbf{g}\_{axl}, \\ \frac{\mathbf{d} \, v\_{yl}}{\mathbf{d}t} &= f\_{yl} - \frac{v\_{xl}^2}{R\_{\lambda} + h} \mathbf{t} \, \boldsymbol{\phi} - 2\Omega \sin \phi v\_{xl} - v\_{zl} \frac{v\_{yl}}{R\_{\phi} + h} + \mathbf{g}\_{ayl}, \\ \frac{\mathbf{d} \, v\_{zl}}{\mathbf{d}t} &= f\_{zl} + \frac{v\_{yl}^2}{R\_{\phi} + h} + \frac{v\_{zl}^2}{R\_{\lambda} + h} + 2\Omega v\_{zl} \cos \phi + \mathbf{g}\_{axl}. \end{split} \tag{15}$$

To integrate these equations we need to know the initial values of *ϕ*, *λ*, *h* , *vxl* , *vyl* , *vzl* , and, also, the components of *f* <sup>→</sup> and *<sup>g</sup>* <sup>→</sup> in ENU frame. Because the IMU of the strap-down inertial navigation system contains three accelerometers and three gyros, its inputs will be the components of the vehicle absolute acceleration and angular speed in the vehicle frame:

$$\left\|\vec{f}\right\|\_{v} = \left\|f\_{xv'} \left|f\_{yv'} \left|f\_{zv}\right|\right|^{T}\right\} \tag{16}$$

$$\begin{bmatrix} \vec{\alpha\_v} \end{bmatrix}\_v = \begin{bmatrix} \alpha\_{xv'} & \alpha\_{yv'} & \alpha\_{zv} \end{bmatrix}^T. \tag{17}$$

The components of the specific force in ENU can be determinate by using the relation:

$$\|\vec{f}\|\_{l} = \mathcal{R}\_{v}^{l} \|\vec{f}\|\_{v} \tag{18}$$

where *Rv l* is the rotation matrix performing the coordinate change between SV frame and ENU frame and can be calculated by solving the next Poisson equation (Farrell, 2008):

$$
\dot{\mathcal{R}}\_v^l = \mathcal{R}\_v^l \tilde{\boldsymbol{\phi}}\_v - \tilde{\boldsymbol{\phi}}\_l \mathcal{R}\_v^l. \tag{19}
$$

In eq. (19) *<sup>ω</sup>*˜ *<sup>v</sup>*and *<sup>ω</sup>*˜*<sup>l</sup>* have the expressions: Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 313

$$
\tilde{o}\_v = \begin{bmatrix}
0 & -o\_{zv} & o\_{yv} \\
o\_{zv} & 0 & -o\_{xv} \\
\end{bmatrix},
\tilde{o}\_l = \begin{bmatrix}
0 & -o\_{zl} & o\_{yl} \\
o\_{zl} & 0 & -o\_{xl} \\
\end{bmatrix}.
\tag{20}
$$

Can be easily observed that eq. (19) has the general form:

$$
\dot{X} = XA - BX\_\prime \tag{21}
$$

with *A*=*<sup>ω</sup>*˜ *<sup>v</sup>* and*<sup>B</sup>* <sup>=</sup>*<sup>ω</sup>*˜*<sup>l</sup>* . Considering that for a short period of time*Δt*, between *tn* and *tn*+1 times, the angular speeds *ωxv*, *ωyv*, *ωzv* and *ωxl* , *ωyl* , *ωzl* are constant, we obtains:

$$
\Delta\phi\_{xv} = \int\_{t\_u}^{t\_{u\pm 1}} \alpha\_{xv} \mathbf{d}t = \alpha\_{xv} \Delta t\_{\prime} \,\,\Delta\phi\_{yv} = \int\_{t\_u}^{t\_{u\pm 1}} \alpha\_{yv} \mathbf{d}t = \alpha\_{yv} \Delta t\_{\prime} \,\,\Delta\phi\_{xv} = \int\_{t\_u}^{t\_{u\pm 1}} \alpha\_{xv} \mathbf{d}t = \alpha\_{xv} \Delta t \,\tag{22}
$$

and

*λ* and *ϕ* are the longitude and the latitude. The angular speed*ω*

frame, has in ENU frame the next components:

*r rxl ryl rzl l*

2

l

*yl zl xl*

l

*v v v*

2 2

<sup>→</sup> and *<sup>g</sup>*

www

w

312 MATLAB Applications for the Practical Engineer

Therefore, equations (9) become:

d

d

d

d

also, the components of *f*

where *Rv l*

In eq. (19) *<sup>ω</sup>*˜ *<sup>v</sup>*and *<sup>ω</sup>*˜*<sup>l</sup>*

→

[, , ] , , tg .

é ù é ù <sup>=</sup> = -ê ú ë û +++ ë û

tg 2 sin 2 cos , <sup>d</sup>

= + +W - +W + ç ÷ + + è ø

*<sup>f</sup> v v <sup>g</sup> t Rh R h*

 

*xl yl zl axl*

2 cos .

navigation system contains three accelerometers and three gyros, its inputs will be the components of the vehicle absolute acceleration and angular speed in the vehicle frame:

[] [ , , ], *<sup>T</sup>*

[ ] [ , , ]. *<sup>T</sup>*

The components of the specific force in ENU can be determinate by using the relation:

[] [], *<sup>l</sup>*

. *ll l RR R* & *v vv lv* = w w

frame and can be calculated by solving the next Poisson equation (Farrell, 2008):

is the rotation matrix performing the coordinate change between SV frame and ENU

www

tg 2 sin , <sup>d</sup>

= - -W - + + +

*v v <sup>v</sup> <sup>f</sup> vv g t Rh R h*

*zl xl azl*

To integrate these equations we need to know the initial values of *ϕ*, *λ*, *h* , *vxl*

*yl xl zl ayl*

 

*xl yl xl xl*

*v v v v*

*yl yl xl*

*<sup>f</sup> v g t R hR h*

= + + +W + + +

 l

w

have the expressions:

*T yl xl xl*

*v v v R hR hR h* ll

> l

æ ö

<sup>→</sup> in ENU frame. Because the IMU of the strap-down inertial

*v xv yv zv f fff* <sup>=</sup> <sup>r</sup> (16)

*v v xv yv zv* <sup>=</sup> <sup>r</sup> (17)

*l vv f Rf* <sup>=</sup> r r (18)

% % (19)

 

<sup>r</sup> (14)

*<sup>r</sup>*, relative to the ECEF reference

(15)

, and,

, *vyl* , *vzl*

*T*

$$
\Delta\phi\_{\rm xl} = \int\_{t\_n}^{t\_{n+1}} \alpha\_{\rm xl} \mathbf{d}t = \alpha\_{\rm xl} \Delta t\_{\prime}
\
\Delta\phi\_{\rm yl} = \int\_{t\_n}^{t\_{n+1}} \alpha\_{\rm yl} \mathbf{d}t = \alpha\_{\rm yl} \Delta t\_{\prime}
\
\Delta\phi\_{\rm zl} = \int\_{t\_n}^{t\_{n+1}} \alpha\_{\rm zl} \mathbf{d}t = \alpha\_{\rm zl} \Delta t. \tag{23}
$$

*Δϕxv*, *Δϕyv*, *Δϕzv*, respectively *Δϕxl* , *Δϕyl* , *Δϕzl* are the increments of the angular rotations measured around the roll, pitch and yaw axes, respectively the increments of the angular rotations around the ENU frame axes calculated by the navigation processor. In this way, the value provided for the *X* matrix at the *tn*+1 time is given by:

$$
\dot{X}X\_{n+1} = X\_n + \dot{X}\_n \Delta t = X\_n + X\_n A \Delta t - B \Delta t X\_{n'} \tag{24}
$$

from where it is obtained:

$$X\_{n+1} = X\_n(I + A\Delta t) - B\Delta t\\X\_n = X\_n A\_n - B\_n X\_{n'} \tag{25}$$

with:

$$\begin{aligned} \boldsymbol{A}\_{n} = \begin{bmatrix} 1 & -\Delta\phi\_{zv} & \Delta\phi\_{yv} \\ \Delta\phi\_{zv} & 1 & -\Delta\phi\_{xv} \\ -\Delta\phi\_{yv} & \Delta\phi\_{xv} & 1 \end{bmatrix}, \boldsymbol{B}\_{n} = \begin{bmatrix} 0 & -\Delta\phi\_{zl} & \Delta\phi\_{yl} \\ \Delta\phi\_{zl} & 0 & -\Delta\phi\_{xl} \\ -\Delta\phi\_{yl} & \Delta\phi\_{xl} & 0 \end{bmatrix}. \end{aligned} \tag{26}$$

Therefore, the solution of the eq. (19) has the form:

$$\left. \mathcal{R}\_v^l \right|\_{n+1} = \left. \mathcal{R}\_v^l \right|\_n \left. A\_n - \left. \mathcal{B}\_n \mathcal{R}\_v^l \right|\_n \right|\_n. \tag{27}$$

0 [ ] [ (0)] [ ] d [ ] , *t*

it results:

ENU:

where *r* → ′

from which, with the model of the gravitational field for ECEF reference frame (Radix, 1993),

1 3 22

*r rr*

*P P*

é ù æ ö æ ö ê ú ç ÷ + ç - ÷ +W × ç ÷ è ø è ø é ù

*P P a P aY P*

ê ú æ ö æ ö = = + ç - ÷ +W × ê ú ç ÷ ç ÷ ê ú è ø è ø ê ú ë û æ ö æ ö ç ÷ + ç -÷ ç ÷ ë û è ø è ø

1 3 22

<sup>5</sup> [ ] 1 1;

*y Az gg A <sup>y</sup> r rr*

, *<sup>A</sup>*<sup>2</sup> <sup>=</sup> <sup>−</sup>6, <sup>66425</sup>⋅10<sup>10</sup> <sup>m</sup><sup>2</sup>

Finally, the vehicle coordinates in ENU are obtained with the equation:

description of the algorithm, it results the block diagram in Fig. 3.

**3. Error model of the navigation algorithm**

*P P P*

*aX*

*g*

*g*

*<sup>A</sup>*<sup>1</sup> <sup>=</sup> <sup>−</sup>3, <sup>986005</sup>⋅10<sup>14</sup> <sup>m</sup><sup>3</sup> / <sup>s</sup><sup>2</sup>

*aZ*

2 2 2

<sup>5</sup> 1 1

*x Az A x*

2 2 2

2 1 3 22

*P P*

*z Az <sup>A</sup> r rr*

frame, starting from the model (33), are calculated by using the inverse transform ECEF to

[ ] [ ]. *<sup>l</sup>*

0 [ ] [ (0)] [ ] d [ ] , *t*

<sup>5</sup> 1 3

2

<sup>r</sup> (33)

*T P P P PPP r r v t xyz* =+ = ò rr r (32)

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

*P*

(Radix, 1993). Components of *g*

*al P aP g Rg* <sup>=</sup> r r (34)

*T l l l lll r r v t xyz* ¢ ¢ =+ = ò rr r (35)

is the vehicle position vector in ENU reference frame. From the mathematical

The quality of the inertial navigator depends by the precision of the used sensors and by the numerical algorithms implemented in the navigation processor. For the error model developed in this subchapter are taken into account only the errors of the inertial sensors, considering that the numerical algorithm implemented in the navigation processor works free of errors. Thus, the model highlights the dependence of the position, velocity and attitude errors by the errors of the accelerometers and gyros in strap-down IMU. In the development of the error

→

http://dx.doi.org/10.5772/57583

315

*<sup>a</sup>* in ENU

Through the numerical integration of the equations (15) are obtained the components of the relative speed *v* <sup>→</sup> in ENU frame:*vxl* , *vyl* , *vzl* . With the equations (Salychev, 1998):

$$\begin{aligned} \dot{\phi} &= \frac{\upsilon\_{yl}}{R\_{\phi} + h}, \\ \dot{\mathcal{L}} &= \frac{\upsilon\_{zl}}{\left(R\_{\lambda} + h\right)\cos\phi}, \\ \dot{h} &= \upsilon\_{zl}. \end{aligned} \tag{28}$$

it result the geographic coordinated of the vehicle:

$$\begin{aligned} \boldsymbol{\phi}\left(t\right) &= \boldsymbol{\phi}\left(0\right) + \int\_{0}^{t} \frac{\boldsymbol{\upsilon}\_{yl}}{R\_{\phi} + h} \, \mathrm{d}t, \\ \boldsymbol{\mathcal{A}}\left(t\right) &= \boldsymbol{\mathcal{A}}\left(0\right) + \int\_{0}^{t} \frac{\boldsymbol{\upsilon}\_{xl}}{\left(R\_{\lambda} + h\right)\cos\phi} \, \mathrm{d}t, \\ \boldsymbol{h}\left(t\right) &= \boldsymbol{h}\left(0\right) + \int\_{0}^{t} \boldsymbol{\upsilon}\_{zl} \, \mathrm{d}t. \end{aligned} \tag{29}$$

By using the rotation matrix between ENU and ECEF frames (Farrell, 2008; Salychev, 1998; Titterton and Weston, 2004):

$$\begin{aligned} \begin{bmatrix} \mathbf{R}\_l^P \end{bmatrix}^T = \mathbf{R}\_p^l = \begin{bmatrix} -\sin\mathscr{\lambda} & \cos\mathscr{\lambda} & 0\\ -\sin\phi\cos\mathscr{\lambda} & -\sin\phi\sin\mathscr{\lambda} & \cos\phi\\ \cos\phi\cos\mathscr{\lambda} & \cos\phi\sin\mathscr{\lambda} & \sin\phi \end{bmatrix} \end{aligned} \tag{30}$$

the components of the relative speed *v* <sup>→</sup> in ECEF frame result with equation:

$$\mathbb{E}\left[\vec{\boldsymbol{\upsilon}}\right]\_P = \mathbb{R}\_l^P \{\vec{\boldsymbol{\upsilon}}\}\_l. \tag{31}$$

By numerical integration of the relative speed *v* → *<sup>P</sup>* is obtained *r* → *P* : Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 315

$$\begin{aligned} \left[\vec{r}\right]\_P = \left[\vec{r}\left(\mathbf{0}\right)\right]\_P + \oint\_0 \left[\vec{v}\right]\_P \mathbf{d}\,\mathbf{t} = \left[\mathbf{x}\_P \,\,\, y\_P \,\, \mathbf{z}\_P\right]^T \,\, \end{aligned} \tag{32}$$

from which, with the model of the gravitational field for ECEF reference frame (Radix, 1993), it results:

$$\begin{aligned} \lceil \left[ \mathcal{\bar{g}}\_{a} \right]\_{P} = \begin{bmatrix} \mathcal{S}\_{a\chi^{p}} \\ \mathcal{S}\_{a\chi^{p}} \\ \mathcal{S}\_{a\mathcal{L}^{p}} \end{bmatrix} = \begin{bmatrix} A\_{1} \frac{\chi\_{p}}{r^{3}} \left( 1 + \frac{A\_{2}}{r^{2}} \left( \frac{5z\_{p}^{2}}{r^{2}} - 1 \right) \right) + \Omega^{2} \cdot \mathbf{x}\_{P} \\ A\_{1} \frac{\mathcal{Y}\_{p}}{r^{3}} \left( 1 + \frac{A\_{2}}{r^{2}} \left( \frac{5z\_{p}^{2}}{r^{2}} - 1 \right) \right) + \Omega^{2} \cdot \mathbf{y}\_{P} \\ A\_{1} \frac{z\_{p}}{r^{3}} \left( 1 + \frac{A\_{2}}{r^{2}} \left( \frac{5z\_{p}^{2}}{r^{2}} - 3 \right) \right) \end{bmatrix} \tag{33} \end{aligned} \tag{34}$$

*<sup>A</sup>*<sup>1</sup> <sup>=</sup> <sup>−</sup>3, <sup>986005</sup>⋅10<sup>14</sup> <sup>m</sup><sup>3</sup> / <sup>s</sup><sup>2</sup> , *<sup>A</sup>*<sup>2</sup> <sup>=</sup> <sup>−</sup>6, <sup>66425</sup>⋅10<sup>10</sup> <sup>m</sup><sup>2</sup> (Radix, 1993). Components of *g* → *<sup>a</sup>* in ENU frame, starting from the model (33), are calculated by using the inverse transform ECEF to ENU:

$$\mathbb{E}\left[\vec{\mathcal{g}}\_{a}\right]\_{l} = \mathbb{R}\_{p}^{l} \|\vec{\mathcal{g}}\_{a}\|\_{P}.\tag{34}$$

Finally, the vehicle coordinates in ENU are obtained with the equation:

$$\begin{aligned} \left[\vec{r}'\right]\_l = \left[\vec{r}'(0)\right]\_l + \oint\_l \left[\vec{v}\right]\_l \mathbf{d}t = \left[\mathbf{x}\_l \ y\_l \ z\_l\right]^T, \end{aligned} \tag{35}$$

where *r* → ′ is the vehicle position vector in ENU reference frame. From the mathematical description of the algorithm, it results the block diagram in Fig. 3.

#### **3. Error model of the navigation algorithm**

Therefore, the solution of the eq. (19) has the form:

314 MATLAB Applications for the Practical Engineer

<sup>→</sup> in ENU frame:*vxl*

it result the geographic coordinated of the vehicle:

l

*P l <sup>T</sup> R R l P*

By numerical integration of the relative speed *v*

the components of the relative speed *v*

Titterton and Weston, 2004):

relative speed *v*

<sup>1</sup> . *ll l*

( )

l

*yl*

*v R h v R h*

<sup>=</sup> <sup>+</sup>

<sup>=</sup> <sup>+</sup>

.

*zl*

( ) ( ) ( )

0

ò

*t*

0 d,

*R h*

+

l

*v t t*

*<sup>t</sup> yl*

0 d ,

+

By using the rotation matrix between ENU and ECEF frames (Farrell, 2008; Salychev, 1998;

é ù - ê ú = =- -

sin cos 0

 l

 l

 l

<sup>→</sup> in ECEF frame result with equation:

*<sup>P</sup>* is obtained *r*

 

 

*P ll v Rv* <sup>=</sup> r r (31)

→ *P* :

cos cos cos sin sin

→

ë û

*v t t R h*

*xl*

cos

0

ò

*t zl*

0 d.

[ ] sin cos sin sin cos

l

[ ] [ ]. *<sup>P</sup>*

l

l

0

ò

*h v*

=

,

*xl*

, cos

, *vyl* , *vzl*

&

l

&

( ) ( )

 

= +

= +

 l

( ) ( )

*ht h v t*

= +

&

Through the numerical integration of the equations (15) are obtained the components of the

*v v n nv nn n R R A BR* <sup>+</sup> = - (27)

. With the equations (Salychev, 1998):

(28)

(29)

(30)

The quality of the inertial navigator depends by the precision of the used sensors and by the numerical algorithms implemented in the navigation processor. For the error model developed in this subchapter are taken into account only the errors of the inertial sensors, considering that the numerical algorithm implemented in the navigation processor works free of errors. Thus, the model highlights the dependence of the position, velocity and attitude errors by the errors of the accelerometers and gyros in strap-down IMU. In the development of the error

**Figure 3.** Block diagram of the navigation algorithm.

model are used techniques widely presented in the literature (Dahia, 2005; Farrell, 2008; Salychev, 1998; Savage, 2000).

Denoting with *m* the ideal value of a measurement and with *m* ⌢ its real value, given by the measurement system, the measurement error is calculated with the relation:

$$
\hat{\sigma}\mathcal{S}m = m - \hat{m}.\tag{36}
$$

d*fff* = - , rrr)

dw w w

errors of the vehicle position over the ENU frame axes (*xl*

dj j j dq q q dy y y=- =- = - ,, , ) ) ) (41)

*rrr*

¢¢¢ = -

.

*v v <sup>v</sup> R R l l RI R R <sup>l</sup>*

⌢ *l v*

Weston, 2004), in which are considered the latitude and longitude errors:

 dj

d

*vvv*

= -

dy dq

d

(*δφ* ⋅*δθ* =*δφ* ⋅*δψ*=*δθ* ⋅*δψ*=0), it results:

dy

where *Rl*

(44) we have:

In similar way, for the *RP*

dq dj

*<sup>v</sup>* is the right matrix, and *R*

.

ddd

ddd

linear speed (*vxl*

, *vyl* , *vzl* ): . *vvv*

Similarly can be defined the errors of the attitude angles (*φ*, *θ*, *ψ*-roll, pitch and yaw), the

, ,,

, ,,

)))

Starting from the errors of the attitude angles may be deduced the errors affecting the attitude

SV) (Farrell, 2008; Salychev, 1998; Titterton and Weston, 2004) and considering as negligible the products of the attitude angles errors taken as small perturbations,

> 1 ( ), with 0 , 1 0

) ) % % (44)

) ))

*l ll l ll lll xxx yyy zzz*

*xl xl xl yl yl yl zl zl zl v vv v vv vvv*

matrices. Thus, with the equations expressing the elements of the rotation matrix *Rl*

3 1 0

éù éù - - êú êú <sup>=</sup> -= + = -


=- =- =-

=- =- =-

= - rrr) (40)

), and the errors of the vehicle

http://dx.doi.org/10.5772/57583

*v*

(ENU to

, *yl* , *zl*

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

rrr) (42)

rrr) (43)

dy dq

> dj

dy

3 3 () ( )() ( ) . *l v T TT <sup>v</sup> <sup>l</sup> R R I R R I RR v l <sup>l</sup> <sup>v</sup>* = = + × = -× ) ) % % (45)

*<sup>l</sup>* matrix (ECEF to ENU) (Farrell, 2008; Salychev, 1998; Titterton and

dq dj


(39)

317

Starting from this expression, and having in mind that accelerometric readings are denoted with *f xv*, *f yv*, *f zv*, and gyro readings are denoted with *ωxv*, *ωyv*, *ωzv*, it result the errors of the accelerometric and gyro sensors under the scalar forms (the components for all three IMU axes):

$$
\delta \mathcal{S} f\_{xv} = f\_{xv} - \hat{f}\_{xv'} \; \delta f\_{yv} = f\_{yv} - \hat{f}\_{yv'} \; \delta f\_{zv} = f\_{yv} - \hat{f}\_{yv'} \tag{37}
$$

$$
\delta\delta o\_{xv} = \alpha\_{xv} - \hat{\alpha}\_{xv'} \; \delta\alpha\_{yv} = \alpha\_{yv} - \hat{\alpha}\_{yv'} \; \delta\alpha\_{zv} = \alpha\_{zv} - \hat{\alpha}\_{zv} \tag{38}
$$

and, under the vector forms:

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 317

$$
\delta \vec{f} = \vec{f} - \hat{\vec{f}}\_r \tag{39}
$$

$$
\delta\vec{\partial}\_{\upsilon} = \vec{\partial}\_{\upsilon} - \hat{\vec{\partial}}\_{\upsilon}.\tag{40}
$$

Similarly can be defined the errors of the attitude angles (*φ*, *θ*, *ψ*-roll, pitch and yaw), the errors of the vehicle position over the ENU frame axes (*xl* , *yl* , *zl* ), and the errors of the vehicle linear speed (*vxl* , *vyl* , *vzl* ):

$$
\delta\delta\mathfrak{p} = \mathfrak{q} - \hat{\mathfrak{p}}, \; \delta\Theta = \theta - \hat{\theta}, \; \delta\mathfrak{p} = \mathfrak{y} - \hat{\mathfrak{y}} \, , \tag{41}
$$

$$\begin{aligned} \delta \mathbf{\hat{x}}\_{l} &= \mathbf{x}\_{l} - \hat{\mathbf{x}}\_{l'} \cdot \delta \mathbf{y}\_{l} = \mathbf{y}\_{l} - \hat{\mathbf{y}}\_{l'} \cdot \delta \mathbf{z}\_{l} = \mathbf{z}\_{l} - \hat{\mathbf{z}}\_{l'} \\ \delta \vec{r}' &= \vec{r}' - \hat{\mathbf{r}}'. \end{aligned} \tag{42}$$

$$\begin{aligned} \delta \boldsymbol{\sigma}\_{\text{xl}} &= \boldsymbol{\upsilon}\_{\text{xl}} - \hat{\boldsymbol{\upsilon}}\_{\text{xl}\prime} \; \delta \boldsymbol{\upsilon}\_{\text{yl}} = \boldsymbol{\upsilon}\_{\text{yl}} - \hat{\boldsymbol{\upsilon}}\_{\text{yl}\prime} \; \delta \boldsymbol{\upsilon}\_{\text{z}l} = \boldsymbol{\upsilon}\_{\text{z}l} - \hat{\boldsymbol{\upsilon}}\_{\text{z}l\prime} \\ \delta \vec{\boldsymbol{\upsilon}} &= \vec{\boldsymbol{\upsilon}} - \hat{\vec{\boldsymbol{\upsilon}}} . \end{aligned} \tag{43}$$

Starting from the errors of the attitude angles may be deduced the errors affecting the attitude matrices. Thus, with the equations expressing the elements of the rotation matrix *Rl v* (ENU to SV) (Farrell, 2008; Salychev, 1998; Titterton and Weston, 2004) and considering as negligible the products of the attitude angles errors taken as small perturbations, (*δφ* ⋅*δθ* =*δφ* ⋅*δψ*=*δθ* ⋅*δψ*=0), it results:

model are used techniques widely presented in the literature (Dahia, 2005; Farrell, 2008;

⌢

) (36)

its real value, given by the

Denoting with *m* the ideal value of a measurement and with *m*

measurement system, the measurement error is calculated with the relation:

d

ddd

*mmm* = - .

,,, *xv xv xv yv yv yv zv yv yv*

, , *xv xv xv yv yv yv zv zv zv*

 w w dw

*fff fff fff* =- =- =- ))) (37)

 w w

=- =- =- ))) (38)

Starting from this expression, and having in mind that accelerometric readings are denoted with *f xv*, *f yv*, *f zv*, and gyro readings are denoted with *ωxv*, *ωyv*, *ωzv*, it result the errors of the accelerometric and gyro sensors under the scalar forms (the components for all three IMU

Salychev, 1998; Savage, 2000).

316 MATLAB Applications for the Practical Engineer

**Figure 3.** Block diagram of the navigation algorithm.

dw

and, under the vector forms:

 w w dw

axes):

$$R\_l^\upsilon = \hat{R}\_l^\upsilon \begin{bmatrix} 1 & -\delta\nu & \delta\theta \\ \delta\nu & 1 & -\delta\rho \\ -\delta\theta & \delta\rho & 1 \end{bmatrix} = \hat{R}\_l^\upsilon (I\_3 + \tilde{R}), \quad \text{with} \quad \tilde{R} = \begin{bmatrix} 0 & -\delta\nu & \delta\theta \\ \delta\nu & 0 & -\delta\rho \\ -\delta\theta & \delta\rho & 0 \end{bmatrix} \tag{44}$$

where *Rl <sup>v</sup>* is the right matrix, and *R* ⌢ *l v* -the matrix provided by the navigation system. From eq. (44) we have:

$$\boldsymbol{R}\_{\boldsymbol{v}}^{\boldsymbol{I}} = \left(\boldsymbol{R}\_{\boldsymbol{I}}^{\boldsymbol{v}}\right)^{\boldsymbol{T}} = \left(\boldsymbol{I}\_{\boldsymbol{3}} + \tilde{\boldsymbol{R}}\right)^{\boldsymbol{T}} \cdot \left(\hat{\boldsymbol{R}}\_{\boldsymbol{I}}^{\boldsymbol{v}}\right)^{\boldsymbol{T}} = \left(\boldsymbol{I}\_{\boldsymbol{3}} - \tilde{\boldsymbol{R}}\right) \cdot \hat{\boldsymbol{R}}\_{\boldsymbol{v}}^{\boldsymbol{I}}.\tag{45}$$

In similar way, for the *RP <sup>l</sup>* matrix (ECEF to ENU) (Farrell, 2008; Salychev, 1998; Titterton and Weston, 2004), in which are considered the latitude and longitude errors:

$$
\delta \mathcal{S} \mathbb{X} = \mathbb{X} - \hat{\mathbb{X}}, \quad \delta \phi = \phi - \hat{\phi} \,. \tag{46}
$$

dw

 dw

dw

Substituting relations (50) and (52) in (56), get to the formula:

w

⌢ *l v*

Considering expressions of *<sup>R</sup>*˜, *δω*˜ *<sup>v</sup>* and*δω*˜*<sup>l</sup>*

which, by multiplication on the left with(*R*

With formulas (12) and (28) it results:

*l xl yl zl <sup>l</sup>* é ù ë û

 w w w

w

between small quantities (*R*

 dw

dw

From (44) we obtain:

i.e.

 dw

which, through derivation, implies:

dw

*v zv xv l zl xl yv xv yl xl*

<sup>3</sup> ( )( ). *vv v v v RRRR R R R R I R l l l l l l l vl* ×= ×× - × ×+ × - × +

, *vv v v v RRRR R RR R l l l l l l l vl* ×= ×× - × ×+ × - ×

 w

⌢ *l v* ) *T*

() . *v v <sup>T</sup> RR R R R*

[ , , ] , cos cos , sin sin ,

 

<sup>=</sup> = - +W é ù + W ë û

 dw

 l

 dw dw)) ) ) ) &%% % % % % %% (57)

> ⌢ *l v*

 dw dw)) ) ) ) &%% % % % %% (58)

 dw*l l l l vl* =× - ×+ - × × ) ) &%% % %% % % (59)

*T T*

<sup>r</sup> & & & (60)

 l

 

 

 w

<sup>⋅</sup>*δω*˜*<sup>l</sup>* <sup>⋅</sup>*<sup>R</sup>*˜and*δω*˜ *<sup>v</sup>* <sup>⋅</sup> *<sup>R</sup>*

w

w w  dw

<sup>é</sup> - - ù é <sup>ù</sup> <sup>ê</sup> ú ê <sup>ú</sup> = -= - <sup>ê</sup> ú ê <sup>ú</sup> <sup>ê</sup> ú ê <sup>ú</sup> - - êë ú ê û ë úû

0 , 0. 0 0

 dw

dw

% % (53)

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

*zv yv zl yl*

dw

 dw  dw

, *vvv R R RR lll* -=× ) ) % (54)

, *vvv v R R RRRR lll l* - = ×+ × )) ) & & & & %% (55)

<sup>3</sup> ( ). *v vv RRR R I R l ll* ×= - × + ) ) & & %& % (56)

<sup>⋅</sup>*<sup>R</sup>*˜) and we obtain:

, leads to the relation:

, in formula (57) can be neglected the products

dw

http://dx.doi.org/10.5772/57583

319

it results:

$$\boldsymbol{R}\_{P}^{l} = (\boldsymbol{I}\_{3} - \tilde{\boldsymbol{P}}) \cdot \hat{\boldsymbol{R}}\_{P}^{l}.\tag{47}$$

where *RP <sup>l</sup>* is the right matrix, *R* ⌢ *P l* -the matrix provided by the navigation system, and *<sup>P</sup>*˜ has the form:

$$
\tilde{P} = \begin{bmatrix}
0 & -\delta p\_z & \delta p\_y \\
\delta p\_z & 0 & -\delta p\_x \\
\end{bmatrix} \tag{48}
$$

with:

$$
\delta \delta p\_x = -\delta \phi\_\prime \,\,\delta p\_y = \cos \hat{\phi} \cdot \delta \vec{\omega} \,\,\,\delta p\_z = \sin \hat{\phi} \cdot \delta \vec{\omega} \,\,\,\tag{49}
$$

One of the form of the attitude Poisson equation is (Farrell, 2008; Grigorie, 2007; Salychev, 1998; Titterton and Weston, 2004):

$$
\dot{\mathbf{R}}\_l^v = \mathbf{R}\_l^v \cdot \tilde{\boldsymbol{\alpha}}\_l - \tilde{\boldsymbol{\alpha}}\_v \cdot \mathbf{R}\_l^v \tag{50}
$$

where *<sup>ω</sup>*˜*<sup>l</sup>* and *<sup>ω</sup>*˜ *<sup>v</sup>* have the expressions given by equations (20). Due to the erroneous meas‐ urements, the inertial system integrates the next equation:

$$
\hat{\vec{R}}\_l^v = \hat{\vec{R}}\_l^v \cdot \hat{\vec{\sigma}}\_l - \hat{\vec{\sigma}}\_v \cdot \hat{\vec{R}}\_l^v \,. \tag{51}
$$

Thus, it results:

$$
\hat{\vec{R}}\_{l}^{\upsilon} = \hat{R}\_{l}^{\upsilon} \cdot \{\tilde{\alpha}\_{l} - \delta \tilde{\alpha}\_{l}\} - \{\tilde{\alpha}\_{\upsilon} - \delta \tilde{\alpha}\_{\upsilon}\} \cdot \hat{R}\_{l}^{\upsilon} \,. \tag{52}
$$

with

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 319

$$
\delta\delta\tilde{o}\_{v} = \begin{bmatrix} 0 & -\delta o\_{zv} & \delta o\_{yv} \\ \delta o\_{zv} & 0 & -\delta o\_{xv} \\ -\delta o\_{yv} & \delta o\_{xv} & 0 \end{bmatrix}, \begin{bmatrix} 0 & -\delta o\_{zl} & \delta o\_{yl} \\ \delta o\_{zl} & 0 & -\delta o\_{xl} \\ -\delta o\_{yl} & \delta o\_{xl} & 0 \end{bmatrix}. \tag{53}$$

From (44) we obtain:

$$
\hat{\mathbf{R}}\_l^v - \hat{\mathbf{R}}\_l^v = \hat{\mathbf{R}}\_l^v \cdot \tilde{\mathbf{R}}\_r \tag{54}
$$

which, through derivation, implies:

$$
\dot{\vec{R}}\_l^v - \dot{\vec{R}}\_l^v = \dot{\vec{R}}\_l^v \cdot \vec{\mathcal{R}} + \hat{\mathcal{R}}\_l^v \cdot \dot{\vec{\mathcal{R}}}\_r \tag{55}
$$

i.e.

dl l l d 

⌢ *P l*

0

d

d


0

ë û

, cos , sin . *xy z*

One of the form of the attitude Poisson equation is (Farrell, 2008; Grigorie, 2007; Salychev,

and *<sup>ω</sup>*˜ *<sup>v</sup>* have the expressions given by equations (20). Due to the erroneous meas‐

 dl d

, *vv v RR R* & *l l l vl* = ×- × w w

. *vv v RR R l l l vl* = ×- × w w

( )( ) , *v v <sup>v</sup> R R l l l l v vl* =×- - - ×

 w dw *R* ) ) ) & %% % % (52)

w dw d

é ù - ê ú <sup>=</sup> -

*y x*

 d

*Pp p p p*

*z x*

0

*z y*

 d

> d

*p p*

it results:

where *RP*

form:

with:

where *<sup>ω</sup>*˜*<sup>l</sup>*

Thus, it results:

with

*<sup>l</sup>* is the right matrix, *R*

318 MATLAB Applications for the Practical Engineer

d

1998; Titterton and Weston, 2004):

 d d

urements, the inertial system integrates the next equation:

=- =- , ,

) ) (46)

<sup>3</sup> ( ), *l l R I PR P P* = -× ) % (47)


% (48)

 dl*pp p* =- = × = × ) ) (49)

% % (50)

) ) ) )) & % % (51)

$$
\hat{R}\_l^v \cdot \dot{\tilde{R}} = \dot{R}\_l^v - \dot{\hat{R}}\_l^v \cdot (I\_3 + \tilde{R}).\tag{56}
$$

Substituting relations (50) and (52) in (56), get to the formula:

$$
\hat{R}\_l^v \cdot \dot{\tilde{R}} = \hat{R}\_l^v \cdot \tilde{\mathcal{R}} \cdot \tilde{\alpha}\_l - \hat{R}\_l^v \cdot \tilde{\alpha}\_l \cdot \tilde{\mathcal{R}} + (\hat{R}\_l^v \cdot \delta \tilde{\alpha}\_l - \delta \tilde{\alpha}\_v \cdot \hat{R}\_l^v)(I\_3 + \tilde{\mathcal{R}}).\tag{57}
$$

Considering expressions of *<sup>R</sup>*˜, *δω*˜ *<sup>v</sup>* and*δω*˜*<sup>l</sup>* , in formula (57) can be neglected the products between small quantities (*R* ⌢ *l v* <sup>⋅</sup>*δω*˜*<sup>l</sup>* <sup>⋅</sup>*<sup>R</sup>*˜and*δω*˜ *<sup>v</sup>* <sup>⋅</sup> *<sup>R</sup>* ⌢ *l v* <sup>⋅</sup>*<sup>R</sup>*˜) and we obtain:

$$
\hat{\mathbf{R}}\_{l}^{v} \cdot \dot{\tilde{\mathbf{R}}} = \hat{\mathbf{R}}\_{l}^{v} \cdot \tilde{\mathbf{R}} \cdot \tilde{\boldsymbol{\alpha}}\_{l} - \hat{\mathbf{R}}\_{l}^{v} \cdot \tilde{\boldsymbol{\alpha}}\_{l} \cdot \tilde{\mathbf{R}} + \hat{\mathbf{R}}\_{l}^{v} \cdot \delta \tilde{\boldsymbol{\alpha}}\_{l} - \delta \tilde{\boldsymbol{\alpha}}\_{v} \cdot \hat{\mathbf{R}}\_{l}^{v} \, \tag{58}
$$

which, by multiplication on the left with(*R* ⌢ *l v* ) *T* , leads to the relation:

$$
\dot{\tilde{R}} = \tilde{\boldsymbol{R}} \cdot \tilde{\boldsymbol{o}} \tilde{\boldsymbol{o}}\_{l} - \tilde{\boldsymbol{o}} \tilde{\boldsymbol{o}}\_{l} \cdot \tilde{\boldsymbol{R}} + \delta \tilde{\boldsymbol{o}} \tilde{\boldsymbol{o}}\_{l} - \{\hat{\boldsymbol{R}}\_{l}^{v}\}^{T} \cdot \delta \tilde{\boldsymbol{o}} \tilde{\boldsymbol{o}}\_{v} \cdot \hat{\boldsymbol{R}}\_{l}^{v} \,. \tag{59}
$$

With formulas (12) and (28) it results:

$$\begin{bmatrix} \vec{\alpha} \\ \end{bmatrix}\_l = \begin{bmatrix} \alpha \boldsymbol{\rho}\_{\text{x}} & \alpha \boldsymbol{\rho}\_{\text{y}} & \alpha \boldsymbol{\rho}\_{\text{z}} \end{bmatrix}^T = \begin{bmatrix} -\dot{\boldsymbol{\phi}} & \dot{\boldsymbol{\lambda}} \cos \boldsymbol{\phi} + \boldsymbol{\Omega} \cos \boldsymbol{\phi} & \dot{\boldsymbol{\lambda}} \sin \boldsymbol{\phi} + \boldsymbol{\Omega} \sin \boldsymbol{\phi} \end{bmatrix}^T,\tag{60}$$

Evaluating the terms of differential equation (59), we obtain:

$$
\tilde{\mathcal{R}} \cdot \tilde{\boldsymbol{\alpha}}\_{l} - \tilde{\boldsymbol{\alpha}}\_{l} \cdot \tilde{\mathcal{R}} = \begin{bmatrix}
0 & -\{\boldsymbol{\alpha}\_{\rm yl} \delta \boldsymbol{\phi} - \boldsymbol{\alpha}\_{\rm xl} \delta \boldsymbol{\theta}\} & \boldsymbol{\alpha}\_{\rm xl} \delta \boldsymbol{\nu} - \boldsymbol{\alpha}\_{\rm zl} \delta \boldsymbol{\phi} \\\\ \boldsymbol{\alpha}\_{\rm yl} \delta \boldsymbol{\phi} - \boldsymbol{\alpha}\_{\rm xl} \delta \boldsymbol{\theta} & 0 & -\{\boldsymbol{\alpha}\_{\rm zl} \delta \boldsymbol{\theta} - \boldsymbol{\alpha}\_{\rm yl} \delta \boldsymbol{\nu}\} \\\\ -\{\boldsymbol{\alpha}\_{\rm zl} \delta \boldsymbol{\nu} - \boldsymbol{\alpha}\_{\rm zl} \delta \boldsymbol{\phi}\} & \boldsymbol{\alpha}\_{\rm zl} \delta \boldsymbol{\theta} - \boldsymbol{\alpha}\_{\rm yl} \delta \boldsymbol{\nu} & 0
\end{bmatrix},
\tag{61}
$$

[] [ , , ]*<sup>T</sup>*

the vector having the components equal with the errors of the attitude angles, it appears that:

*xl zl l l*

[ ] [ ] ( ) [ ] [ ], *<sup>v</sup> <sup>T</sup>* F =- ´F - × + *l l l l vv l l*

ê ú - =- ´F

[ ],

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

 w

dw

contains the errors of the angular velocities assessment committed by navigation

To derive the equation that characterizes the speed error evolution in time it starts from relation

<sup>d</sup> . <sup>d</sup> *l a*

*<sup>v</sup> f v vg <sup>t</sup>* = + ´ + W´ w

[ ] [ ] [ ] [( ) ] [ ] [ ] [( ) ] , *<sup>l</sup> l l al l l v v al l <sup>l</sup> v f g vR* = + - +W ´ = × + - +W ´

> [ ] [ ] [ ] [( ) ] . *<sup>l</sup> l v v al l <sup>l</sup> v Rf g* = × + - +W ´

[ ] [ ] [ ] [ ] [ ] [ ] [ ] {[( ) ] [( ) ] }, *l l l l l v v v v al al l l l l*

*v v v Rf Rf g g* = - = × - × + - - +W ´ - +W ´

that, in the hypothesis of erroneous measurement of accelerations and angular velocities,

w*v*

*v v* rrr) r r) ) r )r) ) ) rr r rr r & && (72)

*fg v* <sup>r</sup> r r rr r rr r r r & (70)

<sup>r</sup> <sup>r</sup> r r rr <sup>r</sup>

 dw

*<sup>v</sup> <sup>v</sup>* represents the errors due to gyro measurements in ENU frame, and

<sup>r</sup> r rr <sup>r</sup> ) & (68)

 w

)r )r ) )r) ) ) rr r & (71)

w

 w

<sup>r</sup> (66)

<sup>r</sup> <sup>r</sup> (67)

http://dx.doi.org/10.5772/57583

321

(69)

d dq dy

*<sup>l</sup>* F =

*zl yl*

é ù - ê ú

w dq w dy

w dy w d

w d w dq

w

and the matrix equation (59) can be transfigured as:

where (*R*

It results:

becomes:

d

*δω* → *l l* ⌢ *l v* ) *T* ⋅ *δω* →

(8) in which it is considered *ω*

Thus, the speed error will be:

*yl xl*

ê ú - ë û

*R*

processor. Equation (67) is the differential equation of the attitude error.

*l*

→ *<sup>N</sup>* =*ω* → *l* :

w

$$\begin{aligned} \left[\delta\ddot{o}\_{l}\right]\_{l} = \begin{bmatrix} \delta o\_{xl} \\ \delta o\_{yl} \\ \delta o\_{zl} \end{bmatrix} = \begin{bmatrix} -\delta \dot{\phi} \\ -\sin\phi \cdot \dot{\lambda} \cdot \delta\phi + \cos\phi \cdot \delta\dot{\lambda} - \Omega \cdot \sin\phi \cdot \delta\phi \\ \cos\phi \cdot \dot{\lambda} \cdot \delta\phi + \sin\phi \cdot \delta\dot{\lambda} + \Omega \cdot \cos\phi \cdot \delta\phi \end{bmatrix} \tag{62}$$

and for the product (*R* ⌢ *l v* ) *T* <sup>⋅</sup>*δω*˜ *<sup>v</sup>* <sup>⋅</sup> *<sup>R</sup>* ⌢ *l v* the resulting matrix elements are given by the expressions:

$$\begin{aligned} a\_{11} &= a\_{22} = a\_{33\prime} \\ a\_{21} &= -a\_{12} = -\sin\hat{\theta} \cdot \delta o\_{xv} + \sin\hat{\phi} \cos\hat{\theta} \cdot \delta o\_{yv} + \cos\hat{\phi} \cos\hat{\theta} \cdot \delta o\_{zv}, \\ a\_{13} &= -a\_{31} = \cos\hat{\theta} \sin\hat{\psi} \cdot \delta o\_{xv} + (\sin\hat{\phi} \sin\hat{\theta} \sin\hat{\psi} + \cos\hat{\phi} \cos\hat{\psi}) \cdot \delta o\_{yv} + \\ &+ (\cos\hat{\phi} \sin\hat{\theta} \sin\hat{\psi} - \sin\hat{\phi} \cos\hat{\psi}) \cdot \delta o\_{zv}, \\ a\_{32} &= -a\_{23} = \cos\hat{\theta} \cos\hat{\psi} \cdot \delta o\_{xv} + (\sin\hat{\phi} \sin\hat{\theta} \cos\hat{\psi} - \cos\hat{\phi} \sin\hat{\psi}) \cdot \delta o\_{yv} + \\ &+ (\cos\hat{\phi} \sin\hat{\theta} \cos\hat{\psi} + \sin\hat{\phi} \sin\hat{\psi}) \cdot \delta o\_{zv}. \end{aligned} \tag{63}$$

Therefore, the elements of the matrix *<sup>R</sup>*˜˙ from equation (59) are calculated by using relations of the form:

$$\begin{aligned} r\_{11} &= r\_{22} = r\_{33} = 0, \\ r\_{21} &= -r\_{12} = \{\alpha\_{yl}\delta\phi - \alpha\_{xl}\delta\theta\} - a\_{21} + \delta\phi\_{z1'} \\ r\_{13} &= -r\_{31} = \{\alpha\_{xl}\delta\nu - \alpha\_{zl}\delta\phi\} - a\_{13} + \delta\phi\_{yl'} \\ r\_{32} &= -r\_{23} = \{\alpha\_{zl}\delta\theta - \alpha\_{yl}\delta\nu\} - a\_{32} + \delta\phi\_{xl'} \end{aligned} \tag{64}$$

Taking into account that:

$$\begin{bmatrix} \delta \vec{o} \vec{o}\_v \end{bmatrix}\_v = \begin{bmatrix} \delta o\_{xv'} & \delta o\_{yv'} & \delta o\_{zv} \end{bmatrix}^T \tag{65}$$

can be quickly demonstrated that the elements described by formulas (63) come from a product by the form (*R* ⌢ *l v* ) *T* ⋅ *δω* → *<sup>v</sup> <sup>v</sup>*. Thus, denoting with:

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 321

$$\begin{bmatrix} \vec{\Phi} \end{bmatrix}\_l = \begin{bmatrix} \delta \phi \, \prime & \delta \theta \, \prime & \delta \psi \end{bmatrix}^T \tag{66}$$

the vector having the components equal with the errors of the attitude angles, it appears that:

$$\begin{bmatrix} \alpha\_{\text{xl}} \delta \Theta - \alpha\_{\text{yl}} \delta \Psi\\ \alpha\_{\text{xl}} \delta \Psi - \alpha\_{\text{zl}} \delta \Phi\\ \alpha\_{\text{yl}} \delta \phi - \alpha\_{\text{xl}} \delta \Theta \end{bmatrix} = -[\vec{\alpha}] \times \vec{\Phi}\mathbf{J}\_{l\prime} \tag{67}$$

and the matrix equation (59) can be transfigured as:

$$\{\vec{\dot{\Phi}}\}\_{l} = -[\vec{\alpha}\!\!] \times \vec{\Phi}\!]\_{l} - \left(\hat{R}\_{l}^{\upsilon}\right)^{T} \cdot \left[\delta\vec{\alpha}\!\!]\_{v}\right]\_{v} + \left[\delta\vec{\alpha}\!\!]\_{l}\right]\_{l'} \tag{68}$$

⌢ *v T*

Evaluating the terms of differential equation (59), we obtain:

 w d w dq

w dy w d

*xl*

dw

dw

*zl*

<sup>⋅</sup>*δω*˜ *<sup>v</sup>* <sup>⋅</sup> *<sup>R</sup>* ⌢ *l v*

*l l yl*

,

q dw

q y dw

 q y

q y dw

 q y

=- = × +

⌢ *l v* ) *T*

 dw

*R R*

320 MATLAB Applications for the Practical Engineer

dw

11 22 33 21 12 13 31

= =

*aaa a a a a*

32 23

*a a*

Taking into account that:

⌢ *l v* ) *T* ⋅ *δω* →

by the form (*R*

the form:

w w

and for the product (*R*

0( )

é ù -- - ê ú × - ×= - - -

w d w dq

*l l yl xl zl yl xl zl zl yl*


 l d

r & &

 l d

(cos sin sin sin cos ) , cos cos (sin sin cos

(cos sin cos sin sin ) .

21 12 21 13 31 13 32 23 32

=- = - - + =- = - - + =- = - - +

[ ] [ , , ], *<sup>T</sup> v v xv yv zv*

can be quickly demonstrated that the elements described by formulas (63) come from a product

*r r a r r a r r a*

w d w dq

w dy w d

w dq w dy

> dw dw dw

*<sup>v</sup> <sup>v</sup>*. Thus, denoting with:

0,

 q dw

=- =- × + × + ×

*xv*

)) ) ) )

)) ) ) )

+ -×

+ +×

11 22 33

dw

= = =

*rrr*

 w dq w dy

[ ] sin cos sin ,

é ù é ù - ê ú ê ú = = - × × + × -W× × ê ú

( ) 0

ë û

cos sin cos

ê ú × × + × +W× × ë û ë û

sin sin cos cos cos , cos sin (sin sin sin cos cos )

) ) ) ) ) cos sin )

Therefore, the elements of the matrix *<sup>R</sup>*˜˙ from equation (59) are calculated by using relations of

( ), ( ), ( ).

*yl xl zl xl zl yl zl yl xl*

) )) )) ) )) ) ) ))

 q y

=- = × + + ×+

 q y

 y dw

 y dw

*xv yv zv*

*xv yv zv*

*zv*

 dw

> dw

> > dw

<sup>=</sup> <sup>r</sup> (65)

 q dw

> y dw

 y dw


) )

& &

% % % % (61)

d

&

 dl

 dl

0 ( ) ,

 d (62)

(63)

(64)

 d

the resulting matrix elements are given by the expressions:

*yv*

w dq w dy

*yl xl xl zl*

 w dy w d

> where (*R l* ) ⋅ *δω* → *<sup>v</sup> <sup>v</sup>* represents the errors due to gyro measurements in ENU frame, and *δω* → *l l* contains the errors of the angular velocities assessment committed by navigation processor. Equation (67) is the differential equation of the attitude error.

> To derive the equation that characterizes the speed error evolution in time it starts from relation (8) in which it is considered *ω* → *<sup>N</sup>* =*ω* → *l* :

$$\vec{f} = \frac{\mathbf{d} \,\vec{v}}{\mathbf{d}t}\bigg|\_{\vec{l}} + \vec{o}\mathbf{j} \times \vec{v} + \vec{\Omega} \times \vec{v} - \vec{g}\_a. \tag{69}$$

It results:

$$\mathbf{I}\left[\vec{\boldsymbol{\upsilon}}\right]\_l = \left[\vec{f}\right]\_l + \left[\vec{\boldsymbol{\varrho}}\_a\right]\_l - \left[\left(\vec{\boldsymbol{\alpha}}\right)\_l + \vec{\boldsymbol{\Omega}}\right) \times \vec{\boldsymbol{\upsilon}}\right]\_l = \mathbf{R}\_v^l \cdot \left[\vec{f}\right]\_v + \left[\vec{\boldsymbol{\varrho}}\_a\right]\_l - \left[\left(\vec{\boldsymbol{\alpha}}\right)\_l + \vec{\boldsymbol{\Omega}}\right) \times \vec{\boldsymbol{\upsilon}}\right]\_l\tag{70}$$

that, in the hypothesis of erroneous measurement of accelerations and angular velocities, becomes:

$$\{\hat{\vec{\boldsymbol{\upsilon}}}\}\_{l} = \hat{\mathcal{R}}\_{\boldsymbol{\upsilon}}^{l} \cdot [\hat{\vec{f}}]\_{\boldsymbol{\upsilon}} + [\hat{\mathcal{G}}\_{a}]\_{l} - [(\hat{\vec{\boldsymbol{\alpha}}}\_{l} + \hat{\vec{\boldsymbol{\Omega}}}) \times \hat{\vec{\boldsymbol{\upsilon}}}]\_{l}.\tag{71}$$

Thus, the speed error will be:

$$[\delta\vec{\bar{v}}]\_{l} = [\vec{\bar{v}}]\_{l} - [\hat{\bar{\bar{v}}}]\_{l} = \mathsf{R}\_{v}^{l} \cdot [\vec{f}]\_{v} - \hat{\mathsf{R}}\_{v}^{l} \cdot [\hat{\bar{f}}]\_{v} + [\vec{\bar{g}}\_{a}]\_{l} - [\hat{\bar{\bar{g}}}\_{a}]\_{l} - \{[(\vec{a}\_{l} + \vec{\Omega}) \times \vec{v}]\_{l} - [(\hat{\bar{a}}\_{l} + \hat{\bar{\Omega}}) \times \hat{\bar{v}}]\_{l}\}\_{l} \tag{72}$$

but, according to formula (45), *Rv l* =(*I*<sup>3</sup> <sup>−</sup>*<sup>R</sup>*˜) <sup>⋅</sup> *<sup>R</sup>* ⌢ *v l* , and we have:

$$\begin{split} [\delta\vec{\boldsymbol{\sigma}}]\_{l} &= \hat{\boldsymbol{R}}\_{\boldsymbol{v}}^{l} \cdot [\delta\vec{f}]\_{v} - \tilde{\boldsymbol{R}} \cdot \hat{\boldsymbol{R}}\_{\boldsymbol{v}}^{l} \cdot [\vec{f}]\_{v} + [\delta\vec{g}\_{a}]\_{l} - \{ [(\vec{a}\vec{j} + \vec{\Omega}) \times \vec{v}]\_{l} - [(\hat{\vec{a}}\vec{j} + \hat{\vec{\Omega}}) \times \hat{\vec{v}}]\_{l} \} = \\ &= -[\vec{\Phi}]\_{l} \cdot [\vec{f}]\_{v} + \hat{\boldsymbol{R}}\_{v}^{l} \cdot [\delta\vec{f}]\_{v} + [\delta\vec{g}\_{a}]\_{l} - [(\vec{a}\vec{j} + \vec{\Omega}) \times \delta\vec{v}]\_{l} - [(\delta\vec{a}\vec{j} + \delta\vec{\Omega}) \times \vec{v}]\_{l'} \end{split} \tag{73}$$

in which:

$$[\delta\vec{g}\_a]\_l = [0, \ 0, \ -3, 08 \cdot 10^{-6} \cdot \delta h] \approx [0, \ 0, \ -2\frac{\text{K}M\_p}{a^2} \cdot \frac{\delta h}{a}] = [0, \ 0, \ -2\frac{\text{g}}{a} \cdot \delta h]. \tag{74}$$

$$\left[\delta\vec{\mathbf{Q}}\right]\_l = \left[0, -\Omega\cdot\sin\phi\cdot\delta\phi, \,\Omega\cdot\cos\phi\cdot\delta\phi\right]^T.\tag{75}$$

Equations (28) give the expressions for the speed components:

$$\begin{aligned} \upsilon\_{xl} &= (\mathcal{R}\_{\lambda} + h) \cdot \cos \phi \cdot \dot{\mathcal{A}}\_{\prime} \\ \upsilon\_{yl} &= (\mathcal{R}\_{\phi} + h) \cdot \dot{\phi}\_{\prime} \\ \upsilon\_{zl} &= \dot{h}\_{\prime} \end{aligned} \tag{76}$$

cos ( ) sin ( ) cos cos ,

 l

 

 dl

 l d

& &&

  dl

¶ =× -× ¶ (80)

 dl

 l d

& & &

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

 l d

http://dx.doi.org/10.5772/57583

(79)

323

(81)

(82)

(83)

*v R h R h h*

= × - + × ×× + + × × + ×× ê ú ¶ ë û

Can be easily verified, starting from the expressions of *Rλ* and*Rϕ*, that is valid the formula:

cos sin sin .

( ) cos ( ) sin cos ,

 d

( ) cos ( ) sin cos ,

( ) sin [ ] cos [ ],

One observes that the derivative of the position error on the vertical channel is equal with the

   dl l d

 dl

*v Rh Rh h*

*x v Rh h h*

[ ] [ , , ] [ , cos , sin ] , *T T l xyz p ppp* = =- × ×

 d  dl

<sup>r</sup> ) ) (84)

= - + × × × -× + × × -×

& & & & &

= + × × - + × ×× + ××

*R R*

l

() ,

() ,

 d d

 

 d

*x Rh Rh h*

& & & &

= + × × - + × ×× + ××

 

 l d

() ,

 d d

*v Rh h*

= ×× + + × +×

l

& & &

*R*

l

*y Rh h*

& & & &

 dl

= ×× + + × +×

*v Rh h*

= ×× + + × +×

 dl

& & &

,

 dl l d

é ù ¶

.

&

*R*

¶

¶ =

 d

Thus, relations (78) and (79) become:

*l*

d

d

d d

and

*l*

*R*

¶

¶ =

l

,

&

.

*y vh h*

= +× -×

d d d

& & &

> d d

*R*

¶

¶ =

l

 d

 d

*l*

*xl*

d

d

d

from where we obtain:

d

d

d d

speed error.

If we denote:

&

*yl*

*zl*

*v h*

*l xl l yl l zl*

 d

 d

*z v*

=

 d

*z h*

& &

*R*

l

*xl*

d

d

d

*yl*

*zl*

*v h*

 d

from where are obtained the positioning errors on the axes of the ENU frame:

$$\begin{aligned} \delta \mathbf{x}\_l &= (\mathbf{R}\_\lambda + h) \cdot \cos \phi \cdot \delta \mathbf{\lambda}\_\prime \\ \delta y\_l &= (\mathbf{R}\_\phi + h) \cdot \delta \phi\_\prime \\ \delta z\_l &= \delta h. \end{aligned} \tag{77}$$

By derivation with respect to time, equations (77) imply:

$$\begin{split} \delta \dot{\boldsymbol{x}}\_{l} &= \left[ \frac{\partial R\_{\dot{\lambda}}}{\partial \dot{\phi}} \cdot \dot{\boldsymbol{\phi}} \cdot \cos \phi - (R\_{\dot{\lambda}} + \boldsymbol{h}) \cdot \sin \phi \cdot \dot{\phi} \right] \delta \dot{\lambda} + (R\_{\dot{\lambda}} + \boldsymbol{h}) \cdot \cos \phi \cdot \delta \dot{\lambda} + \cos \phi \cdot \dot{\boldsymbol{h}} \cdot \delta \dot{\lambda}, \\ \delta \dot{\mathbf{y}}\_{l} &= \frac{\partial R\_{\dot{\phi}}}{\partial \dot{\phi}} \cdot \dot{\phi} \cdot \delta \phi + (R\_{\phi} + \boldsymbol{h}) \cdot \delta \dot{\phi} + \dot{\boldsymbol{h}} \cdot \delta \phi, \\ \delta \dot{\boldsymbol{z}}\_{l} &= \delta \dot{\boldsymbol{h}}\_{l} \end{split} \tag{78}$$

and, by differentiating the velocity components (76), we get expressions:

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 323

$$\begin{split} \delta v\_{xl} &= \left[\frac{\partial \mathcal{R}\_{\lambda}}{\partial \phi} \cdot \cos \phi - (\mathcal{R}\_{\lambda} + h) \cdot \sin \phi \right] \cdot \dot{\mathcal{A}} \cdot \delta \phi + (\mathcal{R}\_{\lambda} + h) \cdot \cos \phi \cdot \delta \dot{\mathcal{A}} + \cos \phi \cdot \dot{\mathcal{A}} \cdot \delta h, \\ \delta v\_{yl} &= \frac{\partial \mathcal{R}\_{\phi}}{\partial \phi} \cdot \dot{\phi} \cdot \delta \phi + (\mathcal{R}\_{\phi} + h) \cdot \delta \dot{\phi} + \dot{\phi} \cdot \delta h, \\ \delta v\_{zl} &= \delta \dot{h}. \end{split} \tag{79}$$

Can be easily verified, starting from the expressions of *Rλ* and*Rϕ*, that is valid the formula:

$$\frac{\partial R\_{\lambda}}{\partial \phi} \cos \phi = R\_{\lambda} \cdot \sin \phi - R\_{\phi} \cdot \sin \phi. \tag{80}$$

Thus, relations (78) and (79) become:

$$\begin{aligned} \delta \dot{\boldsymbol{x}}\_{l} &= (\boldsymbol{R}\_{\lambda} + \boldsymbol{h}) \cdot \cos \phi \cdot \delta \dot{\boldsymbol{\lambda}} - (\boldsymbol{R}\_{\phi} + \boldsymbol{h}) \cdot \sin \phi \cdot \dot{\phi} \cdot \delta \dot{\boldsymbol{\lambda}} + \cos \phi \cdot \dot{\boldsymbol{h}} \cdot \delta \dot{\boldsymbol{\lambda}}, \\ \delta \dot{\boldsymbol{y}}\_{l} &= \frac{\partial \boldsymbol{R}\_{\phi}}{\partial \phi} \cdot \dot{\phi} \cdot \delta \phi + (\boldsymbol{R}\_{\phi} + \boldsymbol{h}) \cdot \delta \dot{\phi} + \dot{\boldsymbol{h}} \cdot \delta \phi, \\ \delta \dot{\boldsymbol{z}}\_{l} &= \delta \dot{\boldsymbol{h}} \end{aligned} \tag{81}$$

and

but, according to formula (45), *Rv*

322 MATLAB Applications for the Practical Engineer

 d

d

*a l*

d

in which:

*l l*

*l*

*l* d

Equations (28) give the expressions for the speed components:

*xl yl zl*

*v h*

*l l l*

=

d

d

d d

() ,

and, by differentiating the velocity components (76), we get expressions:

 d

By derivation with respect to time, equations (77) imply:

é ù ¶

l

*y Rh h*

& & & &

= ×× + + × +×

,

*R*

¶

¶ =

 d

*R*

l

 

*l*

d

d

d d

*l*

*l*

*z h*

& &

=

d

6

 d

*l*

=(*I*<sup>3</sup> <sup>−</sup>*<sup>R</sup>*˜) <sup>⋅</sup> *<sup>R</sup>*

[ ] [ ] [ ] [ ] {[( ) ] [( ) ] }

*v R f RR f g v v*

<sup>r</sup> r r r )r) ) ) ) r r rr r % &

 d

*lv v v v a l l ll l*

<sup>2</sup> [ ] [0, 0, 3,08 10 ] [0, 0, 2 ] [0, 0, 2 ]. *<sup>P</sup>*

[ ] [0, sin , cos ] . *<sup>T</sup>*

( ) cos ,

( ) cos , ( ),

= +× × = +×

d

 dl

cos ( ) sin ( ) cos cos ,

 l  dl  dl

&

 l

&

( ),

= +× × = +×

,

from where are obtained the positioning errors on the axes of the ENU frame:

*x Rh y Rh z h*

.

*x R h R h h*

& && & &

= ×× - + × × + + × × + ×× ê ú ¶ ë û

 dl

 d

l

&

*v Rh v Rh*

l

W = -W× × W× × d

*KM <sup>h</sup> <sup>g</sup> g h <sup>h</sup>*

dw

= × - × × + - +W ´ - +W ´ = =-F × + × + - +W ´ - + W ´

⌢ *v l*

[ ] [ ] [ ] [ ] [( ) ] [( ) ] ,

*l v v v al l l l l*

 w

*fRf g v v*

, and we have:

 d

d

 d<sup>r</sup> (75)


r r r ) rr rr r r r (73)

 w

> dw d

*a a a*

d

(76)

(77)

(78)

$$\begin{split} \delta \upsilon\_{zl} &= (R\_{\dot{\lambda}} + h) \cdot \cos \phi \cdot \delta \dot{\lambda} - (R\_{\phi} + h) \cdot \sin \phi \cdot \dot{\lambda} \cdot \delta \phi + \cos \phi \cdot \dot{\lambda} \cdot \delta h, \\ \delta \upsilon\_{yl} &= \frac{\partial R\_{\phi}}{\partial \phi} \cdot \dot{\phi} \cdot \delta \phi + (R\_{\phi} + h) \cdot \delta \dot{\phi} + \dot{\phi} \cdot \delta h, \\ \delta \upsilon\_{zl} &= \delta \dot{h}\_{\prime} \end{split} \tag{82}$$

from where we obtain:

$$\begin{split} \delta \dot{\boldsymbol{x}}\_{l} &= \delta \boldsymbol{v}\_{xl} - (\boldsymbol{R}\_{\phi} + \boldsymbol{h}) \cdot \sin \phi \cdot [\dot{\boldsymbol{\phi}} \cdot \delta \boldsymbol{\ell} - \dot{\boldsymbol{\lambda}} \cdot \delta \boldsymbol{\phi}] + \cos \phi \cdot [\dot{\boldsymbol{h}} \cdot \delta \boldsymbol{\ell} - \dot{\boldsymbol{\lambda}} \cdot \delta \boldsymbol{h}], \\ \delta \dot{\boldsymbol{y}}\_{l} &= \delta \boldsymbol{v}\_{yl} + \dot{\boldsymbol{h}} \cdot \delta \boldsymbol{\phi} - \dot{\boldsymbol{\phi}} \cdot \delta \boldsymbol{h}, \\ \delta \dot{\boldsymbol{z}}\_{l} &= \delta \boldsymbol{v}\_{zl}. \end{split} \tag{83}$$

One observes that the derivative of the position error on the vertical channel is equal with the speed error.

If we denote:

$$\begin{bmatrix} \left[\vec{p}\right]\_l = \left[\delta p\_{x'} \cdot \delta p\_{y'} \cdot \delta p\_z\right]^T = \left[-\delta \phi , \cos\hat{\phi} \cdot \delta \lambda , \sin\hat{\phi} \cdot \delta \lambda\right]^T \end{bmatrix} \tag{84}$$

then:

$$\begin{aligned} \left[ \left[ \vec{p} \times \vec{v} \right] \right]\_l = \begin{bmatrix} \cos \phi \cdot \dot{h} \cdot \delta \dot{\lambda} - (R\_{\phi} + h) \cdot \sin \phi \cdot \dot{\phi} \cdot \delta \dot{\lambda} \\\\ (R\_{\lambda} + h) \cdot \sin \phi \cdot \cos \phi \cdot \dot{\lambda} \cdot \delta \dot{\lambda} + \dot{h} \cdot \delta \phi \\\\ -(R\_{\phi} + h) \cdot \dot{\phi} \cdot \delta \phi - (R\_{\lambda} + h) \cdot \cos^{2} \phi \cdot \dot{\lambda} \cdot \delta \dot{\lambda} \end{bmatrix}. \end{aligned} \tag{85}$$

The resulting model consists of a system of coupled differential equations and contains nine variables: three variables are errors in the determination of the attitude angles

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

variables of the model are the errors of the six inertial sensors used in the strap-down inertial navigation system. In addition to the nine variables, in the error model are involved the global positioning errors of the vehicle*δλ*, *δϕ*, *δh* , linking the nine differential equations. Numer‐ ical integration of the error model is rather difficult due to the couplings between its equations, but also due to the time evolution considered for inertial sensors errors. It can be performed, however, some numerical simulations, for different sources of error affecting the inertial sensors, in order to highlight their influence on the final errors of the navigation algorithm.

The validation of the navigation algorithm and of its error model is achieved by building Matlab/Simulink models for them followed by numerical simulation of these models for

Following is conducted a study of the dependence of the inertial navigator outputs errors by the errors of the used inertial sensors. For this purpose, the Matlab/Simulink models built for acceleration and gyro sensors are used; on the inertial navigator inputs are considered three miniaturized optical integrated accelerometers (MOEMS) and three fiber optic gyros with the associated errors according to their data sheets. Due to the fact that the accelerometer and gyro software developed models allow users to work independently with each sensor error in the theoretical model, are studied the influences of the noise, bias and scale factor sensors errors on the navigation solution components. Simulations are performed for three different navi‐ gation cases, the vehicle having the same initial position in all three cases: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration on the *x*-axis, 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along x-axis, which means the sensing of an acceleration of-0.0516 m/s2 (-0.0053 g) along *y*-axis.

Thus, starting from the navigation algorithm block scheme in Fig. 3 the Matlab/Simulink model in Fig. 4 is obtained. Also, the software implementation of the navigator error model leads to

With these two models it results the validation model in Fig. 6; "REAL" and "IDEAL" are blocks modelling the navigation algorithm (as in Fig. 4), having as inputs accelerations and angular speeds signals disturbed by the errors of strap-down inertial sensors, respectively undisturbed by the errors of strap-down inertial sensors. "ERROR" is a block by the form in Fig. 5. The input blocks "Acc" and "Gyro" are accelerometers and gyros models as in Fig. 1 and Fig. 2, and theirs outputs are applied to the "REAL" block. The values of the input constants are considered to be ideal signals, un-disturbed by the acceleration and rotation sensors, these

, *δvyl*

, *δ yl*

, *δzl*

http://dx.doi.org/10.5772/57583

, *δvzl* ), 325

). The input

(*δϕ*, *δθ*, *δψ*), three variables are errors in the determination of the speed (*δvxl*

and three variables are errors in determination of the position (*δxl*

**4. Numerical simulations**

several navigation particular cases.

the Matlab/Simulink model in Fig. 5.

being applied to the "IDEAL" block.

Also, having in mind the expressions for the angular speed components relative to the ECEF frame *ω* → *<sup>r</sup>* in ENU frame (eq. (14)), it results:

$$\begin{aligned} \left[ \left[ \vec{\phi} \right]\_r \times \delta \vec{r}' \right]\_l = \begin{bmatrix} \cos \phi \cdot \dot{\lambda} \cdot \delta h - (R\_\phi + h) \cdot \sin \phi \cdot \dot{\lambda} \cdot \delta \phi \\\\ (R\_\lambda + h) \cdot \sin \phi \cdot \cos \phi \cdot \dot{\lambda} \cdot \delta \vec{\lambda} + \dot{\phi} \cdot \delta h \\\\ -(R\_\phi + h) \cdot \dot{\phi} \cdot \delta \phi - (R\_\lambda + h) \cdot \cos^2 \phi \cdot \dot{\lambda} \cdot \delta \lambda \end{bmatrix} \end{aligned} \tag{86}$$

and, from there:

$$\begin{bmatrix} \left[\vec{p} \times \vec{v}\right]\_l - \left[\vec{\alpha}\_r \times \delta\vec{r}\right]\_l = \begin{bmatrix} \cos\phi \cdot (\dot{h} \cdot \delta\lambda - \dot{\lambda} \cdot \delta h) - (R\_\phi + h) \cdot \sin\phi \cdot (\dot{\phi} \cdot \delta\lambda - \dot{\lambda} \cdot \delta\phi) \\\\ \dot{h} \cdot \delta\phi - \dot{\phi} \cdot \delta h \\\\ 0 \end{bmatrix}. \tag{87}$$

With the notation:

$$[\delta\vec{r}']\_l = [\delta\dot{x}\_{l'} \cdot \delta\dot{y}\_{l'} \cdot \delta\dot{z}\_l]\_r \tag{88}$$

the equation characterizing the evolution in time of the positioning error (eq. (83)) becomes:

$$[\delta\vec{\bar{r}'}]\_l = [\delta\vec{\bar{v}}]\_l + [\vec{p} \times \vec{\bar{v}}]\_l - [\vec{\alpha}\_r \times \delta\vec{r}']\_l. \tag{89}$$

In conclusion, the error model of the navigation algorithm in terrestrial non-inertial reference frames by using attitude matrices is described by next equations:

$$\begin{cases} \left[\vec{\text{[\bar{O}]}}\right]\_l = -\left[\vec{\alpha}\vec{b}\_l \times \vec{\text{\bar{O}}}\right]\_l - \left(\vec{R}^v\_l\right)^T \cdot \left[\delta\vec{\alpha}\_v\right]\_v + \left[\delta\vec{\alpha}\right]\_l\right]\_{l'} \\ \left[\delta\vec{\bar{\nu}}\right]\_l = -\left[\vec{\text{d}}\right]\_l \times \left[\vec{f}\right]\_v + \vec{R}^l\_v \cdot \left[\delta\vec{f}\right]\_v + \left[\delta\vec{\bar{g}}\_a\right]\_l - \left[\left(\vec{\alpha}\_l + \vec{\Omega}\right) \times \delta\vec{\bar{v}}\right]\_l - \left[\left(\delta\vec{\alpha}\_l + \delta\vec{\Omega}\right) \times \vec{\bar{v}}\right]\_{l'} \\ \left[\delta\vec{\bar{r'}}\right]\_l = \left[\delta\vec{\bar{v}}\right]\_l + \left[\vec{p} \times \vec{\bar{v}}\right]\_l - \left[\vec{\alpha}\_r \times \delta\vec{\bar{r}'}\right]\_l \end{cases} \tag{90}$$

The resulting model consists of a system of coupled differential equations and contains nine variables: three variables are errors in the determination of the attitude angles (*δϕ*, *δθ*, *δψ*), three variables are errors in the determination of the speed (*δvxl* , *δvyl* , *δvzl* ), and three variables are errors in determination of the position (*δxl* , *δ yl* , *δzl* ). The input variables of the model are the errors of the six inertial sensors used in the strap-down inertial navigation system. In addition to the nine variables, in the error model are involved the global positioning errors of the vehicle*δλ*, *δϕ*, *δh* , linking the nine differential equations. Numer‐ ical integration of the error model is rather difficult due to the couplings between its equations, but also due to the time evolution considered for inertial sensors errors. It can be performed, however, some numerical simulations, for different sources of error affecting the inertial sensors, in order to highlight their influence on the final errors of the navigation algorithm.

### **4. Numerical simulations**

then:

frame *ω* →

and, from there:

With the notation:

2

 l dl

2

 l dl

> dl l d

& &&& (88)

<sup>r</sup> r rr r r & (89)

 l d

 d (85)

(86)

(90)

 dl

cos ( ) sin [ ] ( ) sin cos . ( ) ( ) cos

*pv R h h Rh Rh*

r r & &

´ = + × × ×× +×

 d

ld

r r & &

 d

´ = + × × ×× +× ¢

l

[ ] ( ) sin cos

*pv r h h*

d

 d

frames by using attitude matrices is described by next equations:

dw

d

w d

d

[ ] [ ] ( ) [ ] [ ],

*R*

<sup>r</sup> r rr <sup>r</sup> ) &

*v T l l l l vv l l l*

[ ] [ ] [ ] [ ].

*r v pv r*

<sup>r</sup> r rr r r &

<sup>ï</sup> ¢ ¢ = +´ - ´ <sup>î</sup>

<sup>ì</sup> F =- ´F - × + <sup>ï</sup>

w

 d

d

ï

ï

d

*l l lr l*

 dl l d

l

 dl

*h Rh*

é ù ×× - + × ×× ê ú

 l

Also, having in mind the expressions for the angular speed components relative to the ECEF

cos ( ) sin

*r Rh h Rh Rh*

*hRh*


é ù ×× - + × ×× ê ú

 l

 l dl d

& &

cos ( ) ( ) sin ( )

& & & &

d d

rr r r & & (87)

0

ë û

& &

( ) ( ) cos

[ ][ ] .

[ ] [ , , ], *l lll*

[ ] [ ] [ ] [ ]. *l l lr l*

[ ] [ ] [] [ ] [ ] [( ) ] [( ) ] ,

*v fRf g v v*

<sup>r</sup> <sup>r</sup> r r ) r rr r r r <sup>r</sup> &

 dw

í =-F + × + ´ - +W ´ - + W ´

 d

*l l vv v l a l ll l*

w

 d  dw d

*r v pv r* ¢ ¢ = +´ - ´

ddd*r xyz* ¢ <sup>=</sup> <sup>r</sup>

the equation characterizing the evolution in time of the positioning error (eq. (83)) becomes:

In conclusion, the error model of the navigation algorithm in terrestrial non-inertial reference

w d

é ù × × -× - + × × × -× ê ú ´-´ = ¢ × -×

*h h Rh*


 l dl

& &

& &

*l*

*<sup>r</sup>* in ENU frame (eq. (14)), it results:

*r l*

w d

324 MATLAB Applications for the Practical Engineer

*lr l*

w d The validation of the navigation algorithm and of its error model is achieved by building Matlab/Simulink models for them followed by numerical simulation of these models for several navigation particular cases.

Following is conducted a study of the dependence of the inertial navigator outputs errors by the errors of the used inertial sensors. For this purpose, the Matlab/Simulink models built for acceleration and gyro sensors are used; on the inertial navigator inputs are considered three miniaturized optical integrated accelerometers (MOEMS) and three fiber optic gyros with the associated errors according to their data sheets. Due to the fact that the accelerometer and gyro software developed models allow users to work independently with each sensor error in the theoretical model, are studied the influences of the noise, bias and scale factor sensors errors on the navigation solution components. Simulations are performed for three different navi‐ gation cases, the vehicle having the same initial position in all three cases: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration on the *x*-axis, 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along x-axis, which means the sensing of an acceleration of-0.0516 m/s2 (-0.0053 g) along *y*-axis.

Thus, starting from the navigation algorithm block scheme in Fig. 3 the Matlab/Simulink model in Fig. 4 is obtained. Also, the software implementation of the navigator error model leads to the Matlab/Simulink model in Fig. 5.

With these two models it results the validation model in Fig. 6; "REAL" and "IDEAL" are blocks modelling the navigation algorithm (as in Fig. 4), having as inputs accelerations and angular speeds signals disturbed by the errors of strap-down inertial sensors, respectively undisturbed by the errors of strap-down inertial sensors. "ERROR" is a block by the form in Fig. 5. The input blocks "Acc" and "Gyro" are accelerometers and gyros models as in Fig. 1 and Fig. 2, and theirs outputs are applied to the "REAL" block. The values of the input constants are considered to be ideal signals, un-disturbed by the acceleration and rotation sensors, these being applied to the "IDEAL" block.

The error model validation is realized through the comparison of the differences between the outputs of the "IDEAL" and "REAL" blocks with the outputs of the error model. In Fig. 7 a. are depicted the attitude angles errors, the first column containing the differences between the outputs of the "IDEAL" and "REAL" blocks, and the second column-the outputs of the error model. In the same mode are built Fig. 7 b. (for the positioning errors in ENU reference frame)

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

http://dx.doi.org/10.5772/57583

327

The reading errors of the accelerometers (*δ f xv*, *δ f yv*, *δ f zv*) and of the gyros (*δωxv*, *δωyv*, *δωzv*) applied at the error model inputs are presented in Fig. 8. For the acceler‐ ometers were neglected the effects of the cross-axis accelerations, while for the gyros were neglected the effects of the sensitivity to the accelerations; the data for three miniaturized optical integrated accelerometers (MOEMS) and three fiber optic gyros were used in sensors

The un-disturbed inputs were null on all rotation axes and on the *x* and *y* axes of acceleration, while for the *z* channel of acceleration the input was the local gravitational acceleration.

Analysing the error curves in all of the three parts of Fig. 7 can be easily concluded that the allures of the curves in the first columns are similarly with the allures of the curves in the second columns. As a consequence, the error model described by the equations (90) charac‐ terizes precisely the deviations of the attitude angles, and of the vehicle coordinates and speeds in ENU frame from their right values, free of the inertial sensors errors influence. On the other way, we can observe that the errors appearing in the altitude channel are much bigger than the errors in the two horizontal channels, and the attitude angles errors are comparable as

For the next steps of the numerical study, just the outputs of the error model in Fig. 6 are considered. As we already mention, simulations are performed for three different navigation cases, starting from the same initial position: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration in the North (x-axis) 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along x-axis, which

For the first case, the curves in Fig. 9 are obtained and the absolute maximal values of the

According to the graphical characteristics and to the numerical results presented in Table 1, the errors of the accelerometers and gyros scale factors have an insignificant weight in the increase of the attitude angles errors, accelerometers biases cause an increase with 0.26% percent of the roll and pitch angles errors and not affect the yaw angle error, and gyros biases have important weights in all attitude angles errors, producing an increase with 76.78% in roll angle error, with 95.52% in the pitch angle error and with 149.5% in the yaw angle error. Considering simultaneously the biases and scale factor errors at accelerometers produces an increase with 0.2432% of the error in the roll channel and with 0.26% in the pitch channel, while the error in the yaw channel in approximately constant. Can be observed that the combination of the two accelerometers errors with the noise has beneficial effects in the roll channel by

(-0.0053 g) along y-axis.

and Fig. 7 c. (for the speed errors in ENU reference frame).

values in all of the three channels, having an oscillatory behaviour.

means the sensing of an acceleration of-0.0516 m/s2

attitude, position and speed errors in Table 1.

models.

**Figure 4.** Matlab/Simulink model of the navigation algorithm.

**Figure 5.** Matlab/Simulink implementation of the inertial navigator error model.

The error model validation is realized through the comparison of the differences between the outputs of the "IDEAL" and "REAL" blocks with the outputs of the error model. In Fig. 7 a. are depicted the attitude angles errors, the first column containing the differences between the outputs of the "IDEAL" and "REAL" blocks, and the second column-the outputs of the error model. In the same mode are built Fig. 7 b. (for the positioning errors in ENU reference frame) and Fig. 7 c. (for the speed errors in ENU reference frame).

om\_v ~

326 MATLAB Applications for the Practical Engineer

2 om\_v

> 1 f\_v

Matrice Tilda

om\_l

A <sup>B</sup> A\*B Matrix Multiply2 f\_l

Matrice Tilda1

om\_l ~

A B A\*B Matrix Multiply

W+

om\_l

R P-l

ga\_P


om\_rel

A <sup>B</sup> A\*B Matrix Multiply5

**Figure 4.** Matlab/Simulink model of the navigation algorithm.

A <sup>B</sup> A\*B Matrix Multiply

Rv-l\_re d\_om\_v

Rv-l\_re df\_v

A <sup>B</sup> A\* B Matrix Multiply1

> A <sup>B</sup> <sup>C</sup> Cross Product C = AxB

d\_om\_l + d\_Om


sin(u) cos(u)

13 Phi\_re

12 d\_La

11 d\_Phi

10 v\_l

9 d\_Om

8 Om

7 d\_ga\_l

6 df\_v

5 f\_v

4 d\_om\_l

3 d\_om\_v

2 om\_l

1 Rv-l\_re W

Rv-l

A <sup>B</sup> A\*B Matrix Multiply1


v\_derivat v\_l h

u(3) v\_zl

1/s 1/s

u(2) v\_yl

> u(1) v\_xl

FI\_deriv FI

1/ s

om\_l + Om om\_l + Om

A <sup>B</sup> <sup>C</sup> Cross Product C = AxB

om\_l - Om dr\_l\_deriv dr\_l

v\_l dv\_l

v\_l p

A <sup>B</sup> <sup>C</sup> Cross Product C = AxB

**Figure 5.** Matlab/Simulink implementation of the inertial navigator error model.

sin(u) cos(u)

0

A <sup>B</sup> <sup>C</sup> Cross Product C = AxB 3x3 Cross Product

om\_l

1/s DCM2Eul

1/s

1/u 1/ (R\_fi+h) v\_l

uT

Matrix P-->l

Direction Cosine Matrix to Euler Angles

R l-P v\_P r\_P

A B A\*B Matrix Multiply3

1/s

r\_l

1/s

9 ga\_l

> Rv-l\_re om\_l d\_om\_v d\_om\_l f\_v df\_v d\_ga\_l Om d\_Om v\_l d\_Phi d\_La Phi\_re

d\_phi,theta,psi

ERROR

Error model

dv\_l

dr\_l

8 Rv-l 7 om\_l

6 Om

5 phi, theta, psi

4 h, Phi, La f\_v

r\_l r\_P v\_l h, Phi, La phi, theta, psi Om om\_l Rv-l ga\_l

om\_v

Navigation algorithm

3 v\_l

2 r\_P

1 r\_l

l

1/s

1/u 1/ (R\_la+h)

1/cos(u)

om\_l

FI

dv\_l

om\_l - Om

1/ s

A <sup>B</sup> <sup>C</sup> Cross Product C = AxB

A <sup>B</sup> <sup>C</sup> Cross Product C = AxB

dr\_l

3 dr\_l

2 dv\_l

1 d\_phi,theta,psi

dv\_l\_deriv dv\_l

1/ s

A <sup>B</sup> <sup>C</sup> Cross Product C = AxB

f(u) R\_la

f(u) R\_fi

The reading errors of the accelerometers (*δ f xv*, *δ f yv*, *δ f zv*) and of the gyros (*δωxv*, *δωyv*, *δωzv*) applied at the error model inputs are presented in Fig. 8. For the acceler‐ ometers were neglected the effects of the cross-axis accelerations, while for the gyros were neglected the effects of the sensitivity to the accelerations; the data for three miniaturized optical integrated accelerometers (MOEMS) and three fiber optic gyros were used in sensors models.

The un-disturbed inputs were null on all rotation axes and on the *x* and *y* axes of acceleration, while for the *z* channel of acceleration the input was the local gravitational acceleration.

Analysing the error curves in all of the three parts of Fig. 7 can be easily concluded that the allures of the curves in the first columns are similarly with the allures of the curves in the second columns. As a consequence, the error model described by the equations (90) charac‐ terizes precisely the deviations of the attitude angles, and of the vehicle coordinates and speeds in ENU frame from their right values, free of the inertial sensors errors influence. On the other way, we can observe that the errors appearing in the altitude channel are much bigger than the errors in the two horizontal channels, and the attitude angles errors are comparable as values in all of the three channels, having an oscillatory behaviour.

For the next steps of the numerical study, just the outputs of the error model in Fig. 6 are considered. As we already mention, simulations are performed for three different navigation cases, starting from the same initial position: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration in the North (x-axis) 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along x-axis, which means the sensing of an acceleration of-0.0516 m/s2 (-0.0053 g) along y-axis.

For the first case, the curves in Fig. 9 are obtained and the absolute maximal values of the attitude, position and speed errors in Table 1.

According to the graphical characteristics and to the numerical results presented in Table 1, the errors of the accelerometers and gyros scale factors have an insignificant weight in the increase of the attitude angles errors, accelerometers biases cause an increase with 0.26% percent of the roll and pitch angles errors and not affect the yaw angle error, and gyros biases have important weights in all attitude angles errors, producing an increase with 76.78% in roll angle error, with 95.52% in the pitch angle error and with 149.5% in the yaw angle error. Considering simultaneously the biases and scale factor errors at accelerometers produces an increase with 0.2432% of the error in the roll channel and with 0.26% in the pitch channel, while the error in the yaw channel in approximately constant. Can be observed that the combination of the two accelerometers errors with the noise has beneficial effects in the roll channel by

limiting the growth of error, compared to the situation in which is present only the bias. Proceeding similar for the gyros, the effect of the simultaneous considering of bias and scale factor errors is reflected by an increase with 76.77% of the error in the roll channel, with 95.52% in the pitch channel and with 149.5% in the yaw channel. Analysing the results obtained when all errors of the inertial sensors in IMU are taken into account, can be noticed an increase with 77.02% of the roll angle error, with 96.12% of the pitch angle error and with 149.5% of the yaw angle error. As a conclusion, the attitude angles errors are decisive influenced by the gyros biases (the strongest in the yaw channel), in a small degree by the accelerometers biases (in the roll and pitch channels) and negligible by the accelerometers and gyros scale factor errors.

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

Figure 7: Errors of attitude angles, positioning and speed at validation.

*xv f* [m/s ]

<sup>0</sup> <sup>10</sup> -1

<sup>0</sup> <sup>10</sup> <sup>0</sup>

[m/s] *yl v*

[m/s] *xl v*

<sup>0</sup> <sup>10</sup> -5

<sup>0</sup> <sup>10</sup> -5

[ ] <sup>o</sup>

[ ] <sup>o</sup>

<sup>0</sup> <sup>10</sup> -5

[ ] <sup>o</sup>

<sup>0</sup> <sup>10</sup> -0.06

[m/s] *zl v*

0.0265 0.027 0.0275 0.028 0.0285

*xv* [ /s] <sup>o</sup>


<sup>1</sup> x 10-3

<sup>0</sup> <sup>5</sup> <sup>10</sup> 0.026

2

0.05 0.1

c.


0.2 0.4

a. b.



> -0.5 0

*t*[s]

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1

*t*[s]

<sup>0</sup> <sup>5</sup> <sup>10</sup> -0.072

*zv*

2 *zv f*

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1


*yv* [ /s] <sup>o</sup>


<sup>1</sup> x 10-3



*yv f* [m/s ]

<sup>0</sup> <sup>10</sup> <sup>0</sup>

[m/s] *yl v*

[m/s] *xl v*

<sup>0</sup> <sup>10</sup> -3

<sup>0</sup> <sup>10</sup> <sup>0</sup>

[m] *<sup>l</sup> y*

[m] *<sup>l</sup> x*

<sup>0</sup> <sup>10</sup> -0.4

[m] *<sup>l</sup> z*

<sup>0</sup> <sup>10</sup> -0.06

[m/s] *zl v*

<sup>0</sup> <sup>10</sup> -1

Figure 8: Reading errors of the accelerometers and of the gyros.

<sup>0</sup> <sup>5</sup> <sup>10</sup> 0.0095

[ /s] <sup>o</sup>

[m/s ] 2

**Figure 7.** Errors of attitude angles, positioning and speed at validation.

0.05 0.1

0

0 5 <sup>10</sup> x 10-5

0 5 <sup>10</sup> x 10-5

*t*[s]

<sup>5</sup> x 10-5


> -0.5 0

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1.5

0.01 0.0105 0.011 0.0115

<sup>0</sup> <sup>10</sup> -5

[ ] <sup>o</sup>

[ ] <sup>o</sup>

<sup>0</sup> <sup>10</sup> -5

[ ] <sup>o</sup>

<sup>0</sup> <sup>10</sup> -5

0

0 5 <sup>10</sup> x 10-5

0 5 <sup>10</sup> x 10-5

<sup>5</sup> x 10-5


329

<sup>0</sup> <sup>10</sup> <sup>0</sup>

[m] *<sup>l</sup> y*

[m] *<sup>l</sup> x*

http://dx.doi.org/10.5772/57583

<sup>0</sup> <sup>10</sup> -0.4

[m] *<sup>l</sup> z*

<sup>0</sup> <sup>10</sup> -3

0.2 0.4


> -2 -1 0

*t*[s]

**Figure 6.** Matlab/Simulink validation model.

limiting the growth of error, compared to the situation in which is present only the bias. Proceeding similar for the gyros, the effect of the simultaneous considering of bias and scale factor errors is reflected by an increase with 76.77% of the error in the roll channel, with 95.52% in the pitch channel and with 149.5% in the yaw channel. Analysing the results obtained when all errors of the inertial sensors in IMU are taken into account, can be noticed an increase with 77.02% of the roll angle error, with 96.12% of the pitch angle error and with 149.5% of the yaw angle error. As a conclusion, the attitude angles errors are decisive influenced by the gyros biases (the strongest in the yaw channel), in a small degree by the accelerometers biases (in the roll and pitch channels) and negligible by the accelerometers and gyros scale factor errors.

<sup>0</sup> <sup>5</sup> <sup>10</sup> 0.026

2

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1

*t*[s]

<sup>0</sup> <sup>5</sup> <sup>10</sup> -0.072

*zv*

2 *zv f*

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1


*yv* [ /s] <sup>o</sup>


<sup>1</sup> x 10-3



*yv f* [m/s ]

0.0105 0.0275 **Figure 7.** Errors of attitude angles, positioning and speed at validation.

[m/s ] 2

<sup>0</sup> <sup>5</sup> <sup>10</sup> 0.0095

[ /s] <sup>o</sup>

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1.5

0.01


0.011 0.0115

Figure 8: Reading errors of the accelerometers and of the gyros.

Figure 7: Errors of attitude angles, positioning and speed at validation.

0.0265 0.027

*xv* [ /s] <sup>o</sup>


<sup>1</sup> x 10-3

0.028 0.0285

*xv f* [m/s ]

om\_real

Gyro y

om\_v

0

w1

om\_yl

ar

w

d\_h

d\_La

d\_Phi

Scope7

dv\_zl

dv\_xl

dz\_P

dx\_P

dz\_l

dx\_l

dy\_l

Scope4

dy\_P

Scope6

Scope5

328 MATLAB Applications for the Practical Engineer

dv\_yl

0

w1

om\_zl

ar

Gyro z

w

f\_ di eal

d\_f\_v

0

ai

f\_xl

ac

Acc x

a

f\_real

Acc y

dv\_zl1

Scope18

dx\_l1

Scope13

dz\_l1

Scope15

dy\_ 1l

dr\_l

Scope14

dv\_yl1

dv\_l

Scope17

dv\_xl1

Scope16

d\_ps1i

d\_theta1

d\_phi,theta,psi

Scope12

d\_ph 1i

Rv-l\_re

om\_l

d\_om\_v

d\_om\_l

f\_v

df\_v

d\_ga\_l

ERROR

Om

d\_Om

v\_l

d\_Phi

d\_La

Phi\_re

u(2)

Ph \_i re

Error model

Scope10

Scope11

d\_ga\_ 1l

Scope3

Scope2

Scope1

d\_ga\_l

d\_om\_l

d\_psi

d\_phi

d\_theta

Scope9

Scope8

0

ai

f\_v

r\_l

r\_

P

v\_l

h, Phi, La

REAL

phi, theta, psi Om

om\_l

Rv-l

ga\_l

Navigation algorithm1

f\_yl

ac

a


ai

ac

Acc z

a

om\_v

om\_ di eal

w

d\_om\_v

f\_v

r\_l

r\_

P

v\_l

h, Phi, La

IDEAL

phi, theta, psi Om

om\_l

Rv-l

ga\_l

Navigation algorithm

0

w1

om\_xl

**Figure 6.** Matlab/Simulink validation model.

ar

Gyro x

331

<sup>0</sup> <sup>50</sup> -4

<sup>o</sup>

http://dx.doi.org/10.5772/57583

<sup>0</sup> <sup>50</sup> -80

<sup>0</sup> <sup>50</sup> -3

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -100

<sup>0</sup> <sup>50</sup> -3

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1


<sup>o</sup> [ ]


> -2 -1 0

> > 0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]


> -2 -1 0

> > 0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



Figure 9. Simulation results for the first navigation case.

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -0.04

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[ ]

<sup>0</sup> <sup>50</sup> -5

<sup>0</sup> <sup>50</sup> -4

<sup>0</sup> <sup>50</sup> -0.2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -2

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -10

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

*t*[s]

*BGyro* ≠ 0

*t*[s]

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

[ ]

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[ ]

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -0.04

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -0.04

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

0 50

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -2

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -2

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

*t*[s]

*K Gyro* ≠ 0

*t*[s]

0 2 <sup>4</sup> x 10-4

<sup>o</sup> [ ]



> 0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

0 2 4

0.05 0.1

> 0 <sup>2</sup> x 10 -4

<sup>o</sup> [ ]

0 2 4

0.05 0.1

0 2 <sup>4</sup> x 10-4

Just noise All non-null

5 10

0.2 0.4

*BAcc* ≠ 0 *KAcc* ≠ 0

0 2 <sup>4</sup> x 10-4



> 0 2 <sup>4</sup> x 10-4



Regarding the positioning errors we can observe that: the scale factor error of gyros does not produce any increase in the errors of the three channels; the accelerometers scale factor error influences negligible the errors increase in the horizontal channels (with 0.00968% in *x* channel and with 0.00962% in *y* channel) and influences strongly the increase of the vertical channel error (with 280.33%); the accelerometers bias influences strongly the increase of the errors in

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -30

<sup>0</sup> <sup>50</sup> -1

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -4

<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



> 0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



> -2 0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

0 2 4

0.05 0.1

> 0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



> 0 2 <sup>4</sup> x 10-4

<sup>o</sup> [ ]

2 4 6

0.1 0.2

**Figure 9.** Simulation results for the first navigation case.

0 2 <sup>4</sup> x 10-4



> 0 2 <sup>4</sup> x 10-4

5 10 15

0.2 0.4

> 0 5 <sup>10</sup> x 10-4


0 0.2

For the next steps of the numerical study, just the outputs of the error model in Fig. 6 are considered. As we already mention, simulations are performed for three different navigation cases, starting from the same initial position: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration in the North (x-axis) 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running

For the first case, the curves in Fig. 9 are obtained and the absolute maximal values of the attitude,

**Figure 8.** Reading errors of the accelerometers and of the gyros. on the track with acceleration 0.1 g along x-axis, which means the sensing of an acceleration of (-0.0053 g) along y-axis.

position and speed errors in Table 1.



Table 1. Absolute maximal values of the attitude, position and speed errors for first navigation case.

of the accelerometers and gyros scale factors have an insignificant weight in the increase of the attitude angles errors, accelerometers biases cause an increase with 0.26% percent of the roll and pitch angles errors and not affect the yaw angle error, and gyros biases have important weights in all attitude angles errors, producing an increase with 76.78% in roll angle error, with 95.52% in the pitch angle error and with 149.5% in the yaw angle error. Considering simultaneously the biases and scale factor errors at accelerometers produces an increase with 0.2432% of the error in the roll channel and with 0.26% in the pitch channel, while the error in the yaw channel in approximately constant. Can be observed that the combination of the two accelerometers errors with the noise has beneficial effects in the roll channel by limiting the growth of error, compared to the situation in which is present only the bias. Proceeding similar for the gyros, the effect of the simultaneous considering of bias and scale factor errors is reflected by an increase with 76.77% of the error in the roll channel, with 95.52% in the pitch channel and with 149.5% in the yaw channel. Analysing the results obtained when all errors of the inertial sensors in IMU are taken into account, can be noticed an increase with 77.02% of the roll angle error, with 96.12% of the pitch angle error and with 149.5% of the yaw angle error. As a conclusion, the attitude angles errors are decisive influenced by the gyros biases (the strongest in the yaw channel), in a small degree by the accelerometers biases (in the roll and pitch channels) and

According to the graphical characteristics and to the numerical results presented in Table 1, the errors **Table 1.** Absolute maximal values of the attitude, position and speed errors for first navigation case.

negligible by the accelerometers and gyros scale factor errors.

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System... http://dx.doi.org/10.5772/57583 331

0 5 10

]/s[o

]m/s[ 2

330 MATLAB Applications for the Practical Engineer

0 5 10

position and speed errors in Table 1.

All null (just noise) 4

0 *BGiro* & 0 *K Giro* <sup>4</sup>

All non-null 4

**Figure 8.** Reading errors of the accelerometers and of the gyros.

3.6132 10

6.3874 10

6.3964 10

(-0.0053 g) along y-axis.

0 5 10

*xv* dw ]/s[o dw*yv* ]/s[o

Analysing the error curves in all of the three parts of Fig. 7 can be easily concluded that the allures of the curves in the first columns are similarly with the allures of the curves in the second columns. As a consequence, the error model described by the equations (90) characterizes precisely the deviations of the attitude angles, and of the vehicle coordinates and speeds in ENU frame from their right values, free of the inertial sensors errors influence. On the other way, we can observe that the errors appearing in the altitude channel are much bigger than the errors in the two horizontal channels, and the attitude angles errors are comparable as values in all of the three channels, having an oscillatory

2

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1

*t* ]s[

For the next steps of the numerical study, just the outputs of the error model in Fig. 6 are considered. As we already mention, simulations are performed for three different navigation cases, starting from the same initial position: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration in the North (x-axis) 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along x-axis, which means the sensing of an acceleration of

For the first case, the curves in Fig. 9 are obtained and the absolute maximal values of the attitude,

 <sup>4</sup> 1.5281 10

 <sup>4</sup> 1.5281 10

 <sup>4</sup> 1.5281 10

 <sup>4</sup> 1.5281 10

 <sup>4</sup> 3.8127 10

 <sup>4</sup> 1.5281 10

 <sup>4</sup> 3.8127 10

 <sup>4</sup> 3.8127 10

**Table 1.** Absolute maximal values of the attitude, position and speed errors for first navigation case.

Table 1. Absolute maximal values of the attitude, position and speed errors for first navigation case.

According to the graphical characteristics and to the numerical results presented in Table 1, the errors of the accelerometers and gyros scale factors have an insignificant weight in the increase of the attitude angles errors, accelerometers biases cause an increase with 0.26% percent of the roll and pitch angles errors and not affect the yaw angle error, and gyros biases have important weights in all attitude angles errors, producing an increase with 76.78% in roll angle error, with 95.52% in the pitch angle error and with 149.5% in the yaw angle error. Considering simultaneously the biases and scale factor errors at accelerometers produces an increase with 0.2432% of the error in the roll channel and with 0.26% in the pitch channel, while the error in the yaw channel in approximately constant. Can be observed that the combination of the two accelerometers errors with the noise has beneficial effects in the roll channel by limiting the growth of error, compared to the situation in which is present only the bias. Proceeding similar for the gyros, the effect of the simultaneous considering of bias and scale factor errors is reflected by an increase with 76.77% of the error in the roll channel, with 95.52% in the pitch channel and with 149.5% in the yaw channel. Analysing the results obtained when all errors of the inertial sensors in IMU are taken into account, can be noticed an increase with 77.02% of the roll angle error, with 96.12% of the pitch angle error and with 149.5% of the yaw angle error. As a conclusion, the attitude angles errors are decisive influenced by the gyros biases (the strongest in the yaw channel), in a small degree by the accelerometers biases (in the roll and pitch channels) and

0 5 10

*zv* dw

2 *zv* d*f*

<sup>0</sup> <sup>5</sup> <sup>10</sup> -1



] Position errors [m] Speed errors [m/s]

1.0329 2.0788 17.6175 0.0321 0.0984 0.5878

11.688 6.4823 28.2643 0.3937 0.1876 0.943

1.033 2.079 67.0058 0.0321 0.0984 2.2356

11.6629 6.3902 77.6825 0.3929 0.1846 2.5918

2.503 4.0711 17.6175 0.1028 0.1981 0.5878

1.0328 2.0788 17.6175 0.0321 0.0984 0.5878

2.503 4.0711 17.6175 0.1028 0.1981 0.5878

10.1927 4.3979 77.6825 0.3195 0.0981 2.5918

*<sup>l</sup> x <sup>l</sup> y <sup>l</sup> z xl v yl v zl v*

0

0.5

<sup>1</sup> x 10-3




*yv* d*f* ]m/s[

0.026 0.0265 0.027 0.0275 0.028 0.0285

*xv* d*f* ]m/s[


Attitude angles errors [<sup>o</sup>

 <sup>4</sup> 1.5614 10

 <sup>4</sup> 1.5654 10

 <sup>4</sup> 1.5614 10

 <sup>4</sup> 1.5654 10

 <sup>4</sup> 3.0529 10

 <sup>4</sup> 1.5614 10

 <sup>4</sup> 3.0529 10

 <sup>4</sup> 3.0623 10

4 3.6226 10

4 3.6132 10

4 3.6221 10

4 6.3875 10

4 3.6132 10

negligible by the accelerometers and gyros scale factor errors.

0

0.5

<sup>1</sup> x 10-3

0.0095

18

<sup>1</sup> x 10-3


behaviour.


Sensors errors

0 *BAcc*

0 *KAcc*

0 *BAcc* & 0 *KAcc*

0 *BGiro*

0 *K Giro*

0.01 0.0105

0.011 0.0115

> Regarding the positioning errors we can observe that: the scale factor error of gyros does not produce any increase in the errors of the three channels; the accelerometers scale factor error influences negligible the errors increase in the horizontal channels (with 0.00968% in *x* channel and with 0.00962% in *y* channel) and influences strongly the increase of the vertical channel error (with 280.33%); the accelerometers bias influences strongly the increase of the errors in

horizontal channels (with 1031.57% in *x* channel and with 211.82% in *y* channel) and with 60.43% in the vertical channel; the gyros bias does not affect the error in vertical channel but has an important influence regarding the increase of the errors in horizontal channels (with 142.32% in *x* channel and with 94.39% in *y* channel); the simultaneous considering of the bias and of the scale factor error for the accelerometers has as result an increase of the error with 1029.14% in *x* channel, with 207.39% in *y* channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combination of the two errors in the presence of noise, but only in the horizontal channels, in the vertical channel a negative effect is achieved in this regard); the simultaneous considering of the bias and of the scale factor error for the gyros has as result an increase of the error with 142.32% in *x* channel and with 94.39% in *y* channel, and does not affect the value of the error in vertical channel; the simultaneous considering of all errors of the inertial sensors in IMU leads to the errors increases with 886.8% in *x* channel, with 111.55% in y channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combi‐ nation of all errors in the presence of noise, but only in the horizontal channels, in the vertical channel, keeping the results obtained when all errors of accelerometers were taken into account). Therefore, the positioning errors are negligibly influenced by the scale factor errors of the sensors in IMU, excepting a strong influence on the vertical channel induced by the accelerometers scale factor errors (280.33%), the accelerometers biases have strong influences on all channels (the biggest is on *x* channel (1031.57%)), and the gyros biases have strong influences on the horizontal channels while in the vertical channel theirs effects are negligible. 20 Regarding the positioning errors we can observe that: the scale factor error of gyros does not produce any increase in the errors of the three channels; the accelerometers scale factor error influences negligible the errors increase in the horizontal channels (with 0.00968% in *x* channel and with 0.00962% in *y* channel) and influences strongly the increase of the vertical channel error (with 280.33%); the accelerometers bias influences strongly the increase of the errors in horizontal channels (with 1031.57% in *x* channel and with 211.82% in *y* channel) and with 60.43% in the vertical channel; the gyros bias does not affect the error in vertical channel but has an important influence regarding the increase of the errors in horizontal channels (with 142.32% in *x* channel and with 94.39% in *y* channel); the simultaneous considering of the bias and of the scale factor error for the accelerometers has as result an increase of the error with 1029.14% in *x* channel, with 207.39% in *y* channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combination of the two errors in the presence of noise, but only in the horizontal channels, in the vertical channel a negative effect is achieved in this regard); the simultaneous considering of the bias and of the scale factor error for the gyros has as result an increase of the error with 142.32% in *x* channel and with 94.39% in *y* channel, and does not affect the value of the error in vertical channel; the simultaneous considering of all errors of the inertial sensors in IMU leads to the errors increases with 886.8% in *x* channel, with 111.55% in y channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combination of all errors in the presence of noise, but only in the horizontal channels, in the vertical channel, keeping the results obtained when all errors of accelerometers were taken into account). Therefore, the positioning errors are negligibly influenced by the scale factor errors of the sensors in IMU, excepting a strong influence on the vertical channel induced by the accelerometers scale factor

Comparing the numerical results in Table 2 with those in Table 1 it can be seen that the percentage of errors influences are almost the same, the only noticeable change being that of

Just noise All non-null

0 5 <sup>10</sup> x 10-4

5 10 15

0.2 0.4

*BGyro* ≠ 0 *K Gyro* ≠ 0

0.02 0.04

*BAcc* ≠ 0 *KAcc* ≠ 0

0 2 <sup>4</sup> x 10-4

0.5 1

> 0 2 <sup>4</sup> x 10-4

<sup>0</sup> <sup>50</sup> -4

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -1

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -1

<sup>0</sup> <sup>50</sup> -30

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1


<sup>o</sup> [ ]




It notes, also, the effect of non-zero entry in acceleration, on the *x* axis of the navigator, on the change of the weights held by the accelerometers scale factor errors in the calculus of the

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -5

[ ]

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -0.1

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -6

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.1

*t*[s]

0 2 <sup>4</sup> x 10-4

<sup>o</sup> [ ]


[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -2

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]


> 0 2 4

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -2

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -4

<sup>o</sup>

http://dx.doi.org/10.5772/57583

<sup>0</sup> <sup>50</sup> -100


<sup>o</sup> [ ]


> -2 -1 0

<sup>0</sup> <sup>50</sup> -100

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]


> -2 -1 0

<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

0.05 0.1

> 0 2 4

0.05 0.1 21

333

accelerometers and gyros biases effect on the positioning error in *x* channel.

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -20

Figure 10. Simulation results for the second navigation case.

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -2

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

0 2 4

0.05 0.1

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -10

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.2

*t*[s]

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]


[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

0 2 <sup>4</sup> x 10-4

<sup>o</sup> [ ]


> 2 4 6

0.1 0.2 0.3

**Figure 10.** Simulation results for the second navigation case.

<sup>0</sup> <sup>50</sup> -5

[ ]

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -0.1

0 2 <sup>4</sup> x 10-4



<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

0 2 <sup>4</sup> x 10-4

5 10

0.2 0.4

<sup>0</sup> <sup>50</sup> -6

<sup>0</sup> <sup>50</sup> -0.2

0 5 <sup>10</sup> x 10-4



position errors.

For the second navigation case, are considered the taxiing of aircraft in which it is mounted the inertial navigator, starting from the same initial position and the same conditions of speed and attitude as in the first case, with the acceleration of 0.1 g along the *x* axis. With this case the aim is to study the influence of sensor errors in the navigator errors when on one of the horizontal axis of the accelerometer is applied a non-zero acceleration. By performing numer‐ ical simulations for the same cases of influence of sensor errors, the absolute maximal values of the navigator errors in Table 2 and the graphical characteristics in Fig. 10 are obtained. errors (280.33%), the accelerometers biases have strong influences on all channels (the biggest is on *x* channel (1031.57%)), and the gyros biases have strong influences on the horizontal channels while in the vertical channel theirs effects are negligible. For the second navigation case, are considered the taxiing of aircraft in which it is mounted the inertial navigator, starting from the same initial position and the same conditions of speed and attitude as in the first case, with the acceleration of 0.1 g along the *x* axis. With this case the aim is to study the influence of sensor errors in the navigator errors when on one of the horizontal axis of the accelerometer is applied a non-zero acceleration. By performing numerical simulations for the same cases of influence of sensor errors, the absolute maximal values of the navigator errors in Table 2 and


Table 2. Absolute maximal values of the attitude and position errors for the second navigation case.

errors influences are almost the same, the only noticeable change being that of accelerometers and

Comparing the numerical results in Table 2 with those in Table 1 it can be seen that the percentage of **Table 2.** Absolute maximal values of the attitude and position errors for the second navigation case.

the graphical characteristics in Fig. 10 are obtained.

gyros biases effect on the positioning error in *x* channel.

Comparing the numerical results in Table 2 with those in Table 1 it can be seen that the percentage of errors influences are almost the same, the only noticeable change being that of accelerometers and gyros biases effect on the positioning error in *x* channel. 21

horizontal channels (with 1031.57% in *x* channel and with 211.82% in *y* channel) and with 60.43% in the vertical channel; the gyros bias does not affect the error in vertical channel but has an important influence regarding the increase of the errors in horizontal channels (with 142.32% in *x* channel and with 94.39% in *y* channel); the simultaneous considering of the bias and of the scale factor error for the accelerometers has as result an increase of the error with 1029.14% in *x* channel, with 207.39% in *y* channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combination of the two errors in the presence of noise, but only in the horizontal channels, in the vertical channel a negative effect is achieved in this regard); the simultaneous considering of the bias and of the scale factor error for the gyros has as result an increase of the error with 142.32% in *x* channel and with 94.39% in *y* channel, and does not affect the value of the error in vertical channel; the simultaneous considering of all errors of the inertial sensors in IMU leads to the errors increases with 886.8% in *x* channel, with 111.55% in y channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combi‐ nation of all errors in the presence of noise, but only in the horizontal channels, in the vertical channel, keeping the results obtained when all errors of accelerometers were taken into account). Therefore, the positioning errors are negligibly influenced by the scale factor errors of the sensors in IMU, excepting a strong influence on the vertical channel induced by the accelerometers scale factor errors (280.33%), the accelerometers biases have strong influences on all channels (the biggest is on *x* channel (1031.57%)), and the gyros biases have strong influences on the horizontal channels while in the vertical channel theirs effects are negligible.

Regarding the positioning errors we can observe that: the scale factor error of gyros does not produce any increase in the errors of the three channels; the accelerometers scale factor error influences negligible the errors increase in the horizontal channels (with 0.00968% in *x* channel and with 0.00962% in *y* channel) and influences strongly the increase of the vertical channel error (with 280.33%); the accelerometers bias influences strongly the increase of the errors in horizontal channels (with 1031.57% in *x* channel and with 211.82% in *y* channel) and with 60.43% in the vertical channel; the gyros bias does not affect the error in vertical channel but has an important influence regarding the increase of the errors in horizontal channels (with 142.32% in *<sup>x</sup>* channel and with 94.39% in *<sup>y</sup>* channel); the simultaneous considering of the bias and of the scale factor error for the accelerometers has as result an increase of the error with 1029.14% in *x* channel, with 207.39% in *y* channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combination of the two errors in the presence of noise, but only in the horizontal channels, in the vertical channel a negative effect is achieved in this regard); the simultaneous considering of the bias and of the scale factor error for the gyros has as result an increase of the error with 142.32% in *<sup>x</sup>* channel and with 94.39% in *y* channel, and does not affect the value of the error in vertical channel; the simultaneous considering of all errors of the inertial sensors in IMU leads to the errors increases with 886.8% in *x* channel, with 111.55% in y channel and with 340.93% in vertical channel (it is noted a beneficial effect of the combination of all errors in the presence of noise, but only in the horizontal channels, in the vertical channel, keeping the results obtained when all errors of accelerometers were taken into account). Therefore, the positioning errors are negligibly influenced by the scale factor errors of the sensors in IMU, excepting a strong influence on the vertical channel induced by the accelerometers scale factor errors (280.33%), the accelerometers biases have strong influences on all channels (the biggest is on *x* channel (1031.57%)), and the gyros biases have strong influences on the horizontal channels while in

20

332 MATLAB Applications for the Practical Engineer

For the second navigation case, are considered the taxiing of aircraft in which it is mounted the inertial navigator, starting from the same initial position and the same conditions of speed and attitude as in the first case, with the acceleration of 0.1 g along the *x* axis. With this case the aim is to study the influence of sensor errors in the navigator errors when on one of the horizontal axis of the accelerometer is applied a non-zero acceleration. By performing numer‐ ical simulations for the same cases of influence of sensor errors, the absolute maximal values of the navigator errors in Table 2 and the graphical characteristics in Fig. 10 are obtained.

For the second navigation case, are considered the taxiing of aircraft in which it is mounted the inertial navigator, starting from the same initial position and the same conditions of speed and attitude as in the first case, with the acceleration of 0.1 g along the *x* axis. With this case the aim is to study the influence of sensor errors in the navigator errors when on one of the horizontal axis of the accelerometer is applied a non-zero acceleration. By performing numerical simulations for the same cases of influence of sensor errors, the absolute maximal values of the navigator errors in Table 2 and

] Position errors [m]

2.7912 2.0834 17.5142

9.9327 6.4778 28.1575

0.7522 2.068 66.9016

13.4510 6.4012 77.5747

4.2608 4.3299 17.3668

2.7912 2.0834 17.5142

4.2608 4.3299 17.3668

11.9814 4.1546 77.4273

*<sup>l</sup> x <sup>l</sup> y <sup>l</sup> z*

 <sup>4</sup> 1.5284 10

 <sup>4</sup> 1.5284 10

 <sup>4</sup> 1.5284 10

 <sup>4</sup> 1.5284 10

 <sup>4</sup> 3.8113 10

 <sup>4</sup> 1.5284 10

 <sup>4</sup> 3.8113 10

 <sup>4</sup> 3.8113 10

Table 2. Absolute maximal values of the attitude and position errors for the second navigation case.

Comparing the numerical results in Table 2 with those in Table 1 it can be seen that the percentage of errors influences are almost the same, the only noticeable change being that of accelerometers and

Attitude angles errors [<sup>o</sup>

 <sup>4</sup> 1.5608 10

 <sup>4</sup> 1.5649 10

 <sup>4</sup> 1.5619 10

 <sup>4</sup> 1.5660 10

 <sup>4</sup> 3.0516 10

 <sup>4</sup> 1.5608 10

 <sup>4</sup> 3.0516 10

 <sup>4</sup> 3.0636 10

**Table 2.** Absolute maximal values of the attitude and position errors for the second navigation case.

the vertical channel theirs effects are negligible.

the graphical characteristics in Fig. 10 are obtained.

3.6130 10

3.6220 10

6.3969 10

gyros biases effect on the positioning error in *x* channel.

4 3.6220 10

4 3.6130 10

4 6.3879 10

4 3.6130 10

4 6.3879 10

Sensors errors

0 *BAcc*

0 *KAcc*

0 *BGiro*

0 *K Giro*

0 *BGiro* & 0 *K Giro*

All null (just noise) 4

0 *BAcc* & 0 *KAcc* <sup>4</sup>

All non-null 4

Figure 10. Simulation results for the second navigation case. **Figure 10.** Simulation results for the second navigation case.

It notes, also, the effect of non-zero entry in acceleration, on the *x* axis of the navigator, on the change of the weights held by the accelerometers scale factor errors in the calculus of the position errors.

Subjecting the vehicle to a gyration angular speed of 0.1o /s, while running on the track with the acceleration 0.1 g along the *x* axis, it has a change of the heading angle of 6 degrees after 1 min and a lateral deviation of 185.4039 m a distance of 1764 m on the East direction. Because the speed is close to the take-off limit, the motion is equivalent to the horizontal movement on a circular arc with a radius of about 16.907 km, which means the sensing of an acceleration of-0.0516 m/s2 (-0.0053 g) along the *y* axis of the vehicle. Considering the same initial conditions as in the two previous cases, the graphical characteristics of the errors obtained through the numerical simulation are given in Fig. 11, while the absolute maximal values of the navigator errors are shown in Table 3.

the same for the three simulated cases. The errors combining in the general case compared to particular cases, in which are studied the influences of each sensor error, is explained by positive and negative values of the parameters found in Table 4, but also by the crossings in the positive and negative area of the error characteristics in Fig. 9. Also, the high values of position errors in the vertical channel are due largely to the presence of 1g acceleration on the z axis of the navigator, amplifying in this way the errors of the accelerometer in this channel. <sup>23</sup>

*BGyro* ≠ 0 *K Gyro* ≠ 0

0.01 0.02 0.03

*BAcc* ≠ 0 *KAcc* ≠ 0

0 2 <sup>4</sup> x 10-4

0.5 1

> 0 2 <sup>4</sup> x 10-4

<sup>0</sup> <sup>50</sup> -4

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -1

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -20

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]


<sup>0</sup> <sup>50</sup> -30

<sup>0</sup> <sup>50</sup> -1

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]


> -20 -10 0


<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1


<sup>o</sup> [ ]




<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[ ]

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

0.5 <sup>1</sup> x 10-3

5 10 15

0.2 0.4

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -0.1

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

All non-null

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -6

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.2

*t*[s]

0 2 <sup>4</sup> x 10-4

<sup>o</sup> [ ]



<sup>0</sup> <sup>50</sup> -1

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.1

*t*[s]

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

0 1 2

0 0.1

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -2

<sup>o</sup>

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -4

<sup>o</sup>

http://dx.doi.org/10.5772/57583

335

<sup>0</sup> <sup>50</sup> -100


<sup>o</sup> [ ]


<sup>0</sup> <sup>50</sup> -100

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



> -2 -1 0

<sup>0</sup> <sup>50</sup> -20

<sup>0</sup> <sup>50</sup> -1

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]



0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

1 2 3

0.05 0.1

Figure 11. Simulation results for the third navigation case.

<sup>0</sup> <sup>50</sup> -2

<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> -10

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> -0.2

*t*[s]

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

0.05 0.1

> -5 0

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

0 2 <sup>4</sup> x 10-4

<sup>o</sup> [ ]


> 2 4 6

0.2 0.4

**Figure 11.** Simulation results for the third navigation case.

<sup>0</sup> <sup>50</sup> -5

[ ]

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -2

[ ]

<sup>0</sup> <sup>50</sup> -3

<sup>0</sup> <sup>50</sup> -0.1

0 2 <sup>4</sup> x 10-4



<sup>0</sup> <sup>50</sup> <sup>0</sup>

<sup>0</sup> <sup>50</sup> <sup>0</sup>

0 2 <sup>4</sup> x 10-4

5 10 15

0.2 0.4

<sup>0</sup> <sup>50</sup> -6

<sup>0</sup> <sup>50</sup> -0.2

0 5 <sup>10</sup> x 10-4



<sup>0</sup> <sup>50</sup> -2

[m] *<sup>l</sup> x* [m] *<sup>l</sup> y* [m] *<sup>l</sup> z*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

[m/s] *xl v* [m/s] *yl v* [m/s] *zl v*

<sup>0</sup> <sup>50</sup> <sup>0</sup>

*t*[s]

0 <sup>2</sup> x 10-4

<sup>o</sup> [ ]

1 2 3


**Table 3.** Absolute maximal values of the attitude and position errors for the third navigation case.

From the graphic and numeric results, it is found the maintaining approximately constant of the final percentages of influence in the navigator error even if the acceleration is not zero in the all three axes of the vehicle, and the angular speed is non-zero on the *z* axis. Also, can be observed the exercise of a stronger influence on the navigator *y* axis by the scale factor error of accelerometer *y*, even the acceleration applied to this axis is the smallest. The big value of the percent (-42.84%) is due to the higher negative value of the scale factor error of acceler‐ ometer *y* (-1.08%) comparatively with the other two accelerometers (-0.2% on *x* axis, and 0.28% on *z* axis) (see Table 4). Having non-zero angular speed on the z axis, can be seen a small influence of gyros scale factor errors in yaw channel.


**Table 4.** Bias and scale factor error for inertial sensors used in simulations.

Assessing the positioning errors in differences relative to the situation when it is considered only the sensors noise, it is observed that the influence of the sensors errors is approximately

the same for the three simulated cases. The errors combining in the general case compared to particular cases, in which are studied the influences of each sensor error, is explained by positive and negative values of the parameters found in Table 4, but also by the crossings in the positive and negative area of the error characteristics in Fig. 9. Also, the high values of position errors in the vertical channel are due largely to the presence of 1g acceleration on the z axis of the navigator, amplifying in this way the errors of the accelerometer in this channel. <sup>23</sup>

Subjecting the vehicle to a gyration angular speed of 0.1o

of-0.0516 m/s2

errors are shown in Table 3.

334 MATLAB Applications for the Practical Engineer

Sensors errors

Sensors

the acceleration 0.1 g along the *x* axis, it has a change of the heading angle of 6 degrees after 1 min and a lateral deviation of 185.4039 m a distance of 1764 m on the East direction. Because the speed is close to the take-off limit, the motion is equivalent to the horizontal movement on a circular arc with a radius of about 16.907 km, which means the sensing of an acceleration

as in the two previous cases, the graphical characteristics of the errors obtained through the numerical simulation are given in Fig. 11, while the absolute maximal values of the navigator

All null (just noise) 3.6773⋅10−<sup>4</sup> 1.6036⋅10−<sup>4</sup> 1.5284⋅10−<sup>4</sup> 2.8475 2.1074 17.4978 *BAcc* ≠0 3.6823⋅10−<sup>4</sup> 1.6078⋅10−<sup>4</sup> 1.5284⋅10−<sup>4</sup> 10.1635 6.0022 28.1410 Δ*KAcc* ≠0 3.6749⋅10−<sup>4</sup> 1.6048⋅10−<sup>4</sup> 1.5284⋅10−<sup>4</sup> 0.7238 1.2121 66.8852 *BAcc* ≠0 & Δ*KAcc* ≠0 3.6832⋅10−<sup>4</sup> 1.6088⋅10−<sup>4</sup> 1.5284⋅10−<sup>4</sup> 13.7066 6.8065 77.5581 *BGiro* ≠0 6.3662⋅10−<sup>4</sup> 3.1738⋅10−<sup>4</sup> 3.8113⋅10−<sup>4</sup> 4.3550 4.3143 17.3350 Δ*KGiro* ≠0 3.6737⋅10−<sup>4</sup> 1.6036⋅10−<sup>4</sup> 1.4694⋅10−<sup>4</sup> 2.8472 2.1125 17.4978 *BGiro* ≠0 & Δ*KGiro* ≠0 6.3662⋅10−<sup>4</sup> 3.1738⋅10−<sup>4</sup> 3.8955⋅10−<sup>4</sup> 4.3547 4.3194 17.335 All non-null 6.3760⋅10−<sup>4</sup> 3.1860⋅10−<sup>4</sup> 3.8955⋅10−<sup>4</sup> 12.1993 4.5943 77.3953

From the graphic and numeric results, it is found the maintaining approximately constant of the final percentages of influence in the navigator error even if the acceleration is not zero in the all three axes of the vehicle, and the angular speed is non-zero on the *z* axis. Also, can be observed the exercise of a stronger influence on the navigator *y* axis by the scale factor error of accelerometer *y*, even the acceleration applied to this axis is the smallest. The big value of the percent (-42.84%) is due to the higher negative value of the scale factor error of acceler‐ ometer *y* (-1.08%) comparatively with the other two accelerometers (-0.2% on *x* axis, and 0.28% on *z* axis) (see Table 4). Having non-zero angular speed on the z axis, can be seen a small

Bias Scale factor error

*%* −3.12⋅10−<sup>4</sup>

*%* −1.456⋅10−<sup>4</sup>

*%*

*x* Axis *y* Axis *z* Axis *x* Axis *y* Axis *z* Axis

Acc. −0.00709128 m/s<sup>2</sup> 0.00472752 m/s<sup>2</sup> 0.0059094 m/s<sup>2</sup> −0.2 *%* −1.08 *%* 0.28 *%*

Assessing the positioning errors in differences relative to the situation when it is considered only the sensors noise, it is observed that the influence of the sensors errors is approximately

**Table 3.** Absolute maximal values of the attitude and position errors for the third navigation case.

influence of gyros scale factor errors in yaw channel.

Gyros 5.64⋅10−<sup>6</sup> <sup>o</sup> / s 4.2⋅10−<sup>6</sup> <sup>o</sup> / s −7.2⋅10−<sup>6</sup> <sup>o</sup> / s −4.056⋅10−<sup>4</sup>

**Table 4.** Bias and scale factor error for inertial sensors used in simulations.

(-0.0053 g) along the *y* axis of the vehicle. Considering the same initial conditions

Attitude angles errors [o] Position errors [m] δφ δθ δψ δ*xl* δ*yl* δ*zl*

/s, while running on the track with

*t*[s]

**Figure 11.** Simulation results for the third navigation case.

Figure 11. Simulation results for the third navigation case.

*t*[s]

#### **5. Conclusions**

The numerical simulation of the influences of the inertial sensors errors on the solution of navigation of strap-down inertial navigator was here presented. To perform the simulations some Matlab/Simulink models for the acceleration and rotation sensors based on the sensors data sheets and on the IEEE equivalent models for the inertial sensors were realized. Also, the solving of a navigation problem relative to terrestrial non-inertial reference frames and the development of an error model for the navigator were achieved. The validation of the navigation algorithm and of its error model was realized by building Matlab/Simulink models for them, followed by numerical simulation of these models for several navigation particular cases. Following, a study of the dependence of the inertial navigator outputs errors by the errors of the used inertial sensors was conducted. Simulations were made for three different navigation cases, the vehicle having the same initial position in all three cases: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration on the *x*-axis, 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along *x*-axis, which means the sensing of an acceleration of-0.0516 m/s2 (-0.0053 g) along *y*axis.

[4] Barbour, N. & Schmidt, G. (2001) Inertial Sensor Technology Trends, *IEEE Sensors*

Modelling and Simulation Based Matlab/Simulink of a Strap-Down Inertial Navigation System...

http://dx.doi.org/10.5772/57583

337

[5] Barbour, N. (2010). Inertial Navigation Sensors, NATO RTO Lecture Series, RTO-EN-SET-116, Low-Cost Navigation Sensors and Integration Technology, March 2010 [6] Dahia, K. (2005). *Nouveles methodes en filtrage particulaire. Application au recalage de nav‐ igation inertielle par mesures altimetriques*, Office National d'Etudes et de Recherches A ´erospatiales (ONERA), Universite Joseph Fourier, These du Doctorat, France, 4 Jan.

[7] Divakaruni, S. & Sanders S. (2006). Fiber optic gyros – a compelling choice for high accuracy applications, 18th International Conference on Optical Fiber Sensors, Can‐

[8] Dumke, R. & Mueller, T. (2010). Technology with Cold Atoms. *Innovation*, Vol. 9, No.

[9] Edu, I.R., Obreja, R. & Grigorie, T.L. (2011). Current technologies and trends in the development of gyros used in navigation applications – a review, Conference on Communications and Information Technology (CIT-2011), July, 14-16, Corfu, Greece,

[10] Farrell, J. (2008). *Aided Navigation: GPS with High Rate Sensors*, McGraw-Hill, ISBN:

[11] Grewal, M. S., Andrews, A. P. & Bartone, C. (2013). *Global navigation satellite systems, inertial navigation, and integration,* John Wiley & Sons, ISBN-13: 978-1118447000, USA,

[12] Grigorie, T. L., Hiliuta, A., Botez, R. M., Aron, I. (2006). Étude numérique et expéri‐ mentale d'un algorithme d'attitude pour un système inertiel à composantes liés. *Transactions of the Canadian Society of the Mechanical Engineering (CSME)*, Vol. 30(3),

[13] Grigorie, T. L. (2007). *Strap-Down Inertial Navigation Systems. Optimization studies*. Si‐

[14] Grigorie, T. L., Lungu, M., Edu, I. R. & Obreja, R. (2010 a). Concepts for error model‐ ing of miniature accelerometers used in inertial navigation systems, *Annals of the Uni‐ versity of Craiova, Electrical Engineering series*, Vol. 34, pp. 212-219, ISSN: 1842-4805,

[15] Grigorie, T. L., Lungu, M., Edu, I. R. & Obreja, R. (2010 b). Concepts for error model‐ ing of miniature gyros used in inertial navigation systems, Proceedings of the IEEE International Conference on Mechanical Engineering, Robotics and Aerospace (IC‐

[16] Grigorie, T. L., Lungu, M., Edu, I. R. & Obreja, R. (2010 c). Analysis of the Miniatur‐ ized Inertial Sensors Stochastic Errors with the Allan Variance Method: A Review,

*Journal*, Vol. 1, No. 4, pp. 332-339, 2001

cun, Mexico, October 2006

978-0-07-149329-1, USA, 2008

pp. 429-442, ISSN: 0315-8977, 2006

tech, ISBN: 978-973-746-723-2, Craiova, Romania, 2007

MERA 2010), Bucharest, Romania, December 2-4, 2010

2005

2, 2010

2013

2010

pp. 63-68, 2011

The methodology presented here can be used successfully to estimate the effects of the sensors errors on the solution of navigation precision since in the design phase of the inertial navigator, without a prior acquisition of inertial sensors, and based only on their data sheet.

#### **Author details**

Teodor Lucian Grigorie1 and Ruxandra Mihaela Botez2


#### **References**


[4] Barbour, N. & Schmidt, G. (2001) Inertial Sensor Technology Trends, *IEEE Sensors Journal*, Vol. 1, No. 4, pp. 332-339, 2001

**5. Conclusions**

336 MATLAB Applications for the Practical Engineer

axis.

**Author details**

**References**

Teodor Lucian Grigorie1

1 University of Craiova, Romania

2 École de Technologie Supérieure, Canada

978-8120333536, India, 2008

The numerical simulation of the influences of the inertial sensors errors on the solution of navigation of strap-down inertial navigator was here presented. To perform the simulations some Matlab/Simulink models for the acceleration and rotation sensors based on the sensors data sheets and on the IEEE equivalent models for the inertial sensors were realized. Also, the solving of a navigation problem relative to terrestrial non-inertial reference frames and the development of an error model for the navigator were achieved. The validation of the navigation algorithm and of its error model was realized by building Matlab/Simulink models for them, followed by numerical simulation of these models for several navigation particular cases. Following, a study of the dependence of the inertial navigator outputs errors by the errors of the used inertial sensors was conducted. Simulations were made for three different navigation cases, the vehicle having the same initial position in all three cases: 1) the vehicle is immobile, 2) the vehicle runs at 0.1 g acceleration on the *x*-axis, 3) The vehicle is subjected to turning with angular velocity 0.1 degree/s, while running on the track with acceleration 0.1 g along *x*-axis, which means the sensing of an acceleration of-0.0516 m/s2 (-0.0053 g) along *y*-

The methodology presented here can be used successfully to estimate the effects of the sensors errors on the solution of navigation precision since in the design phase of the inertial navigator,

[1] Bekir, E. (2007). *Introduction to Modern Navigation Systems*, World Scientific Publish‐

[2] Bose, P. (2008). *Modern Inertial Sensors and Systems,* Prentice-Hall, ISBN-13:

[3] Barbour, N., Hopkins, R., Kourepenis, A. & Ward, P. (2010). Inertial MEMS System Applications, NATO RTO Lecture Series, RTO-EN-SET-116, Low-Cost Navigation

without a prior acquisition of inertial sensors, and based only on their data sheet.

and Ruxandra Mihaela Botez2

ing Co. Pte. Ltd., ISBN-10: 9812707662, USA, 2007

Sensors and Integration Technology, March 2010


Proceedings of the International Conference of Aerospace Sciences "AEROSPATIAL 2010", Section 5, pp. 1-10, ISSN 2067-8622, Bucharest, 20-21 October, 2010

**Chapter 12**

**Tool of the Complete Optimal Control**

Additional information is available at the end of the chapter

The main objective of the chapter is to use Matlab software in order to develop the optimal control knowledge in relation to the biggest worldwide power consumers: electrical machines. These two major components, optimal control and electrical machines form an optimal drive system. The proposed drive system conducts to the energy efficiency increasing during transient regimes (starting, stopping and reversing), therefore reducing the impact upon the environment. Taking into account the world energy policies, the chapter is strategically oriented towards the compatibility with the priority requirements from world research programmes. This chapter will offer an original Matlab tool in order to implement a highly performant control for electrical drives. The optimal solution provided by the optimal problem is obtained by numerical integration of the matrix Riccati differential equation (MRDE). The proposed optimal control, based on energetic criterion, can be implemented on-line by using a real time experimental platform. The solution has three terms: the first term includes the reference state of the electrical drive system, the second term assures a fast compensation of the disturbance (i.e. the load torque in case of the electrical drives) by using feedforward control, and the third is the state feedback component. Therefore, there is a completed optimal control for linear dynamic systems. The simulation and experimental results will be provided. By using knowledge regarding the electrical machines, electrical and mechanical measure‐ ments, control theory, digital control, and real time implementation, a high degree of inter‐ disciplinarity is obtained in the developed Matlab tool The optimal problem statement is without constraints; by choosing adequate weighting matrices, the magnitude constraints of

> © 2014 The Author(s). Licensee InTech. This chapter is distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

**for Variable Speed**

http://dx.doi.org/10.5772/57521

control and of state variable are solved.

**Electrical Drives**

Marian Gaiceanu

**1. Introduction**

