ITCS20110024A1 - DEVICE AND METHOD FOR THE ESTIMATE OF THE STATE OF A VEHICLE IN NAVIGATION - Google Patents

Description

Title of the invention

DEVICE AND METHOD FOR ESTIMATING THE STATE OF A VEHICLE IN NAVIGATION.

Technical field associated with the invention

The invention relates to a device and a method for calculating the estimates of the state of a vehicle underway that flies over a certain terrain.

The main, but not exclusive, application considered here for the said device and method is on board "lander" vehicles, ie vehicles used to land on surfaces of celestial bodies, such as Mars and the Moon, for space exploration purposes. The vehicles considered must perform a braked descent maneuver that leads to landing. This maneuver also requires, in particular, to identify at the moment the landing point, a priori not very accurately defined, which does not present obstacles or dangers for the vehicle itself such as for example. rocks or large boulders or excessive slopes.

To carry out a maneuver of this type, these vehicles must be able to estimate their state autonomously and in real time.

Vehicle status includes the following components: position, speed, attitude and angular velocity.

State-of-the-art state estimation devices, in this context, are typically composed of the following components:

• A camera that outputs a sequence of images taken in successive moments, showing the land flown over

• An Inertial Measurement Unit (IMU), which produces measurements of angular velocity and linear acceleration, in a reference system integral with the aircraft • An Image Processing device that repeatedly performs the following operations:

- receives the acquired images in input

- identifies some points on the images which have the characteristic of being easily “traceable” in the sequence of images

- performs the "tracking" operation of said points - in other words, it determines their apparent movement

- outputs a series of "traces", which describe this apparent movement, in terms of row and column coordinate sequences of the pixel matrix of the camera (the rigorous definition of the output data of this device is provided in section 1.1.) . • A device called "Navigation Filter", which produces estimates of the state as a function of the measurements of the IMU and the output data of the image processing device

The scheme represented in Figure 1 summarizes what has been said above.

State-of-the-art Navigation Devices typically implement recursive filtering algorithms within the Navigation Filter, based on the repeated execution of two basic steps; these steps are essentially the propagation of the estimated state, and the updating of the estimated state based on the latest incoming data received. Such algorithms typically have various disadvantages (eg. The possibility of divergence of the estimation error, and a high computational cost).

The invention relates to an innovative Navigation Device, which has the same block diagram (at high level) as the devices of this type at the state of the art, but implements - within the "Navigation Filter" device - a method of different and more performing calculation.

In particular, instead of iteratively updating the state estimates, a closed, non-recursive expression is used to explicitly calculate the state estimates in an efficient, robust and accurate way, using the input data provided by the IMU and the image processing device. This device does not have the disadvantages present in state-of-the-art devices, and also allows a significant increase in performance - in particular, there is a significant increase in estimation accuracy, robustness and an important reduction in computational cost.

Relevant "Prior art"

The Navigation Filters used within the state-of-the-art Navigation Devices (in the context considered) are generally based on an approach that follows the concept of the Kalman Filter or one of its variants (such as the Extended Kalman). The starting point for defining the algorithms implemented on these devices are typically the following two equations:

• The observation (or measurement) equation, used to calculate the measured values starting from the current state of the system (vehicle),

• The transition (or propagation) equation of the state, used to calculate the subsequent state of the vehicle starting from the current state and from the inputs to the system.

It is important to take into account the fact that, together with the estimated state, the algorithms often also calculate the uncertainty associated with the estimated state, typically expressed through the estimation error covariance matrix.

The core of the estimation process typically consists of the repeated execution of a cycle with the following two basic steps:

• Prediction: here the estimated state (and the associated error) are propagated using the transition equation

• Update: here the state estimate obtained in the previous step is updated taking into account the latest measurements; this is typically done by adding, to the state estimate, a "correction" that is obtained by multiplying the "innovation" (ie the difference between the actual measures, and those that would have been obtained if the estimated state had been the "true" one) with an appropriate "gain" matrix. One way to obtain this matrix can be the minimization of the expected value of the squared error of the estimate.

On an intuitive level, it is possible to say that the filters used in state-of-the-art devices use an "indirect" way to obtain the state of the vehicle starting from the measurements: in essence, instead of using a closed expression that directly derives the state of the vehicle as a function of the set of measures, the algorithms used iteratively improve the estimate of the state, according to the latest measures received.

On the other hand, the (Navigation) Device that is the subject of this invention uses, within the Navigation Filter, an expression in closed form to directly derive the status of the vehicle.

SPECIFIC DOCUMENTS CONCERNING THE "PRIOR ART"

• [D1] X. Sembély et al, "Mars Sample Return: Optimizing the Descent and Soft Landing for an Autonomous Martian Lander"

• [D2] “Navigation for Planetary Approach and Landing Final Report”, European Space Agency, Contract Reference: 15618/01 / NL / FM, May 2006.

• [D3] WO 2008/024772 A1 (Univ Florida [US]; Kaiser Michael Kent [US], Gans Nicholas Raphael [US]) 28 February 2008 (2008-02-28)

• [D4] Li et al: "Vision-aided inertial navigation for pinpoint planetary landing", Aerospace Science and Technology, Elsevier Masson, FR, vol 11. no.6, 19 July 2007 (2007-07-19), pages 499 -506, XP022158291, ISSN: 1270-9638, DOI: DOI: 10.1016 / J.AST. 2007.04.006 ASSESSMENT OF THE "PRIOR ART"

Typically, some of the disadvantages of using state-of-the-art devices (such as the devices described in [D1], [D2], [D3] and [D4]) are as follows:

• Considerable importance of the initial / preliminary estimates (in some cases, it is essential to send these data to the filter at the beginning for proper operation);

• Risk of divergence / instability of the filter, or of a high estimation error;

• Potential need for a long verification campaign to evaluate the effect of input data errors on output data errors;

• Limited ability to estimate the local vertical direction, or the gravity vector (in the reference system used to express the estimates of the state);

• Potential need for a long calibration process of the filter parameters;

• Potential need for an IMU with high accuracy;

• High computational cost for real-time execution (in other words, the computing hardware of the Navigation Filter must be able to perform a large number of operations per second).

TECHNICAL PROBLEM TO BE SOLVED, ADVANTAGEOUS EFFECTS OF THE INVENTION

The invention in question has as its object an innovative device for obtaining estimates of the state of the vehicle in real time.

This device does not have the disadvantages present in state-of-the-art devices (listed above). In particular:

• It is not necessary to have initialization values;

• The Navigation Filter cannot diverge;

• It is very simple to estimate the errors on the output data as a function of the errors on the input data; • Local vertical direction can be easily estimated (together with state estimates); • There are no significant filter parameters or other values to be calibrated through a long process;

• The Navigation Filter algorithm is robust with respect to errors in IMU measurements;

• The computational cost of the algorithm implemented in the filter is reduced - the expected reduction compared to state-of-the-art solutions is typically at least 10 times.

In addition, there is also a significant increase in the accuracy of the state estimate, typically at least 10 times.

Using this innovative Navigation Filter within the device, it will be possible to obtain a significant increase in the general performance of the status estimation function, compared to the current state of the art. This performance increase will also have positive repercussions on other functions and systems of the lander. This could also lead to cost and engineering budget reductions at the system level - e.g., reduced computational loads, increased mission reliability due to increased robustness, and reduced propellant mass (e.g. due to the increase in the accuracy of the state estimate, with a consequent reduction in the need for corrective maneuvers).

DESCRIPTION OF A WAY TO REALIZE THE INVENTION

MAIN INNOVATIVE ASPECTS OF THE INVENTION

The key innovative aspect of the invention is the calculation method that is used within the Navigation Filter to calculate the state estimates.

It is possible to say that, unlike state-of-the-art systems, where a general estimation method (eg the Kalman Filter) is adapted for the particular case considered, here the estimation method has been constructed "starting from zero ”, highly optimized for the considered problem; the estimation method uses only “closed form”, non-iterative, efficient and robust expressions.

