The present invention concerns a multi-sensor (multi-IMU and multi-GPS) GPS-aided inertial navigation system and method.
The subject of federated filtering, and specifically federated navigation with multiple IMUs, has received considerable attention over the past three decades. Reference [1] contains a broad systematic review of distributed Kalman filtering, not necessarily in the context of navigation. The seminal works of Carlson, references [2]-[6], have introduced decentralized/federated filtering ideas and techniques into the realm of navigation. These navigation approaches were thoroughly investigated at the USAF institute of technology, references [7]-[8]. Some more recent works on this subject are exemplified in references [9]-[11]. References [12]-[16] are books and papers on inertial navigation, and references [17]-[19] are the cornerstone paper on Kalman filtering and two well-known practical books on the subject. The basic kinematic and dynamic relationships between points within a rigid-body appear in numerous sources, e.g. reference [20].
However, regarding navigation, most if not all of the past research on multi-sensor federated navigation focused solely on its filtering aspects: central versus distributed/cascaded/federated filtering; the depth of information-sharing between the ‘master’ filter and the ‘local’ filters; states sizes and content (at ‘local’ and ‘master’ levels); with or without filters' resets (‘local’/‘master’); and the correlation incurred between local filters due to common dynamics and common updates, etc. An important virtue of multi-sensor federated navigation, emphasized in most if not all previous research on the subject, is redundancy (fault tolerance) and fault detection/isolation capability, regarding both ‘hard’ and ‘soft’ failures.
The present system includes a mobile rigid body (‘vehicle’, e.g. aircraft) on which a plurality of inertial measurement units (IMUs) and one or more GPS antennas and receivers are fixedly installed. A navigation computer (NC) completes the navigation hardware, and interfaces all the above hardware elements. The present GPS-aided inertial navigation system and method fuse the data from all available sensors to produce the navigation solution (position, velocity, attitude). It is assumed that one has already developed a single-IMU/single-GPS INS, and one wishes to modify it and create a multi-GPS/multi-IMU INS with minimal software changes and with minimal additional computational burden. These ‘minimality requirements’ practically exclude prior art federated navigation. Compared to a ‘baseline’ single-IMU/single-GPS inertial navigation system (INS), the present system and method provide (with IMUs and GPS receivers similar to the IMU and receiver of the baseline INS) better navigation accuracy with minimal additional algorithmic and computational complexity. The present method only minimally deviates from a single-IMU/single-GPS GPS-aided inertial navigation scheme, and results in performance and cost benefits associated with a multi-agent navigation system. The cost benefit stems from the fact that several relatively low-accuracy and inexpensive IMUs and GPS receivers can deliver navigation accuracy that otherwise (with a single-IMU/single-GPS INS) requires high-end and very expensive IMU and GPS receiver.
The present system and method provide a navigation approach that differs from past research and practice in several ways. First, there is only one navigation filter, and it practically is a basic ‘standard’ single-IMU/single-GPS navigation filter, except for minute filter-tuning changes. This filter does not have more states than a single-IMU/single-GPS navigation filter, and accommodates only one (fused) IN solution and one (fused) GPS solution, as if there were one IMU and one GPS receiver in the system. Further, despite including only a single ‘standard’ filter, the invention includes individual corrections and resets to all the IMUs and INs, and individual aidings to all the GPS receivers, similarly to the prior art, multi-filter federated navigation. And, the above is accomplished by intensive application of well-known rigid-body static, kinematic and dynamic internal relationships, rather than by yet another complex filtering scheme. Key to the individual sensor corrections is an innovative algorithm that computes a real-time least-squares solution for the body's angular acceleration. Fault detection/isolation, not discussed here, can also be supported within the scope of the invention.
The computational burden and software complexity here are just a little greater than in a single-IMU/single-GPS INS, however both the burden and complexity are significantly smaller than with federated navigation, which is the prior art of navigating with multiple IMUs, where there are multiple filters, usually a dedicated filter per each IMU plus a central filter.
In accordance with embodiments of one aspect of the present invention there is provided a multi-sensor GPS-aided inertial navigation method. The method includes providing multiple sensors, including multiple inertial measurement units (IMUs) and one or more global positioning system receivers and antennas (GPSs); and providing a navigation computer (NC) with embedded navigation software. The below activities of the NC may be performed by a computer which is not navigation-dedicated, however has the required hardware interfaces and navigation software.
The NC performs the following activities: interfacing with all IMUs and all GPS receivers; running, in parallel, multiple standard inertial navigation (IN) schemes, one per each IMU; computing the mean of all the INs, thus obtaining a fused IN solution for the IMUs' mean location (‘mean IMU’ location); computing the mean of all the GPS solutions, thus obtaining a fused GPS solution for the GPS antennas' mean location (‘mean antenna’ location); applying standard lever-arm corrections, with the vector from the ‘mean IMU’ location to the ‘mean antenna’ location, to the fused GPS solution; feeding the fused IN solution and the lever-arm corrected fused GPS solution to a single navigation filter, as if there were a single IMU and a single GPS; and running an IMU/IN/GPS correction module. The correction module is based on rigid-body physics and computes appropriate individual: (i) acceleration and angular rate corrections for each IMU; (ii) position, velocity and attitude corrections for each IN, and (iii) position, velocity, attitude, acceleration and angular rate aiding-message for each GPS receiver.
In some embodiments, the method uses an inertial navigation (IN) scheme per each IMU.
In some embodiments, the method uses only one navigation filter to accommodate one fused IN solution and one fused GPS solution, as if there were a single IMU and a single GPS.
In some embodiments, all corrections and aiding-messages are performed individually for each sensor and IN-output.
In some embodiments, all the corrections and aiding-messages are computed by algorithms based on rigid-body physics.
In some embodiments, the algorithms compute a real-time least-squares solution for the body's angular acceleration.
In some embodiments, individual IMU-corrections are computed as increments to prior corrections, and integrated.
In some embodiments, individual IMU-corrections are computed wholly, after each filter-update anew, without integrators.
In some embodiments, the computation of individual corrections to the sensors uses angular rate and angular acceleration lever-arm corrections, and filter-updated/lever-corrected linear accelerations at the individual IMUs' locations, to define either local IMU correction-increments (that undergo integration) or whole local IMU corrections.
In accordance with embodiments of another aspect of the present invention there is provided a GPS-aided inertial navigation system (INS). The system includes multiple IMUs, a NC with embedded navigation software, and at least one GPS receiver and antenna. The NC is configured to: (a) interface all IMUs and all GPS receivers; (b) run, in parallel, multiple standard inertial navigation (IN) schemes, one per each IMU; (c) compute the mean of all INs, thus obtaining a fused IN solution for the IMUs' mean location (‘mean IMU’ location); (d) compute the mean of all GPS solutions, thus obtaining a fused GPS solution for the GPS antennas' mean location (‘mean antenna’ location); (e) apply lever-arm corrections, with the vector from the ‘mean IMU’ location to the ‘mean antenna’ location, to the fused GPS solution; (f) feed the fused IN solution and the lever-arm corrected fused GPS solution to a single navigation filter, as if there were a single IMU and a single GPS; and (g) run an IMU/IN/GPS correction module. The correction module is based on rigid-body physic and computes appropriate individual (i) acceleration and angular rate corrections for each IMU, (ii) position, velocity and attitude corrections for each IN, and (iii) position, velocity, attitude, acceleration and angular rate aiding-messages for each GPS receiver.
LLLN or L—the true Local-Level Local-North (LLLN) frame, where XYZ is North-East-Down (NED)
ECEF or E—Earth-Centered Earth-Fixed (ECEF) frame
BODY or B—Body-fixed frame of the rigid body, where XYZ is forward-right-down
I—the Inertial frame
CG—Center of Gravity of the rigid body, and origin of B
IN—(standard) Inertial Navigation scheme
INS—(standard) Inertial Navigation System
KF—Kalman filter
MC—Monte Carlo simulation
CEP—Circular Error Probable, the radius of the circle in which 50% of the results reside
R95—The radius of the circle in which 95% of the results reside
˜(μ, R)—Normally (Gaussianly) distributed with mean μ and variance R
LIN—the (erroneous) LLLN computed by the IN
CMN—DCM for transforming vectors from frame M to N
CLB(ψ, θ, φ)—DCM for transforming vectors from some LLLN frame L (true or computed) to the body frame B, where the attitude of the body frame with respect to the LLLN frame is defined by the western standard Euler angles triplet ψ, θ, φ
Ψ=[ΨR, ΨP, ΨY]T—the (minute) angles vector relating LIN to LLLN; the RPY (Roll-Pitch-Yaw) attitude-errors of the IN
CBE—DCM for transforming vectors from the body frame B to ECEF frame E
PVT—Position (3-vectors), Velocity (3-vectors), Time
PVΘAΩ—Position, Velocity, attitude (Θ), Acceleration, and angular rate (Ω) (all 3-vectors)
Δθ, Δν—attitude and velocity increments (IMU outputs)
( )f—( ) fused
( )XZ—( ) of entity X, in frame Z
( )YXZ—( ) of entity X, relative to frame Y, in frame Z
ΩIBB—angular velocity (Ω) of frame B relative to frame I, expressed in frame B (the superscript)
rI
rA
rIA—vector in B from the ‘mean IMU’ location
rIA
rII
The INS assumed to exist (
The invention (
With reference to
The true ECEF position of the ith GPS receiver is PCGE(t)+CBE(t)rA
P
GPS
E(t)=PCGE(t)+CBE(t)rA
where vP
The fused GPS position-fix is obtained by summing equation (2) over the i=1, . . . , N available fixes and dividing by N:
where
Thus, for equal receivers, the mean of the receivers ECEF position-fixes represents a (fused) ECEF position-fix
The ith GPS receiver velocity-fix in ECEF at time t, denoted VGPS
where VCGE is the body's CG velocity in ECEF and vV(0, RV
The fused GPS velocity-fix is obtained by summing equation (4) over i=1, . . . , N and dividing by N:
where
Thus, the mean of the (equal) receivers ECEF velocity-fixes represents a (fused) ECEF velocity-fix
Before looking into IN fusion, note that each IN scheme is identical to the IN scheme of the baseline INS but is individually initialized, with its own best initially-known position and velocity, which may be significant for large bodies and non-rest initializations. If the body is ‘small’ and initially at rest, it may be practically acceptable to initialize all INs with the same position (and zero velocity). Attitude initialization is the same for all INs.
The {Δθ, Δν} output of each IMU is routed (after individual {A, Ω}-corrections, as in the baseline INS; the computation of the corrections will be described later) to a dedicated standard IN scheme. The {P, V, Ω} output of each IN is routed (after individual {P, V, Θ}-corrections, as in the baseline INS) to the IN fusion module; the latter also receives the corrected {Δθ, Δν}. See
Note that though individual errors of each IN will be addressed in this sub-section, and though each IN obviously ‘has’ an error covariance matrix, no ‘local’ filters operate in the navigation method of this invention. IN errors are discussed here just to show how the IMUs'-output noise parameters (that drive IN {P, V, Θ}-errors) should be used as process-noise parameters in the re-tuning of the (single, standard) navigation filter.
IN position-output fusion is similar to GPS position-fix fusion described above. Denote by PIN
P
IN
E(t)=PCGE(t)+CBE(t)rI
The fused IN position-output
where
is the ‘mean IN’ position-error. Thus,
Similarly,
is the ‘mean IN’ velocity-error and
The true CLLLNB(ψ, θ, φ) applies over the whole body, however each IN outputs a slightly different computed CL
By approximating the well-known full Euler-angles DCM formulation to minute angles, CLLLNL
Definition (5) and the latter approximation lead to the well-known result
C
LLLN
L
≈I−[Ψ×]. (13)
Hence, equation (11) can be well approximated as
Summing over j and dividing by M:
is the mean of the DCM-outputs of all the INs and
the INs' mean attitude-error (small-angles vector). Therefore, the fused attitude-output of the INs,
The true ΩIBB applies over the whole body, however each IMU outputs a slightly different measured Ωj, or ΔΘj. The measurement model is
Ωj(t)=ΩIBB(t)+eΩIN
The fused IN angular-rate output is the average of all Ωjs:
ēΩIN is the ‘mean IN’ angular-rate error; it has a covariance M times smaller than that of a single IN.
A fundamental physical fact about rigid bodies (see e.g. reference [20]) is that the inertial linear acceleration expressed in the body frame of a fixed arbitrary point D in the body, aIDB, defined by the fixed position-vector rD=[x, y, z]T in frame B, where the origin O of B is the body's CG, is related to the inertial linear acceleration of the CG, aIOB, through the body's inertial angular velocity ΩIBB and the body's inertial angular acceleration {dot over (Ω)}IBB:
Accelerometers measure inertial sensed acceleration (sa), not true inertial acceleration (a); these two types of acceleration are related by (see reference [15] section 4.3)
where G is the gravitational force acting on the body's total mass m. By subtracting G/m from both sides of equation (18) it is evident that equation (18) holds also for the sensed accelerations saIDB and saIOB.
By the above observations, the jth IMU (or IN) sensed acceleration measurement model is
sa
j=(saIBB)j+esaIN
where both ΩIBB and {dot over (Ω)}IBB apply over the whole body. Thus, when equation (20) is averaged over the M IMUs the following result is obtained for the fused acceleration
where
is the ‘mean IN’ sensed acceleration error, and its covariance is M times smaller than that of esaIN
The IN fusion discussion above leads to the following conclusions regarding filter tuning (de-termination of the matrices P0, Q and R, used in the filter) in the proposed scheme:
of the respective parameters of a single IMU.
of the baseline Q.
of the baseline R.
The fused IN represents
Both the aposteriori (after GPS-update) filter-state and the full-state navigation output apply only to the body-point defined by
Details regarding the standard computation of the {P, V, Θ}-reset in the navigation filter will not be described here. Note that in the present method (similarly to the baseline navigation scheme), after each GPS-update the integrators for the ‘fused {A, Ω}-corrections’ are incremented, and then deducted from the current apriori fused {A, Ω}, which are sums of the current individual corrected {A, Ω}, to obtain ‘fused filter-corrected’ (aposteriori) {A, Ω}.
The inertial navigation (IN) module within a standard INS is traditionally associated with two kinds of ‘corrections’, both performed at the GPS-fix frequency, which is the filter-update rate (and often also the filter-propagation rate), and both are performed after each filter-update.
Note that the {Δθ, Δν} output of the IMU is always corrected before being delivered to the IN; the correction is applied at the IN computation-rate (=IMU-output rate). The {Δθ, Δν} correction terms are integrals of the filter-states regarding {A, Ω} (
So, in a standard INS, after each filter-update:
In the present method, all the above items are individually performed for each IN. See
P
IN
E(+)
=P
r
E
+Ĉ
B
E
r
II
(22)
where ĈBE is the filter-updated DCM-output of the navigation system and (+) indicates (individual) after-reset data.
V
IN
E(+)
=V
r
E
+Ĉ
B
E
[{circumflex over (Ω)}×]r
II
(23)
Correcting Δθ is based on the fact that {circumflex over (Ω)} is the best-known current angular rate at all body-points, thus we wish to attain
Ωj(+)={circumflex over (Ω)},∀j (25)
by manipulating the individual Δθ (or Ω) correction terms. There are two ways to attain this: either directly define the aposteriori ‘full’ individual Ω-correction as the difference between the current individual raw rate-measurement and {circumflex over (Ω)}, thus dispensing with the integrators regarding the rates (more precisely, the rate-error estimation); or maintain rate integrators and define the individual rate-error estimation increment as the difference between the apriori local corrected rate and {circumflex over (Ω)} (this is the option applied in the example). Note that both ways differ from the standard practice (for a single IMU) of incrementing integrators with the appropriate filter-update states, and both lead to ‘noisier’ (however individual) IMU error-estimations.
Individual Δν (or A) corrections are trickier, since according to equation (18) computation of the linear acceleration at a given body-point, given the linear acceleration of the CG, requires knowing the angular acceleration {dot over (Ω)}IBB, which is neither measured nor calculated in INSs. Up to here, it was sufficient to just know that {dot over (Ω)}IBB is the same for the whole body.
One may try to somehow estimate {dot over (Ω)}IBB, for example by a filter based on {circumflex over (Ω)}. This approach incurs a detrimental time-lag. Instead, the present method applies a novel version of a known technique (see e.g. references [22],[23]), that will now be presented.
First, re-write equation (18) by switching sides for aIDB and aIOB:
a
IO
B
=aa
ID
B−([ΩIBB×][ΩIBB×]+[{dot over (Ω)}IBB×])rD.
Since the latter holds for any rD, we have for r1B and r2B
(aIBB)1−([ΩIBB×][ΩIBB×]+[{dot over (Ω)}IBB×])r1B=(aIBB)2−([ΩIBB×][ΩIBB×]+[{dot over (Ω)}IBB])r2B,
whence
(a1−a2)IBB−([ΩIBB×][ΩIBB×]+[{dot over (Ω)}IBB×])(r1B−r2B)=0. (26)
Examining equation (26) in our context, all IMU-locations are known and the IMUs measure accelerations and angular rates. Thus, (26) applied to any two IMUs is a set of three linear equations from which the angular acceleration 3-vector ΩIBB can be extracted, as follows: (26)
[{dot over (Ω)}IBB×](r1B−r2B)=(a1−a2)IBB−[ΩIBB×][ΩIBB×](r1B−r2B).
Note that the RHS above is completely known. The LHS can be simplified by noting that for any two vectors v1,v2
v
1
×v
2
=[v
1
×]v
2
=−v
2
×v
1
=−[v
2
×]v
1.
Hence,
[{dot over (Ω)}IBB×](r1B−r2B)=−[(r1B−r2B)×]{dot over (Ω)}IBB[(r1B−r2B)×]{dot over (Ω)}IBB=[ΩIBB×][ΩIBB×](r1B−r2B)−(a1−a2)IBB. (27)
Now, equation (27) in shorthand is C(1,2){dot over (Ω)}IBB=D(1,2) (the meaning of C(1,2) and D(1,2) is obvious), recognized as the familiar form Cx=D where the vector x is the unknown and the matrix C and vector D are given.
In order to actually apply equation (27), use any two IMU locations rI
[(rI
Since sak and sal and even {circumflex over (Ω)} contain biases and noises, the solution of equation (28) for {dot over (Ω)}IBB is erroneous to some extent. In order to minimize this error it is proposed to gather the available data from all the IMUs and obtain a minimum-variance least-squares solution for {dot over (Ω)}IBB, as follows.
Equation (28) can be written with any two IMUs. Apply eq. (28) to all possible IMU-pairs, concatenate the q M(M−1)/2 resulting equations into one over-determined equation, and solve it by a least-squares procedure. The details: shorthand (28) as
The least-squares solution of equation (29) is {dot over ({circumflex over (Ω)})}=(
To finalize, now that the body's angular acceleration is known it is possible to compute the individual IMU-output A-correction. Since equation (28) applies to any two body-points, choose point l to be the ‘mean IMU’, choose point k to be the jth IMU, and solve for say:
Here, saI is the fused filter-corrected (aposteriori) sensed acceleration at
Individual acceleration-corrections can now be computed in two ways (see the two individual rate-correction ways in the paragraph following equation (25)): with or without integrators.
An individual error-estimation A-increment applicable to the IMU at rI
Both ways differ on two counts from the standard practice (for a single IMU): because of the angular rate/acceleration lever-arm correction terms, and because of the use of local ‘best-known’ aposteriori acceleration (saj) to define the local increments (for the integrators), rather than using filter-update states. The resulting A-error estimations are ‘noisier’ than customary for a single IMU, however truly individual.
The {P, V, Θ, Ω} individual aiding items to the GPS receivers, which can be sent at IN-rate, are computed similarly to equations (22)-(25) except with rIA
sa
i
=sa
I+([{circumflex over (Ω)}×][{circumflex over (Ω)}×]+[{dot over ({circumflex over (Ω)})}×])rIA
When GPS signals become unavailable, after having been available for some time during the navigation, the system continues navigating like any standard INS, that is with pure-inertial navigation using the latest (and ‘frozen’) IMU-corrections. In fact, each IN within the system individually does that, and IN fusion goes on regardless of GPS status.
With GPS data, navigation performance regarding the ‘mean IMU’ location (or ‘IMUs center’)
However, the invention has an additional useful feature that is applicable during GPS outages: though navigation accuracy unavoidably deteriorates during the outage, the fused navigation solution is always significantly better than that of any individual IN within the system. Thus, individual IN-output reset (see equations (22)-(24)) can still be performed, for improved {P, V, Θ}navigation data regarding all rI
Note that rII
The numerical simulation example described below demonstrates the main features of the invention, with several IMUs and a single GPS receiver, and compares the invention's performance to the performance of an (almost) identical navigation scheme (with the same hardware), however lacking the individual IMU corrections.
The main reason for using one GPS receiver in the example, rather than several receivers, is the wish to highlight the contribution of the individual IMUs corrections. The approach to which the invention is compared applies (like the invention) full inertial navigation for each IMU, including individual initializations and individual PVΘ filter-resets. That is, the compared approach is significantly more involved than the basic multi-IMU approach, which consists in averaging the IMUs' outputs and running a single IN. The compared approach differs from the invention only in that it applies the same navigation filter acceleration and rate corrections to all the IMUs (rather than individual corrections to each IMU, in the invention).
The (simulated) hardware configuration used in all the navigation simulations includes five ADIS16448 low-grade IMUs, manufactured by ANALOG DEVICES. Four of these IMUs are ‘placed’ at the four vertices of a tetrahedron whose edge length is 0.2 meter; one IMU is ‘placed’ at the tetrahedron's center. See reference [23] for a discussion of IMU-arrangement effects. The simulation includes modelling of individual IMU locations and misalignments, and of individual bias/scale-factor/random-walk errors (based on the ADIS16448's specification) for each of the 30 sensors (an IMU includes an accelerometer triad and a rate-gyro triad). A second hardware configuration tested was a collapsed version of the first, with all IMUs co-located. For fairness, the same random sequences were used in the simulation runs with the compared approach.
A 3-minute trajectory of a rocket was used, which includes the boost phase (about 20 seconds) and a portion of the ballistic phase. GPS reception is available for the first 2 minutes. Note that the attitude accuracy of navigation in a high-altitude ballistic trajectory is usually poor.
The reason for this success can be seen in
The invention and the compared approach have also been statistically compared by Monte-Carlo simulation runs. The resulting horizontal position navigation accuracies, in terms of CEP and R95, are presented in
Obviously, one can run both an inertial navigation scheme and an individual navigation filter per IMU, and fuse their outputs (see references [10], [2]-[6]) to attain performance similar to that of the invention, perhaps even better. However, the associated real-time code complexity, the extent of deviation from an existing single-IMU/single-GPS real-time code, and the resulting computational load would all be much greater than with the present invention.
Number | Date | Country | Kind |
---|---|---|---|
234691 | Sep 2014 | IL | national |
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/IL2015/050921 | 9/9/2015 | WO | 00 |