Intuitively, compared the state estimation problem considered here with the problem of finding the zeros of a cubic polynomial, the state of the art approach corresponds to the use of a numerical iterative method, while the approach considered with the proposed method it corresponds to the use of an explicit "analytical" solution (eg Cardano's method).

DETAILED DESCRIPTION OF THE INVENTION

This section contains all the information necessary to implement the invention.

The section is structured as follows:

• Basic concepts and assumptions: here are the definitions of the main concepts and the assumptions used in the description;

• Description of the function of the Filter: here the function of the Navigation Filter is strictly defined, in terms of inputs (inputs) received and outputs (outputs) produced;

• Definition of the reference system for the state estimates: here we provide the definition of the reference system in which the state estimates (produced by the Filter) are expressed; • Description of the algorithm: here the algorithm implemented in the Filter (QuickNav) is described, in terms of the operations it performs; this part is divided into:

- Pre-processing step: some initial processing are described here;

- Central step: here the way in which the key operations are carried out is described;

- Post-processing step: here the way in which the final output data is calculated is described;

• Additional information: some additional information regarding the algorithm is provided here.

1. BASIC CONCEPTS AND ASSUMPTIONS

To simplify the notation and descriptions, a “continuous” (rather than “sampled”) time axis will be considered. The time interval for which the motion of the vehicle (and the input and output data) is considered is indicated with I:

I = [t0, t1] (1)

During this time interval, the vehicle will make a certain trajectory in a region of space, over a part of the terrain.

It will be assumed that in this region of space, the gravity acceleration vector is constant.

Its direction defines the concept of "vertical".

Here a generic reference frame (RF) will be called "Fixed" if it is fixed with respect to the ground, during the time interval I.

Furthermore, an RF will be said to have a "Vertical Z Axis" if its z axis is parallel to the gravity vector, but has the opposite direction (and therefore gravity has zero x and y components, and a negative z component).

Here we also assume the existence of a reference system called "Body Reference Frame" (BRF), fixed with respect to the vehicle; the IMU and the camera are also fixed relative to the vehicle.

A carrier expressed in the BRF will be indicated with a superscript “B”.

When there is no risk of ambiguity, vectors expressed in a "Fixed" RF will not have apex. Defined a certain “Fixed” RF, it can be used to express the dynamic state of the vehicle.

For each instant t in I, the dynamic state of the vehicle is defined by the following components:

• r (t): vehicle position: coordinates of the origin of the BRF, expressed in the chosen "Fixed" reference system;

• v (t): vehicle speed;

• A (t): vehicle attitude: for simplicity of notation (but without losing generality), it will be assumed that the attitude is represented by a matrix of director cosines. Consequently, the attitude matrix A contains the components of the three BRF vector units, in the chosen “Fixed” reference system;

• ω (t): angular velocity of the vehicle;

The vehicle acceleration, a (t), can be divided into two components:

• Acceleration of gravity, g

• Acceleration "measurable" on board by the IMU, am (t) = a (t) - g

The measurements made by the IMU are as follows:

• The measurable acceleration, expressed in the BRF: a <B>

m (t)

• The angular velocity, expressed in the BRF: ω <B> (t).

To indicate a generic method to propagate the attitude matrix from the time instant t = t 'to t = t' ', using ω (t) in t ∈ [t', t ''] ⊂ I, the following expression will be used:

t ''

A (t '') <=> ∫ [ω (t) <×>] dt A (t ') (2) t'

Using these definitions, we also obtain the following relations, for [t ’, t’ ’] ⊂ I:

t ''

v (t '') <=> v (t ') <+> ∫am (t) dt <+> g (t' '<−> t')

t '

t '' t

1 2

<r> (t '') <= r> (t ') <+ v> (t') (t '' <−> t ') <+> ∫ ∫ <a m> (<τ>) d <τ > dt <+> 2 <g> (t '' <−> t ')

t 't'

(3)

A general point on the surface of the celestial body under consideration will have the subscript "s".

1.1 THE CONCEPT OF "TRACK" AND ASSOCIATED CONCEPTS

Figure 2 shows the concept of traces of characteristic points of the terrain; “particular” points are recognized in different images (taken at different instants) by the Image Processing device; these points are tracked, traced in their apparent motion on the plane of the image. These plotted points, together with the IMU data, are used by the Navigation Filter (see figure 2).

The concept of "track" (of characteristic points of the terrain) can be accurately defined as follows:

• During the time period I, the Image Processing device repeatedly carried out the following two types of operations:

a) Extraction (or identification) of the characteristic points to be traced;

b) Tracing of the characteristic points;

• The first operation consists in the identification, on the acquired images, of certain particular points of the terrain (not previously known) which apparently have the characteristic of being easily traceable from one image to the next. The set of all points (rgi) extracted during the time interval I will be indicated with Rg:

Rg = (rg1, ... rgN) (4) • In general, the identification of a point rgi occurred at a certain instant in I; this instant, indicated with tAi (for the i-th point), indicates the "beginning" of the i-th track;

• At a certain instant tBi> tAi, the tracing of the i-estimate point ends (eg because it has gone out of the visible area); this instant indicates the "end" of the i-th track;

• Figure 3 shows an example of the temporal arrangement of the tracks:

• The time interval (for a generic i-th track) [tAi, tBi] will be called "Track Interval" (these intervals are, in general, different for different tracks);

• During this time interval, the Image Processing block "tracks" the point supplying, for each t in [tAi, tBi], its apparent position in the image, in terms of coordinates (row and column) of the pixel (for simplicity of notation, here a "continuous" time axis has been used, while in practice it will obviously be "sampled" at a certain frequency):

pi (t) = (rowi (t), columni (t)), ∀t∈ [tAi, tBi] (5)

• The pi (t) function is called the “i-th trace”; the set of all N tracks is indicated with P:

P = (p1 (⋅), p2 (⋅),… pN (⋅)) (6)

where the notation pi (⋅) indicates a function of time (which has as its domain [tAi, tBi]).

Consequently, it can be said that there are three "hierarchy levels" involved in the concept of "trace":

• The object at the highest level is the set of tracks P, which contains N elements (each element is a separate track);

• In turn, each track is a function of time; consequently it can provide a value of pi (t) for each t in its domain, [tAi, tBi] (obviously, in practice it is impossible to produce an infinite number of values; consequently each trace will in practice contain a sequence of vector values pi, associated with time instants uniformly spaced in the interval [tAi, tBi] - eg with a sampling period of 0.1 sec);

• The lowest level are the vectors p, composed of the row and column values.

Closely associated with pi (t) is the concept of the unit vector KI (t) (nb: the letter "I" of "KI (t)" is u reference to the index "i") Similarly to pi (t), KI ( t) indicates the apparent direction of the i-th traced point, but in the three-dimensional space of a "Fixed" RF, and not in the two-dimensional plane of the image:

r gi - r (t)

KI (t) =, ∀t∈ [tAi, tBi] (7) | r gi - r (t) |

(in the expression above, it was assumed that the position of the camera can be approximated with the position of the origin of the BRF). Figure 4 shows the relationship between pi (t) and KI (t).

It is possible to easily transform pi (t) into KI (t), if the attitude of the aircraft is known, as described in the "Pre-processing step" section. Figure 5 shows an example of how the versor KI (t) “tracks” the point on the surface rg as the aircraft moves from r (tAi) to r (tBi).

2. DESCRIPTION OF THE FUNCTION OF THE DEVICE

Taking into account the concepts introduced so far, it is possible to accurately describe the function of the navigation filter.

(NB: here the concept of "profile" of a variable indicates the set of all the values assumed by the variable during a certain time interval; this is indicated by inserting the variable between the round brackets - for example the trajectory of the vehicle during the 'time interval I is indicated with (r (t)) I).

The filter receives the following input data:

• The profile of the IMU measurements in the time interval I, or (a <B>

m (t)) <B>

Ie (ωm (t)) I

• The set of N traces obtained during the time interval I, or P = (p1 (⋅), p2 (⋅), ... pN (⋅))

Using this data, the filter must produce estimates of the vehicle status profile during the time interval I, namely:

• (r (t)) I

• (v (t)) I

• (A (t)) I

• (ω (t)) I.

Obviously, it is essential to specify the reference system in which these estimates will be expressed; this will be done in the next section.

3. DEFINITION OF THE REFERENCE SYSTEM FOR THE ESTIMATES OF THE STATE

Clearly, the reference system in which the state estimates will be expressed will have to be chosen appropriately. An important criterion is that the RF chosen must be such as to allow efficient use of the estimates produced (by other devices / algorithms used on board the aircraft). To meet this condition, the RF chosen must be "Fixed".

Furthermore, it is essential to know the direction of the gravity vector g in the chosen RF. Alternatively, the chosen RF can simply have a Vertical Z Axis by definition, so as to have g = (0,0, - | g |) <T> (here the apex T indicates the transpose). This is the assumption considered below.

Consequently, the RF chosen for the state estimates must be defined with respect to some "local" characteristics of the vehicle's trajectory or observed points.

To define this RF, it is useful to introduce an "Auxiliary" RF, ARF, defined as follows: • It is a "Fixed" RF,

• The origin of the RF coincides with the position of the vehicle for t = t0

• The axes of the RF coincide with the axes of the BRF for t = t0

In other words, for t = t0ARF it coincides with BRF.

ARF has the useful properties that, in it, r (t0) = 0 and A (t0) = I (here I denotes the identity matrix 3x3), by definition. ARF also has the disadvantage of not having, in general, a Vertical Z Axis (unless the vehicle has the "body" z axis oriented vertically for t = t0).

On the other hand, if you can calculate the components of the gravity vector g in the ARF, it will be easy to "rotate" the ARF to align the z axis with gravity. The resulting RF will be called "Final" RF (FRF) and will be used to express the estimates of the vehicle status calculated by the Navigation Filter.

The RF reference systems involved are schematically depicted in figure 6 (for simplicity, the 2D case has been considered in the figure)

The FRF, centered (as well as ARF) in the position of the vehicle for t = t0, has the Vertical Z Axis, and is also "Fixed". Consequently, it is an adequate RF for expressing state estimates.

4. DESCRIPTION OF THE ALGORITHM

The algorithm for the navigation filter can be seen, at a high level, as a sequence of three steps:

1) Pre-processing step, necessary for some initial calculations to be made on the input data; 2) Central step: here the main operations of the filter are carried out; this essentially consists in the estimate (in the ARF) of the vehicle speed (for t = t1) and the acceleration of gravity, g. This is done by processing the IMU data in the ARF and the set of traces processed; this data is provided by the pre-processing step;

3) Post-processing step, here some of the outputs of the preprocessing step are used, and then combined with the outputs of the "central step", to obtain the profile of the state estimated in the FRF.

4.1 PRE-PROCESSING STEP

This step prepares the input data to express them in the correct form - and in particular to express them in the “Auxiliary” reference system (ARF).

4.1.1 CALCULATION OF THE TRIM, ANGULAR SPEED AND ACCELERATION MEASURED IN THE ARF

The first operation carried out aims to calculate the attitude profile and angular velocity in the ARF, namely:

(A <A> (t)) I, (ω <A> (t)) I (8) starting from the angular velocity measurement profile, that is (ω <B> (t)) I. This is a rather simple operation, due to the fact that the initial set-up in ARF is known by definition:

A <A> (t0) = I (9) At this point, to calculate the attitude profile and angular velocity for t> t0, you can use the following expressions:

ω <A> (tn) = A <A> (tn) ω <B> (tn)

A <A> (tn + 1) = [ω <A> (tn) ×] A <A> (tn) ∆t

(10)

where the time axis has been discretized with a step ∆t; for n ≥ 0:

tn = t0 + n∆t (11) Once the profiles (A <A> (t)) Ie (ω <A> (t)) I have been obtained, the measured acceleration is also converted from the BRF to the ARF:

to <A>

m (t) = A <A> (t) a <B>

m (t) (12) 4.1.2 PROCESSING THE TRACKS TO CALCULATE THE ASSOCIATED VERSORS

The goal of this calculation is the processing of the set P of the N traces, P = (p1 (⋅), p2 (⋅), ... pN (⋅)) (obtained from the image processing device), where each trace is a function of time pi (t) = (rowi (t), columni (t)), to obtain a set TP of N "transformed" tracks:

TP = (K1 (⋅), K2 (⋅), ... KN (⋅)) (13) where each "transformed" trace KI (⋅) is simply the vector indicating the direction towards the i-th plotted point, during the "Trace Interval" [tAi, tBi], in the ARF.

Consequently, P can be transformed into TP by calculating - for each trace 1 <i <N, and for each instant t∈ [tAi, tBi] - the value of KI (t) using the value of pi (t).

The calculation of KI (t) using pi (t) can be done as follows:

1) The value of KI (t) in the RF of the camera (CRF) is calculated; this value will be indicated with KRFCI (t). It is assumed to have a CRF with the z axis passing through the optical axis of the camera, and the origin on the focal plane of the camera. Consequently, KRFCI (t) can be calculated using a camera model, which maps pixel coordinates to directions in space. For example, if we consider a simple "pinhole" model, then KRFCI (t) can be calculated simply with:

K () = <1> (<~> (column ()), <~> (row ()), - T

RFCI t x i t y i t 1) (14) <~> x2 (columni (t)) + <~> y 2 (row i (t)) 1

where is it :

~ <column> FOV FOV y x (column) = (- 1 + 2) tan (x), ~ y (row) = (1- 2 <row>) tan () (15)

N column 2 N row 2 and Nrow, Ncolumn, FOVx, FOV are, respectively, the number of rows and columns of the image, and the field of view along the x and y axes of the camera.

2) Given that the position and orientation of the chamber with respect to the vehicle are known, it is possible to transform the KRFCI (t) versor into KRFBI (t).

3) Finally, using the vehicle attitude information in ARF, KRFBI (t) is transformed into KI (t).

To summarize, the outputs of this preprocessing step are as follows:

<A A> • The profiles (A (t)) I, (ω (t)) Ie (a <A>

m (t)) I;

• Tracks TP = (K1 (⋅), K2 (⋅), ... KN (⋅)); each trace contains the profile (ie the values over time) of the unit vector KI (t), which “points” in the direction of the i-th “pursued” point on the surface r <A> These vectors are expressed in the ARF.

4.2 CENTRAL STEP

The preprocessing step data used here are as follows:

• The measured acceleration profile, (a <A>

m (t)) I;

• The TP set, containing the traces of the points, expressed as vector units in the ARF.

In addition, the value of the norm of the acceleration of gravity (in the region of the space considered), gS, is also used; this data is considered as known.

Using this information, the following values will be calculated:

<A> • The acceleration vector of gravity, in the ARF: g

• The vehicle speed for t = t1, in ARF: v <A> (t1)

From now on, in the rest of the description of this part of the algorithm, the superscript "A" will be omitted, since all vectors will be expressed in the ARF.

The estimation of g and v (t1) begins by choosing one of the N traces in the set TP. The index of the generic track chosen will be indicated here with i * ∈ [1, ... N]. Using the i * -th plot as a starting point, a single estimate of g and v (t1) will be computed.

(NB: it is important to note that, during the calculation process of g and v (t1), other tracks in TP will also be used, together with track i *. However, it is possible to see track i * -th as the main one, that is the "Pivot").

The track i * -th, KI (t), begins at instant tAi * and ends at tBi *. The time instant centered between these two will be indicated with tMi *:

t Mi * = tAi * t Bi * (16)

2

The difference between these two time instants will be indicated with T:

Ti * = tBi * - tMi * = tMi * - tAi *. (17) In general, it is also possible to consider an algorithm where the time instant tMi * is not exactly centered between tAi * and tBi *; this can lead to a small increase in accuracy; to obtain a more compact description, this more general case will not be considered here.

It is important to take into account that, at this stage, the coordinates of the ground point rgi * are not known to the Navigation Filter.

At this point the following simplifying notation is introduced, for the positions and speeds at the time instants tAi *, tMi * and tBi *:

rA = r (tAi *), rM = r (tMi *), rB = r (tBi *), vA = v (tA), vM = v (tM) (18)

In addition, the differences in positions are defined:

DELTA_R = ∆rA = rM- rA, ∆rB = rB- rM (19)

These differences are clearly related to the values of vA, vB, g and the acceleration measured by the following expressions (in the integrals, the subscript "i *" has been omitted from the time limit, to simplify the notation):

t M t

<∆rA = vA> T <+> ∫ ∫ <a m> (<τ>) d <τ> dt <+ 1> 2

2 <g> T

t A t A

t B t

<∆rB = vB> T <+ m> (<τ>) d <τ> dt <1>

2 T 2

t ∫ M t ∫ <a g>

M.

(20)

Furthermore, taking into account that:

t M

vM <=> vA <+> ∫am (t) dt <+> g T (21) t A

we have:

∆r 2

A = vAT h <II>

AM + <1>

2 g T

∆r 3

B = vATi * + h I

AMTi * + h II

MB 2

2 g T

(22)

where ∆rA and ∆rB were expressed as a function of vA, g and the following values that indicate the integrals of the measured acceleration:

t M t M t t B t

h I

AM <=> a m (t) dt, <h> II II

<AM = τ> dt <, hMB =>

t ∫ A ∫ ∫ <a> m <(τ)> d ∫ ∫ <a> m <(τ) d τ dt (23)> t A t A t M t M

It is important to take into account the fact that h <I>

AM, h <I>

A <I>

Me h <II>

MB can be calculated by the filter, since the profile of am (t) is known.

At this point, it is useful to divide the differences in positions into a "position" part and a "length" part:

DELTA_R = ∆rA = lAUA, ∆rB = lBUB (24)

where UA and UB are two versors.

The values of UA and UB can be calculated by the filter using the available traces. The procedure to do this will now be described, in the case of a generic value U, which indicates the displacement between two temporal instants T1 and T2, i.e. for which we have that r (T2) - r (T1) = lGU, where lG is one climb. For UA and UB the procedure is similar.

Considering the plane defined by the points r1 = r (T1), r2 = r (T2) and rgi *, we have that the following vector units are all parallel to this plane:

• U (the unit vector from r1 to r2);

• KI 1 (the unit vector from r1 to rgi *);

• KI 2 (the versor from r2 to rgi *)

The triangle of said versors with the vertices r1, r2 and rgi * is shown in figure 7.

Consequently, the versor:

KI1 × KI 2KCI = (25)

| KI1 × KI2 |

it is orthogonal to U, that is KCI ⋅U = 0 (here “⋅” indicates the scalar product).

If we assume that, instead of having a single track from t = T1 to t = T2, which follows a point of the terrain rgi *, there are more than one, it is possible to define more triangles analogous to this one. Consequently, it is also possible to define several KCI vector units, all with the property of orthogonality with respect to U.

For example, considering three traces, which respectively follow the points of the ground rgi, rgi and rgi '', we obtain the situation shown in figure 8.

From here it is easy to see that b <ˆ> ⋅U = 0, b <ˆ> '⋅U = 0 and b <ˆ>' '⋅U = 0. Consequently, it is possible to calculate U by looking for the orthogonal vector a b <ˆ >, b <ˆ> 'and b <ˆ>' '(if there were no errors in the input data, it would be sufficient to make the vector product between two vectors b <ˆ> of this type). Consequently, it is possible to easily construct a method for estimating U (even in the presence of errors) starting from the “pivot” trace KI (t) (which starts at T1 and ends at T2). Using this

trace, we obtain a triangle and a vector b <ˆ>. Then, we add other traces from TP = (K1 (⋅), K2 (⋅), ... KN (⋅)), to obtain more triangles and vector units b <ˆ>.

Since the vector U indicates the "direction of motion" between t = T1 and t = T2, that is:

r (T2) - r (T1) UA = (26) | r (T2) - r (T1) |

the TP tracks that can be added are only those starting before T1 and ending after T2. This is due to the fact that, to define a triangle (and the corresponding versor b <ˆ>) starting from a trace KI (t), it is necessary to be able to evaluate this trace for the time instants t = T1 and t = T2. Figure 9 shows an example of a solution regarding the identification of such "admissible" traces.

The overall method is therefore as follows:

1) Starting from the set TP, a subset Q * = (K1 (⋅), K2 (⋅), ... KM (⋅)) is identified, composed of the traces starting before T1 and ending after T2 (it is You can easily modify the algorithm to take into account even the case when the number of traces identified in this way, M, is too low for an accurate estimate - for example, allowing a modification of T1 and T2;).

2) For each trace KI (⋅) of Q * (with I∈ [1, ... M]), its values are evaluated for t = T1 * and t = T2:

KI1 = KI (T1), KI2 = KI (T2) (27)

3) Then, the vector unit orthogonal to both is constructed:

KKI1 × KI 2CI = (28)

| KI1 × KI2 |

4) At this point, we have a set of M versors, E = (KC1, KC2, ... KCM). If there were no errors in the determination of the traces, all these vector units would be orthogonal to U. In practice, in the presence of errors, it is useful to find U by looking for the “most orthogonal” versor to all (KCI). Here, this is done by minimizing the function:

M.

J (u) = ∑ I = 1 (KCI ⋅ u) <2> (29)

with the constraint:

| u | = 1 (30) To minimize the function, it is necessary to find the points, on the unit sphere, for which the gradient of J (u), ∇J (u), is perpendicular to the plane tangent to the sphere. Consequently ∇J (u) must be parallel to u, that is:

∇J (u) = λ u (31) where λ is a scalar.

The expression for the gradient of J is:

∇J (u) = W u (32) where:

�w1⋅w1 w1⋅w2 w1 ⋅ w 3�

W = 2� �

�w2⋅w1 w2⋅w2 w2 ⋅ w 3� (33) ��w3⋅w1 w3⋅w2 w3 ⋅ w 3��

where w1, w2 and w3 are three vectors with M components, obtained, respectively, from the x, y and z components of the vectors KCI:

w1 = (KC1x, KC2x, ... KCMx) T

w2 = (KC1y, KC2y, ... KCMy) T

w3 = (KC1z, KC2z, ... KCM T

z)

(34)

The vector U must therefore satisfy the condition:

WU = λU (35) for a certain λ.

Consequently, U is found among the eigenvectors of W.

To calculate UA, we use KIA (the versor from rAa to rgi *) instead of K 1, and KIB (the versor from rBa to rgi *) instead of K 2.

To calculate UB, use KIM instead of K 1, and KIB (the versor from rBa to rgi *) instead of K 2. This least squares estimate of U can be expanded (e.g. using the Random Sample Tech Note Consensus, or RANSAC) to make the estimation method more robust to “outlier” traces (ie “spurious” traces).

It is important to take into account the fact that the choice of the set Q * depends on the values of T1 and T2 which, in turn, depend on the choice of the "pivot" track i *.

At this point, assuming we have calculated the estimates of UA and UB, we consider two triangles TRA and TRB (generally non-coplanar), with the following vertices respectively:

• rgi *, rMe rA

• rgi *, rBe rM

The two triangles TRA and TRB and some associated vectors are shown in figure 10:

The two triangles have a common side, along l * (the value of l * is not known at this stage).

The values of ALPHA_A, ALPHA_B, BETA_A and BETA_B are obtained by calculating the angles between some known vector units:

ALPHA_A = arccos (UA ⋅ KIA), ALPHA_B = arccos (-UB ⋅ KIB) BETA_A = arccos (KIA⋅ KIM), BETA_B = arccos (KIB ⋅ KIM),

(36)

Using the sine theorem, we have the following expressions:

sin (ALPHA_ A) = sin (BETA_ A), sin (ALPHA_ B) = sin (BETA_ B) (37) l * lAl * lB

These expressions are used to eliminate l * and get an expression for the relationship between lB and lA. Here this ratio is referred to as RHO:

RHO = <l> B = sin (ALPHA_A) sin (BETA_ B) (38) l A sin (ALPHA_B) sin (BETA_ A)

At this point, we rearrange equations (22) (subtracting the first from the second) to eliminate vA, and then we use the expressions (24) (for the position differences), and (38), (for the ratio between lBe there). The expression obtained is:

UBl - = h I II II 2ARHO UAlA AMTi * hMB − hAM g Ti * (39) This expression can be simplified by introducing the following quantities:

s0 = UBRHO - UA

s I II II

1 = hAMTi * + hMB - hAM

s2 = T 2

the*

(40)

where s0 and s1 are vectors, and s2 is a scalar. It is important to take into account the fact that s0, s1 and s2 can all be calculated by the filter using the available input data.

The resulting equation, which binds A with g, is the following:

s0lA = s1 + s2g (41) This is a vector equation (corresponding to 3 scalar equations), with 4 scalar unknowns (the value of lA, and the three components of the gravity vector g, in the ARF). Adding to this the condition | g | = gS (where gS has been assumed to be known), we obtain a system of 4 scalar equations and 4 scalar unknowns. It is possible to reduce it to a single quadratic equation with lA as unknown, by writing the condition concerning gravity as g⋅g = gs <2> and then replacing g, with (41). The resulting equation to calculate the A is:

| s0 | <2> lA <2> - 2 (s0⋅ s1) lA + | s1 | <2> - gSs2 <2> = 0 (42) This equation has two solutions for lA; the correct one can be identified with simple checks of the values of the estimates obtained (eg typically with the "wrong" solution you get a gravity vector pointing upwards).

Once lA is obtained, g is calculated using equation (41). Then lB is calculated, using the RHO value (calculated with (38)). From lA and lB, the vectors DELTA_R = ∆rA and ∆rB are obtained from (24).

At this point, using (22), vA is also calculated. Finally, the speed value for t = t1 is obtained simply by integration:

t 1

v (t1) <=> vA <+> ∫am (t) dt <+> (t1 <−> t A) g (43) t A

As indicated at the beginning of the description of the central step of the algorithm, these values of v (t1) and g were obtained using a generic trace i * as the "starting point" (or "pivot"). If there were no errors in the input data, it would be sufficient to arbitrarily choose a value for i * in the set [1, ... N], start from the corresponding trace, and then follow the method described here to obtain the correct values of v (t1) and g.

In practice, the input data will have errors; consequently, to reduce the estimation error of v (t1) and g, an average calculation procedure can be used, for example:

1) Each of the N tracks in TP = (K1 (⋅), K2 (⋅), ... KN (⋅)) is taken as a "pivot", i *, to generate a single pair of speed estimates (at t1 ) and severity, indicated with (vi * (t1), gi *);

2) Starting from the N estimates of v (t1) and g obtained in this way, the average is calculated to reduce the estimation errors.

Instead of a simple averaging operation, it is also possible to consider other methods (eg the median, or RANSAC) to obtain a more robust estimate than the "outliers".

Furthermore, another variant consists in the realization of a weighted average of the single estimates, calculating the averages as a function of the variances of the individual estimates (the values of the variances could in turn be estimated by propagating the values assumed for the input uncertainties through the calculations ), minimizing the overall estimate of the error.

4.3 POST-PROCESSING STEP

This step receives the following data:

• From the preprocessing step: attitude profiles, angular velocity and acceleration measured in the ARF, that is (A <A> (t)) <A> I <A>, (ω (t)) Iand (am (t ))THE

<A A> • From the central step: the estimates of v (t1) and g

The goal of this step is the calculation of the state profile (composed of the profiles of attitude, angular velocity, position and linear velocity) in the FRF.

The first operation performed here has as its objective the calculation of the speed and position profile in FRF.

<A> The velocity profile is calculated by simply integrating the estimate of v (t1) using (a <A> m (t)) I and g:

t

<∀t∈I,> v <A> (t) <=> v <A> (t1) <+> aA (τ) d τ (t t A

∫ m <+ -> 1) g (44) t 1

Since r <A> (t0) = 0 by definition, the position profile is calculated by simple integration of the speed:

t

<∀t∈I,> r <A> (t) <=> v <A> (τ) d τ (45) t ∫ 0

At this point, all the components of the state profile in ARF are available (i.e. (r <A> (t)) I, (v <A> (t)) I, (A <A> (t)) I, ( ω <A> (t)) I).

To convert them to FRF, a rotation matrix is applied which describes the transformation ARF → FRF; this transformation takes into account the fact that FRF has the Vertical Z Axis, while ARF does not.

Consequently, the rotation matrix is the one that describes the rotation around an axis by a certain angle, where:

• The axis is the vector orthogonal to both the gravity vector and the Z axis of the ARF.

• The angle ϕ is the angle between the gravity vector and the Z axis of the ARF - consequently, it is obtained from the z component of the gravity vector in the ARF, g <A>

z:

TO

ϕ = arccos (- gz) (46) g S

In this way, intuitively, the rotation will "straighten" all the components of the state profile in ARF, bringing them into FRF.

5. ADDITIONAL INFORMATION

5.1 TEMPORAL OPERATION OF THE FILTER

In the description up to this point, the “batch” mode of the Navigation Filter has been described. In this mode, the algorithm operates as follows:

• receives, as inputs, the data obtained during the entire time interval [t0, t1]

• then, it produces, in output, the estimates of the state, relating to the entire time interval [t0, t1]

In practice, the Filter will typically operate in an "incremental" mode, where:

• The algorithm will be called at a frequency that can be relatively high (e.g. 10 Hz), and

• For each call, the algorithm:

- It will receive the inputs obtained between the previous call and the current one, and:

- Will have to calculate the estimate of the "current" state

To obtain operation in "incremental" mode, the following changes are made: • Between successive calls of the algorithm, the values of the input data are stored (together with other relevant data, such as the estimates of v (t1) and g) in a memory

• At each call, the algorithm inserts the new incoming data into the memory, and then processes all the data contained in it that are relevant for the estimate (eg. From a certain moment in time onwards)

5.2 CASES OF SPEED NOT OBSERVABLE

In general, any Navigation Filter - which uses only IMU data and traces of unknown points on the surface - will not be able to estimate the speed (and therefore not even the position differences) if the acceleration is zero; it can be demonstrated by taking into account the fact that in such a situation there are multiple velocity and position profiles that produce the same input data profile; consequently, the Navigation Filter "cannot decide" the correct trajectory. In the case of the Navigation Filter considered here, if the acceleration is ≈0 during the trace interval [tAi *, tBi *] (corresponding to the track "pivot" i * -th), we will have that:

UB ≈ UA, RHO≈ 1 (47)

and consequently, from (40),

s0≈ 0 (48)

and therefore the estimates of lA obtained with (42) - and also the resulting estimates of vi * (t1) and gi * - will not be reliable. In such situations, you can do the following:

• In the process of accumulating single estimates of v (t1) and g, the values obtained with the "pivot" traces, which have time periods with low acceleration as trace interval, are excluded from the calculation

• Alternatively, IMU and track based estimates can be combined with additional vehicle model information. For example, in the case of a landing on Mars, if the Navigation Filter starts operating while the vehicle is still attached to the parachute, a situation with low acceleration can occur as the vehicle approaches terminal speed (before of the parachute release). However, in such a situation, the equations of dynamics can become simpler (e.g. the motion could be almost vertical and uniform), and therefore the knowledge of the model can be used to estimate limits on the velocity modulus (for example, in a very simple case, the velocity modulus would correspond to the terminal velocity)

It is important to take into account the fact that the direction of movement can also be observed with zero acceleration (using the method for estimating uˆA, of the "Central step").

5.3 ADDITIONAL FEATURES

The Navigation Filter considered so far can also be used to implement some additional features which are described below.

5.3.1 REBUILDING THE FORM OF THE SOIL

This functionality can be added easily, by carrying out a triangulation operation once the values of rA and rM have been estimated. It is possible to take the set of traces Q * = (K1 (⋅), K2 (⋅), ... KM (⋅)) used for the estimation of UA, and the set of resulting vectors (KJ1) and ( KJ2). At this point, for each J ∈ {1, ... M}, the position of the point on the surface rg * J can be obtained by looking for the intersection (or the point of minimum distance) between:

• The ray starting from rAe goes in the direction KJ 1

• The ray starting from rMe goes in the direction KJ 2

5.3.2 ADDITION OF ADDITIONAL SENSORS

The combination of sensors considered - IMU and camera - can be extended by adding other sensors to further increase the robustness and accuracy of the state estimation process. The Navigation Filter can in fact be adapted to receive data from additional sensors such as:

• Star sensors: these can be used to directly provide vehicle attitude measurements (eg for a lunar landing);

• Altimeter: the addition of an altimeter can allow an increase in the accuracy of the state estimates, providing an alternative method to estimate one of the main quantities involved, that is the length of the "position difference" vector, DELTA_R = ∆rA ( considered in the “Central Step” of the filter algorithm). Essentially, by processing the altimeter data, the length of the projection of the DELTA_R vector is measured along the nˆaltimeter direction (this vector indicates the pointing direction of the altimeter); indicating this length with laltimeter, we have that to calculate the value of lA it is possible to use the following expression (where nˆaltimeter K 1 are both calculated in the reference system ARF):

l <A> = l altimeter (49) K1⋅ nˆ altimeter

• Scanning LIDAR: this sensor allows the measurement of distances along the direction of a laser beam, and this direction can be controlled on the two degrees of freedom (a “Flash” LIDAR sensor can be used as an alternative). In this way, a scan of the underlying terrain is obtained, expressed with respect to the vehicle. By estimating the apparent motion of the scanned terrain, the movement of the vehicle can be reconstructed. Similarly to the case of the altimeter, here the Scanning LIDAR represents an alternative way to estimate ∆rA (on the other hand, all 3 components of the vector ∆rA can also be observed here).

5.3.3 ABSOLUTE NAVIGATION

This feature allows the estimation of the vehicle status profile in a "planetocentric" reference system (PRF), that is, in a reference system that is used to specify (unambiguously) the points of the terrain on the celestial body considered - such as for example. through latitude, longitude and height (obviously to be defined appropriately).

To achieve this functionality, the Navigation Filter must also have a set of "preloaded" data available (ie made available before the filter starts operating). In this case, these data correspond to a "map" of the terrain (eg composed of a combination of a Digital Elevation Model and an ortho-image), referred to the PRF reference system.

Without losing generality, it is assumed to have a Cartesian PRF, with the Vertical Z Axis.

Two variants of the filter operation are considered here:

A) Variant "without attitude measurements": here it is assumed that there is no additional input that provides (during the time interval of operation of the filter, I) the attitude profile of the aircraft in the PRF (this profile could be obtainable, for example, through star sensors);

B) Variant “with trim measures”: here it is assumed to have the aforementioned trim profile.

In the case of variant A, the steps of the algorithm to be implemented are the following (the difference between variants A and B is that in the case of variant B the second step is "skipped", since the structure profile in the PRF is already available ):

1) Execution of the "standard" algorithm (described above);

2) Determination of the trim profile in the PRF;

3) Calculation of the position in the PRF;

4) Calculation of the vehicle status profile in the PRF.

The first step aims at estimating the state profile in the FRF.

In the second step, it is necessary to determine the rotation between the FRF and the PRF. Since these two reference systems have a common z axis, it is sufficient to determine the angle of rotation around this axis, necessary to switch from one RF to the other.

This angle, indicated here with α, can be calculated starting from the following values:

1) The direction of the velocity, in the xy plane of the PRF, for a certain tα (even less than t0): it is a typically known quantity - for example, in the case of a landing on Mars or the Moon, this value is obtainable from vehicle orbit parameters (typically known with acceptable accuracy).

2) The direction of speed, in the xy plane of the FRF: it can be calculated starting from the output of the first step (ie from the "standard" algorithm) - if tα <t0, the IMU measurements obtained between tα and t0 are used

Indicating these two directions of velocities respectively with θPRF and θFRF, we have that α is given simply by:

α = θPRF- θFRF (50)

At this point, to determine the complete transformation between the FRF and the PRF (which will allow the transformation of the state profile in the FRF into that in the PRF), it is also necessary to determine the translation (expressed as a vector of the PRF) that leads from the origin of the PRF at the origin of the FRF. This vector obviously coincides with the difference between the vector r (tP), expressed in the PRF, and the same vector, expressed in the FRF (here tP∈ I). Since the position values in the FRF are known (being obtained in the first step), it remains to calculate the vector r (tP) in the PRF. The procedure for doing this is described below.

For simplicity of notation, this vector is hereinafter referred to as r.

By comparing the preloaded "map" of the terrain with the terrain observed by the camera, some points on the image are identified for which the coordinates in the PRF are known. These points (in the PRF) will be indicated with rg1, rg 2, ... rgS. Using the asset estimate in the PRF, it is possible to obtain the vector units (in the PRF) associated with each of these points; these vector units will be indicated here as n <ˆ> g1, n <ˆ> g 2, ... n <ˆ> gS (each vector unit indicates the apparent direction of the corresponding point on the ground).

Each pair (rgi, n <ˆ> gi), for i ∈ {1, ... S}, defines a ray, whose points can be parameterized with li> 0:

qi (li) = rgi + n <ˆ> gili (51)

In the absence of errors, all of these rays would cross at some point r. In other words, we would have for every i ∈ {1, ... S} a value li <*> such that qi (l <*>

i) = r.

Obviously, in practice it is impossible for these rays to cross. To obtain an estimate of r, a least squares approach is followed, minimizing the value of f (r) defined by: f (<r>) = ∑ <2>

i = 1d i (<r>) (52) where (r) indicates the minimum distance between the point r and the i-th ray:

di (r) = min | r - q () | (53) l> i l i

i0

The point qi (l <r>

i) of the ray closest to point r is the one for which the condition is verified:

n <ˆ> gi⋅ (r − qi (l <r>

i)) = 0 (54)

and then <r>

i = n <ˆT>

gi (r - rgi); consequently, we have that:

di (r) = | r − qi (l <r>

i) | = | r − rgi − n <ˆ> gin <ˆT>

gi (r − rgi) | = | Bi (r - rgi) | (55)

whereBi = I − n <ˆ> gin <ˆ T>

already It is therefore possible to rewrite the expression for d <2>

i (r) as:

d <2>

i (r) = (Bi (r − rgi)) <T> (Bi (r − rgi)) = (r − rgi) <T> Ci (r - rgi) (56)

whereCi = B <T>

iBi is a symmetric matrix. The gradient of f (r) is therefore given by:

S.

<∇> f (<r>) <=> ∑ <∇> 2S

i = 1 (d i (<r>)) <=> ∑i = 12 <C> i (<r−r> gi) <= Dr - h> (57) S S

<where D> = <2> ∑ i = 1 <C i e> h = 2 ∑i = 1Ci r gi. Consequently, the value of r for which f (r) is minimum is obtained through the expression:

r = D <- 1> h (58) At this point, the state profile in the PRF can be calculated starting from that in the FRF (obtained with the "standard" algorithm) and performing the following transformations:

• A rotation around the z axis, by an angle α;

• A translation of r.

To further reduce the state estimation error in the PRF, you can do the following:

1) the values of the vehicle position in the PRF are calculated for more time instants than I (instead of only for tP∈ I).

2) then, these estimates, indicated here as r (tP1), r (tP 2), ... r (tPH), are processed as follows:

• For each i ∈ {1, ... H}, an independent estimate of the translation from the PRF to the FRF is obtained (from the value of r (tP i)). By calculating the average of the estimates obtained in this way, the overall estimation error of this translation is reduced.

• Similarly, for a certain instant t * ∈ I, independent estimates of v (t *) (to be averaged later) can be obtained using pairs of points (r (tPi), r (tP j)) (where (i, j ) ∈ {1, ... H} ei ≠ j) through the expression:

t Pj t t *

<v> (t *) <1 => (<r> (tPj) - <r> (tPi) −∫ ∫ <a> m (<τ>) d <τ> dt− <1g> (t −t ) 2) + () (- *) <(59)> tPj - t 2 Pj Pi mtdt tPjt Pi t Pi t Pi t ∫ <a g>

Pi

Claims (10)

CLAIMS 1. Device for estimating the state of a vehicle in navigation, consisting of an inertial measurement unit (IMU), which detects the angular velocities and accelerations of said vehicle, by an image acquisition medium (CAM) and by a device (PROC) for processing photographic images of the terrain taken in successive moments, identifying a series of characteristic points of the terrain, which are traced on the various images, and by a navigation filter (NF) which, through an appropriate algorithm, calculates the state of the vehicle in terms of speed, position, attitude and angular velocity, in a fixed reference system with respect to the ground, characterized by the fact: - that the navigation filter to obtain a unit vector (U), which indicates the direction of the displacement of the vehicle between two time instants (T1) and (T2), performs the following calculation steps: A) first calculates a set of vectors, (E), which in the absence of measurement errors would be they are all orthogonal to said unit vector (U), carrying out the following operations: A.1) integrates the angular velocity measurements of the inertial measurement unit in order to obtain the attitude profile, i.e. the temporal trend of the attitude during a certain time interval, with respect to a fixed reference system with respect to the ground A.2) uses the said attitude profile to process the output of the image processing device, i.e. the set of pixel traces where each trace of pixels describes the apparent motion of a characteristic point of the ground within the sequence of images, with the aim of obtaining a set (TP) of versor traces, where each versor trace is a function of time and describes the motion of said characteristic point in a fixed reference system with respect to time A.3) the set (E) is constructed by making the following calculations on each member (KI) of a subset of (TP): A.3.1) the values of ( KI) to (T1) and (T2), indic given respectively with (KI1) and (KI2), are extracted, A.3.2) the normalized vector product of (KI1) and (KI2), denoted by (KCI), is calculated, A.3.3) (KCI) is inserted into 'set (E) B) determines, by means of a least squares procedure, the vector closest to the orthogonality to said set of vectors (E) and which in this way best approximates the real unit vector (U); - that to obtain the ratio, (RHO), between the lengths of the movements of the vehicle, respectively, from (TA) to (TM), and from (TM) to (TB), where (TA), (TM) and ( TB) are three consecutive time instants, the navigation filter performs the following calculations: A) calculates, using the procedure described above, the vectors (UA) and (UB), which indicate the directions of vehicle movements respectively between the time instants (TA) and (TM), and (TM) and (TB) B) for each vector trace (KI) belonging to said set (TP), two triangles (TRA) and (TRB) are defined when possible, such therefore: the vertices of (TRA) are: the position of the vehicle at (TA), the position of the vehicle at (TB), and the point of the ground traced by (KI) and the vertices of (TRB) are: the position of the vehicle a (TB), the position of the vehicle a (TC), and said plot point; C) the relevant angles of the triangles (TRA) and (TRB) are calculated by carrying out the following operations: a) three versors, (KIA), (KIM) and (KIB), belonging to the said trace (KI), and associated respectively with the time instants (TA), (TM) and (TB), are extracted from the said trace b) the angle between the vectors (KIA) and (UA) is calculated, which is also the angle (ALPHA) of the triangle ( TRA) c) the angle between the vectors (KIA) and (KIM) is calculated, which is also the angle (BETA) of the triangle (TRA) d) the angle between the opposite of the vector (UB) is calculated ) and the vector (KIB), which is also the angle (ALPHA) of the triangle (TRB) and) the angle between the vectors (KIB) and (KIM) is calculated, which is also the angle (BETA ) of the triangle (TRB) D) (RHO) is calculated by multiplying the sine of the angle (ALPHA) of (TRA) with the sine of the angle (BETA) of (TRB), and then dividing the product by the product of the sine of the angle (ALPHA) of (TRB) with the sine of the angle (BETA) of (TRA); it is possible to combine multiple estimates of (RHO) - that in order to calculate the length of the movement of the vehicle between two moments in time, the navigation filter processes the said vector units (U), and the said ratio (RHO), and the measurements of the unit of inertial measurement, suitably processed, together with the value of the gravity acceleration vector or the value of the norm of this vector. 2. Method for estimating the state of a vehicle in navigation starting from the measurements of an inertial measurement unit that detects the angular velocities and accelerations of said vehicle, from the processing of photographic images of the terrain taken in successive moments, identifying on a series of characteristic points of the terrain, which are plotted on the various images, and which through an appropriate algorithm calculates the state of the vehicle in terms of speed, position, attitude and angular velocity, in a fixed reference system with respect to the ground, characterized by fact: - that in order to obtain the vector unit (U), which indicates the direction of the movement of the vehicle between two temporal instants, a set of vectors (E) is first calculated, which in the absence of measurement errors would all be orthogonal to the said versor (U), and subsequently the vector closest to the orthogonality to said set of vectors is determined by means of a least squares procedure ( E) and that in this way better approximates the real unit vector (U); - that to obtain the ratio, (RHO), between the lengths of the movements of the vehicle, respectively, from (TA) to (TM), and from (TM) to (TB), where (TA), (TM) and ( TB) are three consecutive time instants, first calculate the angles of two triangles, having respectively the following vertices: a) the position of the vehicle a (TA), the position of the vehicle a (TM), and a point on the ground traced images, and b) the position of the vehicle a (TM), the position of the vehicle a (TB), and the said plot point of the terrain, and subsequently the geometric sinus theorem is applied; - that in order to calculate the length of the movement of the vehicle between two instants of time, the said vector units (U), the said ratio (RHO), and the measures of the inertial measurement unit, suitably processed, are processed, together with the value of the acceleration vector of gravity or to the value of the norm of that vector. 3. Method for estimating the state of a vehicle in navigation according to claim 2, characterized in that an auxiliary reference system is also used to calculate the profile of said state, or the temporal trend of the state during a time interval , which coincides, at the initial instant of said time interval, with the fixed reference system with respect to the vehicle. 4. Method for estimating the state of a vehicle in navigation according to claim 2, characterized in that the result of the image processing, composed of a set of pixel tracks, where each pixel track describes the apparent movement of a characteristic point of the terrain in the sequence of images, is further processed to obtain a set (TP) of traces of versors, where each trace of versor, (KI), is a function of time and describes the movement of the aforementioned characteristic point in a system fixed reference with respect to the ground. 5. Method for estimating the state of a vehicle in navigation according to claim 2, characterized in that to calculate the unit vector, (U), which indicates the direction of movement of the vehicle between two instants of time, the following operations are carried out: - the set of traces (TP) is processed, obtained by processing the images and measurements of the inertial measurement unit, to first define a set of planes in space, all containing, in the absence of measurement errors, the said unit vector ( U), and then to define a set of vectors, (E), unitary and orthogonal to said set of planes; - the most orthogonal vector unit to the said set of vectors (E) is calculated, analytically minimizing the sum of the squares of the scalar products with the vectors of the said set. 6. Method for estimating the state of a vehicle in navigation according to claim 2, characterized by the fact that to calculate the ratio, (RHO), between the lengths of the movements of the vehicle respectively from (TA) to (TM), and from (TM) a (TB), where (TA), (TM) and (TB) are three time instants for which there is at least one pixel trace (KI) starting at (TA) or earlier, and ending at (TB ) or afterwards, the following operations are carried out: - the directions (UA) and (UB) of the vehicle movements are calculated respectively between (TA) and (TM), and between (TM) and (TB); - two triangles are defined, (TRA) and (TRB), having the following vertices respectively: pixel (KI), and b) the position of the vehicle a (TM), the position of the vehicle a (TB), and the point of the terrain traced by said pixel trace (KI); - the set of traces (TP), that is the result of the processing of the images and of the measurements of the inertial measurement unit, is used together with the vectors (UA) and (UB), to calculate the angles of the said two triangles ( TRA) and (TRB); - the said ratio, (RHO), is calculated between the lengths of the displacements, using the said angles of the triangles (TRA) and (TRB), and applying the geometric theorem of sines; it is possible to combine multiple estimates of (RHO). 7. Method for estimating the state of a vehicle in navigation according to claim 2, characterized in that to calculate the speed of the vehicle at a certain time instant (TA), and the gravity acceleration vector, the following operations are carried out: - two other time instants, (TM) and (TB), are used to calculate the said vector units (UA) and (UB), and the ratio (RHO); - the values calculated in the previous step are used, together with the measures of the inertial measurement unit, appropriately processed, to calculate the vector (DELTA_R) that indicates the movement of the vehicle from (TA) to (TM); - if the gravity vector is not known, but only its norm is known, then starting from said vector (DELTA_R), and from the value of the norm of the gravity vector, and from other values obtained previously, the gravity vector is determined; - starting from the said gravity vector, and from other values obtained previously, the speed in the instant in time (TA) is calculated. 8. Method for estimating the state of a vehicle in navigation according to claim 2, characterized by the fact that, also using the measurements of an altimeter, then the displacement between two temporal instants (DELTA_R) is calculated by carrying out the following operations: - the direction, (U), of the said displacement vector is calculated; - the altimeter measurements are used to calculate the projection of the said displacement vector along the measurement direction of the altimeter; - the values of said direction (U), of said altimeter measurement direction, and of said projection are used to calculate the length of said displacement (DELTA_R). 9. Method for estimating the state of a vehicle in navigation according to claim 2, characterized by the fact that, also receiving the measurements of a LIDAR sensor, scanning or flash, then the displacement, (DELTA_R), between two instants is calculated temporal, by carrying out the following operations: - the direction, (U), of said displacement vector is calculated, using the procedure described in claim 5; - using the said direction, the length of the said displacement is determined by maximizing the overlap between the points of the ground measured with the LIDAR scans obtained in the said two temporal instants. 10. Method for estimating the state of a vehicle in navigation according to claim 2, characterized in that using as input data also a map of the observed terrain, which also includes an ortho-image, or a matrix of pixels, referred to a planetocentric reference system, which indicates the aspect of the observed terrain, then the following operations are carried out to calculate the profile of the state of the vehicle in the said reference system: - a comparison is made between said ortho-image and the images observed by the vehicle, to obtain a set of points on the ground, where both the planetocentric coordinates and the coordinates indicating the apparent position of the point on one or more acquired images are available for each point of this set; - for each said image, the coordinates are processed on the image of said points, to obtain a set of vector units that indicate the directions in which said points are located, with respect to the vehicle, in said planetocentric reference system; - the position of the vehicle in the said planetocentric reference system is calculated, defining a ray for each pair of said point and versor, and then defining a function which, for each point of the three-dimensional space, evaluates the sum of the squared distances between said point of space and the half-lines previously defined, and finally minimizes said function by calculating, by means of an analytical procedure, the point for which the gradient of said function is zero. 1. Device to estimate the state of a moving vehicle, composed of an inertial measurement unit (IMU), measuring the angular velocities and the accelerations of said vehicle, by a camera (CAM) and by a device (PROC) for the processing of the terrain photographic images taken at successive instants, identifying a set of characteristic points of the terrain, which are tracked on the different images, and by a navigation filter (NF) which using a suitable algorithm calculates the state of the vehicle in terms of velocity, position , attitude and angular velocity, in a reference frame fixed with respect to the ground, characterized in that: - the navigation filter, in order to get a unit vector (U), indicating the direction of the displacement of the vehicle between two time instants (T1) and (T2), performs the following two computing steps: A) calculates at first a set of vectors, (E), which in case of no measurement errors would be all orthogonal to the said unit vector (U), by performing the following operations: A.1) integrates the angular velocity measurements of the inertial measurement unit in order to obtain the profile of the attitude, i.e. the temporal behavior of the attitude during a time interval, with respect to a reference frame fixed with respect to the ground A.2) utilizes the said attitude profile in order to process the output of the image processing device, i.e. the set of pixel tracks where each pixel track describes the apparent motion of a characteristic point of the terrain in the sequence of the images, in order to obtain a set (TP) of tracks of unit vectors, where each unit vector track is a function of time and describes the motion of said characteristic point in a reference frame fixed with respect to the ground A.3) the set (E) is constructed by performing the following computations on each member (KI) of a subset of (TP): A.3.1) the values of (KI) at (T1) and (T2), indicated respectively with (KI1) and (KI2), are extracted, A.3.2) the normalized vector cross product of (KI1) and (KI2) , indicated with (KCI), is computed, A.3.3) (KCI) is inserted in the set (E) B) determines, through the method of the least squares, the vector closer to the perpendicular to the said set of vectors ( E) and that in this way better approximates the actual unit vector (U); - to obtain the ratio, (RHO), between the lengths of the displacements of the vehicle, respectively in the timeframe from (TA) to (TM) and from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants, the navigation filter performs the following computations: A) computes, by using the procedure described above, the vectors (UA) and (UB), that indicate the directions of the displacements of the vehicle respectively between time instants (TA) and (TM), and (TM) and (TB) B) for each unit vector track (KI) that is a member of the said set (TP), two triangles, (TRA) and (TRB ), are defined when possible, such that the vertices of (TRA) are: the position of the vehicle at time (TA), the position of the vehicle at time (TM), and the point on the terrain tracked by (KI) and the vertices of (TRB) are: the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the said tracked point of the terrain, C) the relevant angles of the triangles (TRA ) and (T RB) are computed by performing the following operations: a) three unit vectors, (KIA), (KIM) and (KIB), belonging to the said pixel track (KI), and associated respectively to the time instants (TA), ( TM) and (TB), are extracted from the said pixel track b) the angle between vectors (KIA) and (UA), which is also the angle (ALPHA) of the triangle (TRA), is computed, c) the angle between vectors (KIA) and (KIM), which is also the angle (BETA) of the triangle (TRA), is computed, d) the angle between the opposite of the vector (UB), and the vector (KIB), is computed, which is also the angle (ALPHA) of the triangle (TRB), is computed, e) the angle between vectors (KIB) and (KIM), which is also the angle (BETA) of the triangle (TRB), is computed, D) (RHO) is then computed by multiplying the sine of the angle (ALPHA) of (TRA) with the sine of the angle (BETA) of (TRB), and then dividing that product with the product of the sine of the angle (ALPHA) of (TRB) with the sine of the angle (BETA) of (BETWEEN); multiple estimates of (RHO) can be combined - to calculate the length of the displacement of the vehicle between two time instants, the navigation filter processes the said unit vectors (U), the said ratio (RHO), and the measurements of the inertial measurement unit, conveniently treated, together with the value of the gravity acceleration vector or the value of the norm of such vector. 2. Method to estimate the state of a moving vehicle, on the base of the measurements of an inertial measurement unit, sensing the angular velocities and the accelerations of said vehicle, and of the processing of the photographic images of the terrain taken at successive instants , identifying a series of characteristic points of the terrain, which are tracked on the different images, and that by a suitable algorithm calculates the state of the vehicle in terms of velocity, position, attitude and angular velocity, in a reference frame fixed with respect to the ground and characterized in that: - in order to get the unit vector (U), indicating the direction of the displacement of the vehicle between two time instants, calculates at first a set of vectors, (E), which in case of no measurement errors would be orthogonal to the said unit vector (U), and then determines, through the method of the least squares, the vector closer to the perpendicular to the said set of vectors (E) and that in this way better approximates the actual unit vector (U); - to obtain the ratio, (RHO), between the lengths of the displacements of the vehicle, respectively in the timeframe from (TA) to (TM) and from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants, at first the angles of two triangles are calculated, having respectively the following vertices: a) the position of the vehicle at time (TA), the position of the vehicle at time (TM), and one of the points on the terrain tracked in the images, and b) the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the said tracked point of the terrain, and then the triangle law of sines is applied; - to calculate the length of the displacement of the vehicle between two time instants, the said unit vectors (U), the said ratio (RHO), and the measurements of the inertial measurement unit, conveniently treated, are processed together with the value of the gravity acceleration vector or the value of the norm of such vector. 3. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that to calculate the profile of said state, i.e. the temporal behavior of the state during a time interval, an auxiliary reference system is also used, that coincides, at the initial time of said time interval, with the reference system fixed with respect to the vehicle. 4. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that the result of the processing of the images, composed of a set of pixel tracks, where each pixel track describes the apparent motion of a characteristic point of the terrain in the sequence of the images, is further processed in order to obtain a set (TP) of tracks of unit vectors, where each unit vector track, (KI), is a function of time and describes the motion of said characteristic point in a reference frame fixed with respect to the ground. 5. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, to calculate the unit vector, (U), indicating the direction of the displacement of the vehicle between two time instants, the following operations are performed: - the set of tracks, (TP), obtained by processing the images and the measurements of the inertial measurement unit is processed to define at first a set of planes in space, all containing, in case of no measurement errors, the said unit vector (U), and then to define a set of unit vectors, (E), orthogonal to the said set of planes. - the unit vector closer to the perpendicular to the said set of vectors (E) is calculated, analytically minimizing the sum of the squares of the scalar products with the vectors of said set. 6. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, to calculate the ratio, (RHO), between the lengths of the displacements of the vehicle respectively in the timeframe from (TA) to (TM) and in the timeframe from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants for which at least one pixel track exists (KI ) that starts at (TA) or before, and ends at (TB) of after, the following operations are performed: - the directions (UA) and (UB) of the displacements of the vehicle respectively between time (TA) and (TM), and between (TM) and (TB) are calculated; - two triangles, (TRA) and (TRB), are defined having respectively the following vertices: a) the position of the vehicle at time (TA), the position of the vehicle at time (TM), and the point of the terrain tracked in the said pixel track (KI), and b) the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the point of the terrain tracked in the said pixel track (KI) ; - the set of tracks, (TP), obtained by processing the images and the measurements of the inertial measurement unit, is used together with the vectors (UA) and (UB), to calculate the angles of said triangles (TRA) and ( TRB) - the said ratio, (RHO), between the lengths of the displacements, is calculated, by using the said angles of the triangles (TRA) and (TRA), and applying the triangle law of sines; multiple estimates of (RHO) can be combined 7. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, to calculate the velocity of the vehicle in a given time instant (TA), and the gravity acceleration vector, the following operations are performed: - other two time instants, (TM) and (TB), are used to calculate the said unit vectors (UA) and (UB) and the ratio (RHO); - the values calculated at the previous step are used, together with the measurements of the inertial measurement unit, conveniently processed, to calculate the vector (DELTA_R) representing the displacement of the vehicle from (TA) to (TM); - if the gravity acceleration vector is not known, but only its norm is known, then from the said vector (DELTA_R), and from the value of the gravity acceleration norm, and from other values previously obtained, the gravity acceleration vector is determined; - from said gravity acceleration vector, and from others values previously obtained, the velocity at the time instant (TA) is calculated. 8. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, using also the measurements of an altimeter, then the displacement, (DELTA_R), between two time instants, is calculated by performing the following operations: - the direction, (U), of the said displacement vector is calculated; - the measurements of the altimeter are used to calculate the projection of the said displacement vector along the direction of the measurement of the altimeter; - the values of the said direction (U), of the said direction of measurement of the altimeter, and of the said projection are used to calculate the length of the said displacement (DELTA_R). 9. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, having available also the measurements of a LIDAR sensor, of the scanning type or flash type, then the displacement, (DELTA_R), between two time instants is calculated by performing the following operations: - the direction, (U), of the said displacement vector is calculated using the method described in Claim 5; - using said direction, the length of said displacement is determined by maximizing the overlapping between the points of the terrain measured with the LIDAR scans in the said two time instants. 10. Method as in Claim 2 to estimate the state of a moving vehicle, characterized in that, using as input data also a map of the observed terrain, including also an orthoimage, i.e. a pixel matrix, referred to a planeto-centric reference system, showing the aspect of the observed terrain, then the following operations are performed to calculate the profile of the state of the vehicle in the said reference system: - a comparison is done between the said orthoimage and the images observed by the vehicle, to obtain a set of points of the terrain, where for each point of said set, both the planetocentric coordinates and the coordinates giving the apparent position of the point on one or more of the acquired images are available; - for each of said images, a processing of the coordinates of said points on the image is performed, to obtain a set of unit vectors which give the directions of the position of said points with respect to the vehicle, in the said planetocentric reference frame ; - the position of the vehicle in the said planetocentric reference system is calculated, defining a half line for each pair of said point and unit vector, and then defining a function that, for each point in the three-dimensional space, calculates the sum of the squared distances between said point in space and the half lines previously defined, and finally minimizes the said function calculating, with an analytical method, the point for which the gradient of said function is null.