U.S. patent number 4,680,715 [Application Number 06/734,141] was granted by the patent office on 1987-07-14 for method for navigating vehicles, particularly land vehicles.
This patent grant is currently assigned to Teldix GmbH. Invention is credited to Bernd-Christian Pawelek.
United States Patent |
4,680,715 |
Pawelek |
July 14, 1987 |
Method for navigating vehicles, particularly land vehicles
Abstract
A navigation method for the navigation of a vehicle employing
direction and longitudinal movement sensors. Signals from the
sensors are fed to a position computer which determines the vehicle
position relative to a coordinate grid system. When the vehicle
reaches a position of known coordinates the position values which
are determined from the sensor signals are corrected with reference
to the known coordinates, and a Kalman filter is provided which
effects an error estimate and subsequently a correction value
determination from the error estimate which results in a
significant increase in accuracy of the displayed navigation data.
The particular advantage of the method is the use of uncomplicated
sensors which are able, in connection with the Kalman filter, to
obtain highly accurate vehicle position data.
Inventors: |
Pawelek; Bernd-Christian
(Wiesloch, DE) |
Assignee: |
Teldix GmbH (Heidelberg,
DE)
|
Family
ID: |
6235936 |
Appl.
No.: |
06/734,141 |
Filed: |
May 15, 1985 |
Foreign Application Priority Data
|
|
|
|
|
May 16, 1984 [DE] |
|
|
3418081 |
|
Current U.S.
Class: |
701/536; 340/988;
701/518 |
Current CPC
Class: |
G01C
21/28 (20130101); G01C 21/12 (20130101) |
Current International
Class: |
G01C
21/10 (20060101); G01C 21/12 (20060101); G01C
21/28 (20060101); G06F 015/50 () |
Field of
Search: |
;364/424,443,449,450,454,571 ;73/178R ;340/988 ;342/450,451 |
References Cited
[Referenced By]
U.S. Patent Documents
Foreign Patent Documents
Other References
"Vehicle Navigation System FNA 4-15" Operation Manual, Teldix GmbH,
Heidelberg, published prior to Oct. 8, 1975, pp. 5-34, and FIGS.
I-V..
|
Primary Examiner: Chin; Gary
Attorney, Agent or Firm: Spencer & Frank
Claims
What is claimed is:
1. A method for the navigation of a vehicle, the vehicle including:
course reference means for furnishing a course angle signal
.theta.M which represents the direction of the vehicle with
reference to an earth-bound coordinate system; longitudinal
movement sensor means for detecting longitudinal movement of the
vehicle and generating a longitudinal movement signal VM
corresponding to the longitudinal movement of the vehicle; position
computer means for calculating vehicle position data, segragated
into north and east position values, from signals generated by the
course reference means and the longitudinal movement sensor means;
display means connected to the position computer for displaying
vehicle position data calculated by the position computer; and
input means including at least one of manual input means and signal
receiving means for providing navigation support data including at
least one of additional position, course, velocity and path data;
said method comprising:
checking the longitudinal movement signal VM and the course angle
signal .theta.M for plausibility;
adding a known, empirically derived, deterministic velocity error
component value DF(V) to the VM signal to produce, a corrected
longitudinal movement signal CV;
optimally estimating, with the use of a Kalman filter, the
stochastic position and direction errors contained in the VM and
.theta.M signals and using the estimate of such errors to calculate
direction and change-in-direction correction values C(.theta.) and
C(.epsilon.), respectively, and north and east position correction
values C(RN) and C(RE), respectively;
adding the direction correction value C(.theta.) and a known,
empirically derived, deterministic course angle error component
value DF(.theta.) to the .theta.M signal to produce a corrected
course angle signal C.theta.;
feeding the position correction values C(RN) and C(RE) to the
position computer means for use in correcting the position
data;
forming corrected north and east component signals CVN and CVE,
respectively, from the corrected longitudinal vehicle movement
signal CV and from the corrected course angle signal C.theta. and
feeding the CVN and CVE signals to the position computer means;
calculating, with the use of the position computer means, corrected
north and east position coordinate values CRN and CRE,
respectively, in dependence of the C(RN) and C(RE) correction
values and the CVN and the CVE corrected north and east component
signals;
obtaining north and east position bearing data RNS.sup.(jP) and
RES.sup.(jP), respectively, from the input means;
comparing the corrected north and east position coordinate values
CRN and CRE with the position bearing data RNS.sup.(jP) and
RES.sup.(jP), respectively, to form north and east position bearing
signals CZN.sup.(jP) and CZE.sup.(jP), respectively; and
feeding the CZN.sup.jP) and CZE.sup.(jP) signals to the Kalman
filter, with the Kalman filter developing the following error model
of the vehicle course angle error:
wherein .DELTA..theta..sub.1 (t) comprises a component of
exponentially, time correlated, colored noise; .DELTA..theta..sub.2
(t) comprises a time linearly variable component representing drift
angle with an unknown starting value .DELTA..theta..sub.2 (0) and
an unknown pitch .epsilon.(t) representing a random ramp process;
and .DELTA..theta..sub.3 (t) comprises a component of Gaussian
white, time uncorrelated, noise; and wherein the component
.DELTA..theta..sub.1 (t) is described by a form filter excited with
white noise in a Gauss-Markov process of the first order; and error
which is contained in the position bearing data, RNS.sup.(jP) and
RES.sup.(jP) is developed solely by stationary Gaussian white, time
uncorrelated, noise.
2. Navigation method as defined in claim 1, wherein the input means
provides at least one of course and longitudinal vehicle movement
support data, .theta.S.sup.(j.theta.) and VS.sup.(jV),
respectively, and said method further includes:
comparing at least one of the corrected signals C.theta. and CV
with a respective one of the provided course and longitudinal
vehicle movement support data .theta.S.sup.(j.theta.) and
VS.sup.(jV) for the generation of at least one of course and
vehicle longitudinal movement support signals
CZ.theta..sup.(j.theta.) and CZV.sup.(jV), respectively; and
feeding such support signals to the Kalman filter, with the Kalman
filter developing the following error model for the stochastic
component of the course angle error:
wherein .DELTA..varies..sub.1 (t) comprises a component of
exponentially, time correlated, colored noise; .DELTA..theta..sub.2
(t) comprises a time linearly variable component representing drift
angle with an unknown starting value .DELTA..theta..sub.2 (0) and
unknown pitch .epsilon.(t) representing a random ramp process; and
.DELTA..theta..sub.3 (t) comprises a component of Gaussian white,
time uncorrelated, noise; and wherein the component
.DELTA..theta..sub.1 (t) is described by a form filter excited with
white noise in a Gauss-Markov process of the first order; error
which is contained in the support data, .theta.S.sup.(j.theta.) and
VS.sup.(jV) is developed solely by stationary Gaussian white, time
uncorrelated, noise.
3. Naviagation method as defined in claim 1, including:
arranging the Kalman filter for optimum estimation of a stochastic
longitudinal vehicle movement error components of the VM signal,
with the Kalman filter developing the following error model for the
longitudinal vehicle movement error:
from which a longitudinal vehicle movement correction value C(V) is
calculated, and wherein .DELTA.V.sub.1 (t) is a component of
exponentially, time correlated, colored noise and .DELTA.V.sub.2
(t) is a component of Gaussian white, time uncorrelated, noise; the
component .DELTA.V.sub.1 (t) is described by a form filter excited
by white noise in a Gauss-Markov process of the first order;
and
adding the correction value C(V) to the VM signal.
Description
BACKGROUND OF THE INVENTION
The present invention relates to a method for the navigation of a
vehicle, wherein the vehicle includes a course reference device
which furnishes a course signal which represents the direction of
the vehicle with reference to an earthbound coordinate system; a
longitudinal movement sensor for detecting longitudinal movement of
the vehicle and generating a longitudinal movement signal; a
position computer for calculating vehicle position data, segregated
into north and east position values, from signals generated by the
course reference device and the longitudinal movement sensor;
display means connected to the position computer for displaying
vehicle position data calculated by the position computer; and
input means including manual input means and signal receiving means
for providing, respectively, additional position data and course,
velocity and path data for navigation support.
A navigation system of this type is described in German Pat. No.
3,033,279. Such a navigation system is used for determining the
position of a vehicle in a grid coordinate system, namely the UTM
(Universal Transverse Mercator) grid system. The vehicle position
is determined from the course angle furnished by a course reference
device with reference to the UTM grid coordinate system and from
distance signals obtained by integration of the vehicle speed.
Position errors occurring during travel, which have no linear
relationship to the path traveled or the travel time, are
eliminated in that, at the moment at which the vehicle is at a
known point in the terrain, a comparison is made between the
displayed location and the actual location of the vehicle, a path
adaptation factor is determined and the course angle is
corrected.
However, it has been found to be desirable to correct the indicated
positon not only when a known terrain point is reached, but also to
make a correction of the displayed data continuously and in a
discrete-time manner.
SUMMARY OF THE INVENTION
It is therefore an object of the present invention to provide a
navigation system which, with the use of simple sensors, furnishes
all navigation data with the greatest accuracy, with such accuracy
remaining constant over time.
The above and other objects of the invention are accomplished by a
method for navigation of a vehicle in the context of a vehicle
which includes: a course reference means for furnishing a course
angle signal .theta.M which represents the direction of the vehicle
with reference to an earthbound coordinate system; longitudinal
movement sensor means for detecting longitudinal movement of the
vehicle and generating a longitudinal movement signal VM
corresponding to the longitudinal movement of the vehicle; position
computer means for calculating vehicle position data, segregated
into north and east position values, from signals generated by the
course reference means and the longitudinal movement sensor means;
display means connected to the position computer for displaying
vehicle position data calculated by the position computer; and
input means including at least one of manual input means and the
signal receiving means for providing navigation support data
including at least one of additional position, course, velocity and
path data; said method comprising:
checking the longitudinal movement signal VM and the course angle
signal .theta.M for plausibility;
adding a known, empirically derived, deterministic velocity error
component signal DF(V) to the VM signal and a known, emipirically
derived, deterministic course angle error component value to the
.theta.M signal to produce, respectively, a corrected longitudinal
movement signal CV and a corrected course angle signal
C.theta.;
optimally estimating, with the use of a Kalman filter, the
stochastic position and direction errors resulting from the VM and
.theta.M signals and using such errors to calculate direction and
change-in-direction correction values C(.theta.) and C(.epsilon.),
respectively, and north and east position correction values C(RN)
and C(RE), respectively;
adding the direction correction value C(.theta.) to the .theta.M
signal;
feeding the position correction values C(RN) and C(RE) to the
position computer means for use in correcting the position
data;
forming corrected north and east component signals CVN and CVE,
respectively, from the corrected longitudinal vehicle movement
signal CV and from the corrected course angle signal C.theta. and
feeding the CVN and CVE signals to the position computer means;
calculating, with the use of the position computer means, corrected
north and east position coordinate values CRN and CRE,
respectively, in dependence of the C(RN) and C(RE) correction
values and the CVN and the CVE corrected north and east component
signals;
obtaining north and east position bearing data RNS.sup.(jP) and
RES.sup.(jP), respectively, from the input means;
comparing the corrected north and east position coordinate values
CRN and CRE with the position bearing data RNS.sup.(jP) and
RES.sup.(jP), respectively, to form north and east position bearing
signals CZN.sup.(jP) and CZE.sup.(jP), respectively; and
feeding the CZN.sup.(jP) and CZE.sup.(jP) signals to the Kalman
filter, with the Kalman filter developing the following error model
of the vehicle course angle error:
wherein .DELTA..theta..sub.1 (t) comprises a component of
exponentially, time correlated, colored noise; .DELTA..theta..sub.2
(t) comprises a time linearly variable component representing drift
angle with an unknown starting value .DELTA..theta..sub.2 (O) and
an unknown pitch .epsilon.(t) representing a random ramp process;
and .DELTA..theta..sub.3 (t) comprises a component of Gaussian
white, time uncorrelated, noise; and wherein the component
.DELTA..theta..sub.1 (t) is described by a form filter excited with
white noise in a Gauss-Markov process of the first order, error
which is contained in the position bearing data, RNS.sup.(jP) and
RES.sup.(jP), is developed solely be stationary Gaussian white,
time uncorrelated, noise and C(.theta.), C(.epsilon.), C(RN),
C(RE), CVN, CVE, CRN, CRE, RNS.sup.(jP), RES.sup.((jP),
CZN.sup.(jP), CZE.sup.(jP), .DELTA..theta.(t), .DELTA..theta..sub.2
(t), .DELTA..theta..sub.3 (t) and .epsilon.(t) are defined in the
detailed description below.
A significant advantage of the invention lies in the provision of a
navigation system which receives navigation signals from sensors in
the vehicle, such as the course and velocity or path sensors, as
well as from additional input means, and forms, by means of the use
of a modified Kalman filter, optimized navigation data therefrom.
Additional input means include, for example, manual input of the
position, as well as receiving devices for radio and/or satellite
navigation methods known, for example, by the names "Transit" or
"GPS Navstar" (see in this connection German Offenlegungsschrift
[laid-open patent application] No. 2,043,812).
According to a further feature of the invention, course and/or
longitudinal vehicle movement support data are derived from the
signals of a satellite navigation system and compared with the
corrected signals of the course reference device and/or the signals
of the longitudinal vehicle movement sensor. The comparison data
are then likewise fed to the error behavior model forming block and
to the Kalman filter.
According to yet another feature of the invention, a compensation
of stochastic longitudinal vehicle movement error components is
accomplished in addition to the compensation of deterministic
course and velocity error and the stochastic course error
components, for which purpose corresponding velocity correction
values (C(V)) are formed by means of the Kalman filter longitudinal
movement error estimation and these correction values are added to
the longitudinal movement signals VM of the vehicle.
BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is a block circuit diagram of a navigation system employing
a Kalman filter for implementing the method according to the
invention.
FIG. 2 is a time sequence diagram for the individual steps of the
method according to the invention.
FIG. 3 is a diagram showing a dead reckoning position.
FIG. 4 is a block circuit diagram for a simple navigation system
with Kalman filter employing only manual position input which can
be used to implement the method according to the invention.
DESCRIPTION OF THE PREFERRED EMBODIMENTS
Referring to FIG. 1, there is shown a navigation system as it is
used, for example, in a land vehicle. Longitudinal vehicle movement
is sensed by a velocity sensor 1 which produces a measured speed
value (VM), and course direction is detected by a direction sensor
2, for example a course gyro, which produces a measured course
angle (.theta.M) value. Velocity sensor 1 and direction sensor 2
are of known design, for example as described in the "Operation
Manual, Vehicle Navigation System FNA 4-15", provided by Teldix
GmbH of Heidelberg, Federal Republic of Germany. The measured
values (VM) and (.theta.M) furnished by sensors 1 and 2,
respectively, are values which include errors and are thus checked
for plausibility based upon changes in course and velocity, maximum
value determinations and statistical diagnosis calculation concepts
such as, mean value and variance estimates. Such errors are, in
particular, due to seeming drift, random drift, wheel slip and the
like. Therefore, known deterministic error component values (DF(V))
and (DF(.theta.)), which are empirically derived values, are added
to the measured values at linkage points 3, 4. Moreover, direction
(course angle) correction values (C(.theta.)) and velocity
correction values (C(V)) furnished by a Kalman filter to be
described in greater detail below are added at these linkage
points, with (C(V)) being adapted to the actually measured velocity
values from the velocity sensor via a proportionality device 5
which generates a proportionality factor.
The thus corrected signals for velocity and course are fed to a
base navigation unit 6 which segregates the velocity into component
values for north and east and feeds these values to position
computer 7 and an error behavior model forming block 8 to determine
the error ratio.
Position computer 7 also receives starting conditions (B.sub.A),
such as original location, starting orientation of the course gyro
and of the vehicle, starting time and starting speed for
determining the dead reckoning position in the north and east
directions. If the vehicle reaches a terrain point for which the
coordinates are known, for example a certain geodetic point, then
the coordinates of that point are fed to the navigation system
through an input unit 9 and are compared at linkage points 10 and
11, respectively, with respective ones of the north and east values
of the dead reckoning position.
Input unit 9 additionally serves as a display means for radio
and/or satellite navigation devices which may be provided in the
vehicle, and which are able to furnish the actual vehicle position
information, which also must be checked for plausibility, and
corresponding course and/or velocity data. In this case, not only
are the position signals from the radio and/or satellite navigation
systems compared with the dead reckoning position but additionally
comparisons are made at linkage points 12 and 13, respectively,
between the corresponding velocity and/or course signals and the
corrected signals from the velocity sensor 1 and/or direction
sensor 2. Any existing deviations in position in the two coordinate
directions (CZN.sup.(jP)), (CZE.sup.(jP)), as well as the course
and/or velocity differences (CZ.theta..sup.(jP)) and/or
(CZV.sup.(jV)), respectively, are fed to the error behavior model
forming block 8 as well as to Kalman filter 15.
In addition to the already mentioned direction and velocity
correction values (C(.theta.)) and (C(V)), the Kalman filter also
furnishes direction change corrections C(.epsilon.) which are fed
to the error behavior model forming block 8, as well as position
correction values in the north and east directions (C(RN)) and
(C(RE)), respectively, which are additionally fed to position
computer 7 for a correction of the dead reckoning position
data.
Kalman filter 15 serves to estimate all of the modelled navigation
errors.
The thus extrapolated navigation errors are utilized to calculate
the above-mentioned correction values which are returned to the
navigation system for the compensation of errors. The thus designed
system forms a closed control circuit which automatically furnishes
the "optimally" corrected navigation values which can be displayed
by a suitable display means 16.
The starting point of the method of the invention is in the
so-called base navigation system, which is composed of direction
sensor 2 (course gyro) as well as the velocity sensor 1. For the
case of "navigation in the plane", the physical base navigation
equations, i.e. equations for dead reckoning navigation from
vehicle speed V(t) and course angle .theta.(t) (see FIG. 3), are as
follows: ##EQU1## where RN(t) and RE(t) are the vehicle positions
in the north (N) and east (E) directions, respectively.
VN(t) and VE(t) are the vehicle speeds in the north (N) and east
(E) directions, respectively, and
t is time
As already mentioned above, the vehicle position values resulting
from dead reckoning according to Equations (1) and (2) are wrong
due to the errors made by the course and velocity sensors and such
errors are corrected by navigation support data--it being assumed
that these also contain errors.
Below is a description of the formulation of the error behavior
model forming and Kalman filter algorithms for the case in which
position data are fed in from time to time exclusively by manual
input means, such as that described in Teldix Operation Manual for
the FNA 4-15 Vehicle Navigation System referred to above and as
diagrammatically shown in FIG. 4. Input unit 9' in FIG. 1 also
accepts position data which is fed in manually as well as
additional navigation support data which it receives via radio
and/or satellite receiving devices as previously noted. The
following model assumptions are then made for the individual
measured values:
Measured vehicle velocity signal VM(t) from velocity sensor and
checked for plausibility:
where
.DELTA.V(t) is the error-free vehicle speed, and
V(t) is the velocity error
Measured course angle signal .theta.M(t) from direction sensor and
checked for plausibility:
where
.theta.(t) is the error-free course angle and
.DELTA..theta.(t) is the course angle error
Measured vehicle position (position fix) RNS(t), RES(t):
where
RN(t) and RE(t) are error-free vehicle positions in the north (N)
and east (E) directions, respectively; and
.DELTA.RNS(t) and .DELTA.RES(t) are the position measurement
(bearing) errors in the north (N) and east (E) directions,
respectively.
For the "real" base navigation system, Equations (1), (2), (3) and
(4) provide the following continuous-time system equations:
where RN.sup.FOA (t) and RE.sup.FOA (t) are erroneous position
coordinates of the vehicle navigation/orientation system (FOA)
determined from the measured base navigation values by means of
dead reckoning.
The continuous-time measurement (bearing) equations are obtained by
a comparison of the location resulting from dead reckoning
(RN.sup.FOA (t), RE.sup.FOA (t)) with the measured (position fix)
vehicle position (RNS(t), RES(t)), respectively. This means:
where ZN(t) and ZE(t) are the differences between dead reckoning
and bearing in the north (N) and east (E) directions,
respectively.
CONTINUOUS-TIME ERROR EQUATIONS
The use of the error propagation theorem for Equations (7a/7b) as
well as (8a/8b) furnishes the following error equations:
Errors in the base navigation system.fwdarw.system errors:
where .DELTA.RN(t) and .DELTA.RE(t) are position errors after dead
reckoning.
Errors due to position bearings (fixes).fwdarw.measuring
errors:
where .DELTA.ZN(t) and .DELTA.ZE(t) are position error differences
in the north (N) and east (E) directions, respectively, with the
individual errors being modelled as follows:
Velocity error modelling
With the assumption that the (stochastic) speed error can be
modelled by a sum of (time) correlated, i.e. colored noise
(describable by Guass-Markov processes of the first order) and
Gaussian white, i.e. (time) uncorrelated, noise, the following
results:
where the following definitions apply:
.DELTA.V.sub.1 (t)=-.beta..sub.V1 (t).multidot..DELTA.V.sub.1
(t)+W.sub.V1 (t)=form filter description for the error component in
the Gauss-Markov process of the first order;
.beta..sub.V1 =reciprocal autocorrelation time of the form
filter;
.DELTA.V.sub.1 (O).about.N[O;E(.DELTA.V.sub.1.sup.2
(O))=.sigma..sub.V1.sup.2 ]=abbreviated form for the starting value
.DELTA.V.sub.1 (O) of the Gauss-Markov error component with normal
(N) distribution, starting mean O and starting variance
.sigma..sub.V1.sup.2 (O)=.sigma..sub.V1.sup.2 ;
W.sub.V1 (t)=q.sub.V1 .multidot.W(t).about.N[O;E(W.sub.V1.sup.2
(t))=q.sub.V1.sup.2 ]=abbreviated form for the stationary white
noise which drives the form filter with normal (N) distribution,
mean O and spectral power density q.sub.V1.sup.2 ;
.DELTA.V.sub.2 (t).about.N[O;E(.DELTA.V.sub.2.sup.2
(t))=q.sub.V2.sup.2 ]=abbreviated form for the error component of
stationary white noise with normal (N) distribution, mean O and
spectral power density q.sub.V2.sup.2 ; ##EQU2##
Course angle error modelling
For forming the model of the (stochastic) course angle error, it is
assumed that the latter is additively composed of a component of
exponentially (time) correlated (colored) noise
.DELTA..theta..sub.1 (t), a component .DELTA..theta..sub.2 (t)
which is linearly variable in time (drift angle) having an unknown
starting value .DELTA..theta..sub.2 (O) and unknown pitch
.epsilon.(t) (random ramp process) as well as a component of
Gaussian white, i.e. (time) uncorrelated, noise
.DELTA..theta..sub.3 (t). The error component .DELTA..theta..sub.1
(t) can here again be described by the form filter excited by white
noise in the Gauss-Markov process of the first order. As a whole,
the following course angle error model is then obtained:
where
.DELTA..theta..sub.1 (t)=-.beta..sub..theta.1
.multidot..DELTA..theta..sub.1 (t)+W.sub..theta.1 (t)=form filter
description for the error component in the Gauss-Markov process of
the first order;
.beta..sub..theta.1 =reciprocal autocorrelation time of the form
filter;
.DELTA..theta..sub.1 (O).about.N[O;E(.DELTA..theta..sub.1.sup.2
(O))=.sigma..sub..theta.1.sup.2 ]=abbreviated form for the starting
value .DELTA..theta..sub.1 (O) of the Gauss-Markov error component
with normal (N) distribution, starting mean O and starting variance
.epsilon..sub..theta.1.sup.2 (O)=.epsilon..sub..theta.1.sup.2 ;
W.sub..theta.1 (t)=q.sub..theta.1
.multidot.W(t).about.N[O;E(W.sub..theta.1.sup.2
(t))=q.sub..theta.1.sup.2 ]=abbreviated form for the stationary
white noise which drives the form filter with normal (N)
distribution, mean O and spectral power density
q.sub..theta.1.sup.2 ; ##EQU3## .DELTA..theta..sub.3
(t).about.N[O;E(.DELTA..theta..sub.3.sup.2
(t))=q.sub..theta.3.sup.2 ]=abbreviated form for the error
component of stationary white noise with normal (N) distribution,
mean O and spectral power density q.sub..theta.3.sup.2 ;
##EQU4##
Position error modelling
The mathematical modelling of the errors occurring during position
fixes (bearings) is effected under the assumption that they can be
described by Gaussian white, i.e. normally distributed,
uncorrelated, noise. In vector representation, this results in the
following position error model; ##EQU5## with
and ##EQU6##
By inserting Equations (11) through (13) into Equations (9) and
(10), the following equation systems are obtained which describe
the entire error behavior of the present navigation system:
Continuous-time system error equations: ##EQU7##
Continuous-time measurement (bearing) error equations:
The space state representation of the above equations suitable for
design of a Kalman filter, after introduction of the following:
State vector:
System noise vector:
Measurement (bearing) vector:
Measurement (bearing) noise vector:
provides: ##EQU8##
Equations (20) and (21) thus define the error values at the output
of error behavior model forming block 8.
System matrix A(t): ##EQU9##
System noise input matrix D(t): ##EQU10##
Measurement (bearing) matrix M(t): ##EQU11##
System noise matrix Q(t): ##EQU12## where ##EQU13##
Measurement (support) noise matrix V(t):
where ##EQU14## and
Uncorrelated system and measurement noise: ##EQU15##
Providing discrete time
The present navigation system can be realized or simulated with the
aid of a digital computer, particularly a microcomputer, for
example, a fixed program system of two or three microprocessors,
such as Motorola MC 68000 microprocessors integrated with GPS
Navstar. The blocks within the dashed lines of FIGS. 1 and 4 can be
realized by such a microprocessor system. For such a digital
system, the continuous-time system and measurement error
(differential) equations (14) and (15) and (16) through (27),
respectively, must be converted to discrete-time differential
equations--the position fixes (bearings) being taken at discrete
instants in time in any case.
The "time axis" shown in FIG. 2 is intended to explain the
connections between continuous time t, the processing times
required to implement the dead reckoning and Kalman filter
calculations and the instants in time at which position fixes
(bearings) are taken.
The following then apply:
T.sub.KO =duration of dead-reckoning cycle within which dead
reckoning is performed once;
T.sub.KA =duration of a Kalman cycle within which the Kalman filter
calculation is performed once;
T.sub.i =instants in time at which position fixes (bearings) are
taken, i=1, 2, 3, . . . ;
where
l=number of dead reckoning cycles, with l=0, 1, 2, 3, . . . ;
and
k=number of Kalman cycles, with k=0, 1, 2, 3, . . .
DISCRETE-TIME SYSTEM EQUATIONS
The transition from a continuous-time to a discrete-time system
takes place in discrete-time conversion block 14 in FIG. 1 by way
of a determination of the so-called transition matrix. For this
purpose, the broken series set-up is proposed.
With the assumption that the continuous-time system matrix A(t) is
constant during one Kalman interval T.sub.KA =(t.sub.k -t.sub.k-1)
and that T.sub.KA can be selected sufficiently small, the following
results for the transition matrix within the time interval (t.sub.k
=kT.sub.KA, t.sub.k-1 =(k-1)T.sub.KA) ##EQU16## where k=1, 2, 3, .
. . ;
A(t.sub.K)=system matrix at time t.sub.k =(k)T.sub.KA ; and
I=unit matrix
Because the most suitable Kalman cycle duration T.sub.KA from a
calculation point of view often becomes too large for the above
assumption of A.sub.k,k-1 =constant; k=1, 2, 3, . . . , T.sub.KA is
subdivided into ##EQU17## where wnm is a whole number multiple;
identical strips T.sub.KO =(t.sub.l -t.sub.l-1) (the dead reckoning
cycle duration T.sub.KO can be used for this purpose) and the
following can then be set up: ##EQU18## where ##EQU19## according
to Equation (29) and A(t.sub.l) is the system matrix at time
t.sub.l =lT.sub.KO.
In this way, continuous-time system error equation (20) changes to
the discrete-time form
.DELTA.X.sub.O is given;
with the discrete-time system noise vector ##EQU20## and D(.tau.),
W(.tau.) according to Equations (23) and (17).
Corresponding to the procedure in the determination of the
discrete-time system noise vector according to Equation (34), the
discrete-time system noise matrix is obtained as follows: ##EQU21##
where E(W.sub.k-1)=O
k=1, 2, 3, . . . and
Q=E(W(.tau.)WTt.tau.)) according to Equation (25)
For an approximated calculation of Q.sub.k-1, the trapezoidal
integration method is recommended. Accordingly, the following
results: ##EQU22## where D(t.sub.K) is the system noise input
matrix at time t.sub.K =kT.sub.KA.
Since position bearings (fixes) are taken exclusively at discrete
instances in time t=T.sub.i ; i=1, 2, 3, . . . , continuous-time
measurement (bearing) error equation (21) changes to
where M(t=T.sub.i) according to Equation (24).
In discrete-time form, this means: ##EQU23##
Equations (33) and (38) are therefore the main equations for the
discrete-time conversion performed in block 14.
For the discrete-time measurement (bearing) noise matrix, the
following results:
where
E(V.sub.k)=O and
V is used according to Equation (26)
For discrete-time dead reckoning for a calculation of the dead
reckoning position from the actual velocity and course
informations, either of the following two methods can be
employed:
METHOD 1
In this method, differential equations (7a) and (7b) which describe
the "real" base navigation system are put directly in discrete-time
form, i.e. the rectangular integration method is used. The
following then results as the dead reckoning position at time
t.sub.l+1 =(l+1)T.sub.KO ##EQU24##
METHOD 2
The use of the trapezoidal integration method with Equations (7a)
and (7b) furnishes somewhat more accurate results. According to
this method, the following results: ##EQU25## Discrete-time Kalman
filter algorithms (simultaneous bearing data processing)
Discrete-time Kalman filter algorithms suitable for realization by
microcomputer are formulated as follows:
Recursive prediction (extrapolation) algorithms for a priori system
error estimation
A priori estimation error .DELTA.X.sub.k at time t.sub.k =kT.sub.KA
:
where k=1, 2, 3, . . .
.sub..PHI.k,k-1 is according to Equation (31)
A priori estimation covariance matrix P.sub.k * at time t.sub.k
=kT.sub.KA : ##EQU26## where k=1, 2, 3, . . . ;
D(t.sub.K) is the system noise input matrix at time t.sub.k
=kT.sub.KA ; and
Q is according to Equation (25)
Starting estimation error covariance matrix P.sub.o (to be suitably
given).
Algorithms for the correction of the a priori system error
estimation by measurements (position fixes):
Amplification matrix B.sub.k at time t.sub.k =kT.sub.KA : ##EQU27##
where i=1, 2, 3, . . . ;
k=1, 2, 3, . . . ;
M is according to Equation (24); and
V is according to Equation (39).
A posteriori estimation error .DELTA.X.sub.k at time t.sub.k
=kT.sub.KA : ##EQU28## where i=1, 2, 3, . . . ;
k=1, 2, 3, . . . ; and
Z.sub.k =(ZN(t.sub.k), ZE(t.sub.k)).sup.T according to Equations
(8a) and (8b);
A posteriori estimation error covariance matrix P.sub.k at time
t.sub.k =kT.sub.KA :
where ##EQU29## and i=1, 2, 3, . . .
k=1, 2, 3, . . .
MODIFIED ALGORITHMS FOR THE CORRECTED NAVIGATION SYSTEM
The discrete-time Kalman filter 15 thus furnishes quasi
continuously, in addition to the a priori estimation errors and the
a priori and a posteriori estimation error covariance matrices,
also the a posteriori estimation errors. From these estimation
errors with minimum error variance, "optimum" correction values can
now be calculated directly and these are returned to the navigation
system for error compensation. The thus resulting navigation system
is a closed control circuit, corrected navigation system, which
then automatically produces the "optimally" corrected navigation
data, i.e. data with minimum errors.
For the corrected navigation system, the modified algorithms as a
result of returns are given below.
DISCRETE-TIME MATHEMATICAL MODELS FOR THE CORRECTED MEASUREMENT
(INPUT) VALUES (SIGNALS)
Corrected vehicle speed CV.sub.k,k-1 as provided by the
proportionality unit 5 is as follows:
where ##EQU30## k=1, 2, 3, . . .
The following here applies:
CV.sub.k,k -1 =corrected vehicle speeds during the Kalman interval
(t.sub.k =kT.sub.KA, t.sub.k-1 =(k-1)T.sub.KA), i.e. within a range
of kqT.sub.KO >t.sub.1 >(k-1)qT.sub.KO and l=(k-1)q+1, . . .
, kq, respectively;
VM.sub.k,k-1 =measured plausible vehicle speeds during the Kalman
interval (t.sub.k =kT.sub.KA, t.sub.k-1 =(k-1)T.sub.KA), i.e.
within a range of kqT.sub.KO >t.sub.l >(k-1)qT.sub.KO and
l=(k-1)q+1, . . . , Kq, respectively;
VM.sub.k-1 =measured plausible vehicle speed at time t.sub.k-1
=(k-1)T.sub.KA ;
C(V).sub.k-1 =correction value for the measured plausible vehicle
speed at time t.sub.k-1 =(k-1)T.sub.KA ;
DF(V).sub.k-1 =deterministic speed error at time t.sub.k-1
=(k-1)T.sub.KA ;
.chi.k,k-1=proportionality factor for the vehicle speed correction
value C(V).sub.k-1 during the Kalman interval (t.sub.k =kT.sub.KA,
t.sub.k-1 =(k-1)T.sub.KA), i.e. within a range of kqT.sub.KO
>t.sub.l >(k-1)qT.sub.KO and l=(k-1)q+1, . . . , kq,
respectively;
=constant speed value dependent upon the selected velocity
sensor.
Corrected course angle C.theta..sub.k,k-1 :
k=1, 2, 3, . . .
wherein
C.theta..sub.k,k-1 =corrected course angle during the Kalman
interval (t.sub.k =kT.sub.KA, t.sub.k-1 =(k-1)T.sub.KA), i.e.
within a range of kqT.sub.KO >t.sub.l >(k-1)qT.sub.KO and
l=(k-1)q+1, . . . , kq, respectively;
.theta.M.sub.k,k-1 =measured plausible course angle during the
Kalman interval (t.sub.k =kT.sub.KA, t.sub.k-1 =(k-1)T.sub.KA),
i.e. within a range of kqT.sub.KO >t.sub.l >(k-1)qT.sub.KO
and l=(k-1)q+1, . . . , kq, respectively;
C(.theta.).sub.k-1 =correction value for the measured plausible
course angle at time t.sub.k-1 =(k-1)T.sub.KA ;
DF(.theta.).sub.k-1 =deterministic course angle error at time
t.sub.k-1 =(k-1)T.sub.KA.
Vehicle position (position fix):
Position fixes themselves are not corrected.
Corrected, discrete-time base navigation system.fwdarw.corrected,
discrete-time system equations:
Corresponding to Equations (7a) and (7b), Equations (49) through
(51) here yield
where
CVN.sub.k,k-1 and CVE.sub.k,k-1 are corrected vehicle speeds in the
North (N) and East (E) directions, respectively, during the Kalman
interval (t.sub.k =kT.sub.KA, t.sub.k-1 =(k-1)T.sub.KA), i.e.
within a range of kqT.sub.KO >t.sub.l >(k-1)qT.sub.KO and
l=(k-1)q+1, . . . , kq, respectively.
Corrected, discrete-time measurement (bearing) equations
Analogously to Equations (8a) and (8b), a comparison of the
position bearing data (RNS.sub.l ; RES.sub.l) with the corrected
vehicle position (CRN.sub.l ; CRE.sub.l) to be calculated by means
of the dead reckoning calculation shown below, here results in
##EQU31## i=1, 2, 3, . . . CZN.sub.l and CZE.sub.l are "corrected"
position differences in the north (N) and east (E) directions,
respectively, at time t.sub.l =lT.sub.KO.
The dead reckoning calculation in the corrected navigation system
can again be effected according to the above-described two
methods.
METHOD 1
Rectangular integration according to Equations (40) and (40b)
The dead reckoning position at time t.sub.l+1 =(l+1)T.sub.KO, using
Equations (52a) and (52b) is as follows: ##EQU32## i=1, 2, 3, . .
.
METHOD 2
Trapezoidal integration according to Equations (41a) and (41b)
Here one obtains, at time t.sub.l+1 =(l+1)T.sub.KO, using Equations
(52a) and (52b): ##EQU33## i=1, 2, 3, . . .
For this, the following starting conditions must be suitably
given:
The position correction values C(RN).sub.l and C(RE).sub.l,
l=l.sub.i =Ti/T.sub.KO =k.sub.i q; i=1, 2, 3, . . . in Equations
(54) and (55) are calculated in the same manner as correction
values C(V).sub.k and C(.theta.).sub.k, (where k=1, 2, 3, . . . )
by means of the modified discrete-time Kalman filter as formulated
below.
Modified discrete-time Kalman filter algorithms (simultaneous
bearings processing) for the corrected navigation system
After setting up the error equations for the corrected navigation
system by use of the error propagation theorem and subsequently
setting up the error models, the space state representations of the
discrete-time system and measurement (bearing) error equations are
effected according to the procedures for the uncorrected case.
These equations constitute the prerequisite for use of the modified
discrete-time Kalman filter as formulated below for the corrected
navigation system.
Recursive prediction (extrapolation) algorithm for an a priori
system error estimate
Corrected a priori estimating error covariance matrix CP.sub.k * at
time t.sub.k =kT.sub.KA : ##EQU34## where CP.sub.0 =P.sub.0 is
suitably given according to Equation (44). ##EQU35## where
##EQU36## l=1, 2, 3, . . . ; I=unit matrix;
CA(t.sub.l)=corrected system matrix at time t.sub.l =lT.sub.KO
CD(t.sub.k)=corrected system noise input matrix at time t.sub.k
=kT.sub.KA.
with
C.theta..sub.l according to Equation (51),
CVN.sub.l according to Equation (52a),
VCE.sub.l according to Equation (52b), and
Q according to Equation (25).
Recursive algorithms for the correction of the a priori system
error estimate by way of measurements (position fixes):
Corrected amplification matrix CB.sub.k at time t.sub.k =kT.sub.KA
: ##EQU37## i=1, 2, 3, . . . k=1, 2, 3, . . .
with
M according to Equation (24) and
V according to Equation (39).
Correction value vector C.sub.k at time t.sub.k =kT.sub.KA :
##EQU38## i=1, 2, 3, . . . k=1, 2, 3, . . .
where C.sub.k =O: starting conditions
with the limit conditions: ##EQU39## and where ##EQU40## and
with
CZN.sub.k according to Equation (53a)
CZE.sub.k according to Equation (63b).
The finally obtained "optimum" course and velocity correction
values then are:
Corrected a posteriori estimation error covariance matrix CP.sub.k
at time t.sub.k =kT.sub.KA :
with ##EQU41## i=1, 2, 3, . . . k=1, 2, 3, . . .
For the more general use according to FIG. 1, where, quasi
simultaneously, a plurality of vehicle navigation data for
bearings, e.g. position and/or course angle and/or velocity values
from radio and/or satellite navigation systems, are available, the
changes or additions resulting therefrom will be given below in
model forming and Kalman filter algorithms.
The individual bearing values are now modelled as follows (instead
of according to Equations (5), (6)):
Position measurement data checked for plausibility for position
bearings RNS.sup.(jP) (t), RES.sup.(jP) (t):
where
RN(t) and RE(t) are error-free vehicle positions in the north (N)
and (E) directions, respectively;
.DELTA.RNS.sup.(jP) (t) and .DELTA.ES.sup.(jP) (t) are the
jP.sup.th position measurement (bearing) errors in the north (N)
and east (E) directions, respectively; and
jP=1, . . . NP is the number of quasi simultaneously available
position bearing data.
Course angle measurement data checked for plausibility for course
angle bearings .theta.S.sup.(J.theta.) (t):
where
.theta.S(t) is the error-free course angle;
.DELTA..theta.S.sup.(j.theta.) (t) is the j.theta..sup.th course
angle measurement (bearing) error; and
j.theta.=1, . . . , N.theta. is the number of quasi simultaneously
available course angle bearing data.
Velocity measurement data checked for plausibility for velocity
bearings VS.sup.(jV) (t):
where
V(t) is the error-free vehicle velocity
.DELTA.VS.sup.(jV) (t) is the jV.sup.th velocity (bearing)
measurement error; and
jV=1, . . . , NV is the number of quasi simultaneously available
velocity bearing data.
In deviation from Equations (8a) and (8b), one now obtains the
following continuous-time measurement (bearing) equations:
The position bearing equations result from comparisons of the
positions obtained as a result of dead reckoning (RN.sup.FOA (t),
RE.sup.FOA (t)) with the position bearing data (RNS.sup.(jP) (t),
RES.sup.(jP) (t):
jP=1, . . . , NP
where ZN.sup.(jP) (t) and ZE.sup.(jP) (t) is the jP.sup.th
deviation between the dead reckoning position and the jP.sup.th
bearing position in the north (N) and east (E) directions,
respectively.
The course angle bearing equations are obtained by comparing the
course angle measurement signals (.theta.M(t)) with the course
angle bearing data (.theta.S.sup.(j.theta.) (t):
j.theta.=1, . . . , N.theta.
where Z.theta..sup.(j.theta.) (t) is the j.theta..sup.th difference
between the course angle measurement signal and the j.theta..sup.th
course angle bearing value.
The velocity bearing equations are obtained correspondingly in that
the velocity measurement signals (VM(t)) are compared with the
velocity bearing data (VS.sup.(jV) (t)):
jV=1, . . . , NV
where ZV.sup.(jV) (t) is the jV.sup.th deviation between velocity
measurement signal and jV.sup.th velocity bearing value.
Continuous-time Error Equations
Instead of Equations (10a) and (10b), the use of the error
propagation theorem for Equations (72a) to (74) will provide the
following measurement bearing error equations:
Error due to position bearings:
jP=1, . . . , NP
where .DELTA.ZN.sup.(jP) (t) and .DELTA.ZE.sup.(jP) (t) are the
jP.sup.th position error differences in the north (N) and east (E)
directions, respectively.
Error due to course angle bearings:
j.theta.=1, . . . , N.theta.
where .DELTA.Z.theta..sup.(j.theta.) (t) is the j.theta..sup.th
course angle error difference.
Error due to velocity bearings:
jV=1, . . . , NV
where .DELTA.ZV.sup.(jV) (t) is the jV.sup.th velocity error
difference.
The mathematical model formation for the individual bearing errors
is now effected, in deviation from Equation (13), as follows:
It is assumed that all errors occurring in the bearings can be
described by Gaussian white, i.e. normally distributed, (time)
uncorrelated, noise. The following error models then result:
Position bearing error models (in vector representation):
##EQU42##
This means that the vectors of the position error components
(.DELTA.RNSWR.sup.(jP) (t), .DELTA.RESWR.sup.(jP) (t); jP=1, . . .
, NP) are each developed by stationary white noise with normal (N)
distributions, shown in the abbreviated form by mean vectors O and
the covariance or spectral density matrices VP.sup.(jP) with
individual variances in the north (N) and east (E) directions
(.sigma..sub.N.sup.(jP)).sup.2 and
(.sigma..sub.E.sup.(jP)).sup.2.
Course angle bearing error models:
where ##EQU43## is the abbreviated form for the course angle
bearing error simulation (.DELTA..theta.SWR.sup.(j.theta.) (t);
j.theta.=1, . . . , N.theta.) as stationary white noise with normal
(N) distributions, O mean values and spectral power densities or
variances (.sigma..sub..theta.S.sup.(j.theta.)).sup.2,
respectively.
Velocity bearing error models:
where ##EQU44## is the abbreviated form for the velocity bearing
error simulation (.DELTA.VSWR.sup.(jV) (t); jV=1, . . . , NV) as
stationary white noise with normal (N) distributions, O mean values
and spectral power densities or variances
(.sigma..sub.VS.sup.(jV)).sup.2, respectively.
Furthermore, in this connection, assumptions are being made that
the errors .DELTA..theta..sub.3 (t) and
.DELTA..theta.SWR.sup.(j.theta.) (t); j.theta.=1, . . . , N.theta.,
as well as the errors .DELTA.V.sub.2 (t) and .DELTA.VSWR.sup.(jV)
(t); jV=1, . . . , NV are uncorrelated with one another.
By using Equations (78) to (80) in Equations (75) to (77), the
following continuous-time measurement (bearing) error equation
system is obtained instead of Equations (15a) and (15b):
##EQU45##
For the space state representation of the continuous-time
measurement (bearing) error equations (Equations (81) to (83))
according to Equation (21), the corresponding vectors and matrices
(Equations (18), (19), (24) and (26)) must be newly defined. The
following determinations are favorable for microcomputer
realization:
Measurement (bearing) vector (instead of Equation (18)):
where ##EQU46##
Measurement (bearing) matrix (instead of Equation (24)):
where ##EQU47##
Measurement (bearing) noise vector (instead of Equation (19)):
where ##EQU48##
Measurement (bearing) noise input matrix (new):
where ##EQU49##
Measurement (bearing) error equation (analogous to Equation
(21)):
Combination of Equations (84) to (87) and (16) provides:
where V(t)=S.multidot.V(t) according to Equations (86) and
(87).
Measurement (bearing) noise matrix (instead of Equation (26)):
By using Equations (78) to (80), (11) and (12) as well as (86) and
(87), the following results: ##EQU50## with ##EQU51## where
and ##EQU52##
The conversion of the continuous-time system and measurement
(bearing) error (differential) equations according to Equations
(20) and (88) to discrete-time difference equations is effected,
even with the quasi simultaneous availability of a plurality of
navigation data for bearings, by means of the formalisms of
Equations (28) to (39). Here again it is assumed that all bearings
are taken exclusively at discrete points in time t=T.sub.i ; 1, 2,
3, . . . In this way, the continuous-time measurement (bearing)
error equation (88) changes to ##EQU53## where i=1, 2, 3, . . .
k=1, 2, 3, . . . according to Equation (28)
V.sub.k =S.multidot.V.sub.k according to Equation (88)
M is according to Equation (85);
and Equations (89) and (90) apply for the discrete-time measurement
(bearing) noise matrix.
The discrete-time dead reckoning calculation according to Equations
(40) and (41), respectively, which employs the actual velocity and
course informations from the velocity sensor and the direction
sensor remains just as uninfluenced from the quasi simultaneous
multiple bearings.
DISCRETE-TIME KALMAN FILTER ALGORITHMS
Instead of a discrete-time Kalman filter with simultaneous
measurement (bearing) data processing employed heretofore, it is
here possible to use (and thus save computer time) an algorithm
with sequential measuring (bearing) data processing.
Starting from the recursive prediction (extrapolation) equations
for the a priori system error estimate according to Equations (42)
to (44), one now obtains, in deviation from Equations (45) to (48),
the following algorithms for correction of the a priori systmem
error estimate by measurements (bearings):
Amplification matrices B.sub.k.sup.(j) at time t.sub.k =kT.sub.KA :
##EQU54## where
A posteriori estimation error .DELTA.X.sub.k.sup.(j+l) at time
t.sub.k =kT.sub.KA : ##EQU55##
A posteriori estimation error covariance matrices P.sub.k.sup.(j+l)
at time t.sub.k =kT.sub.KA : ##EQU56## where
V.sup.(j) =S.sup.(j) .multidot.V.sup.(j)
.multidot.(S.sup.(j)).sup.T according to Equation (89);
i=1, 2, 3, . . . ,
k=1, 2, 3, . . . , according to Equation (23);
j=(jP), (j.theta.), (jV)=1, . . . , p;
jP=1, . . . , NP;
j.theta.=1, . . . , N.theta.
jV=1, . . . , NV;
P=(NP+N.theta.+NV);
M.sup.(j) is according to Equation (85);
S.sup.(j) is according to Equation (87);
V.sup.(j) is according to Equation (90);
Z.sub.k.sup.(j=jP) =ZP.sub.k.sup.(jP) =(ZN.sub.k.sup.(jP),
ZE.sub.k.sup.(jP)).sup.T according to Equations (72a) and
(72b);
Z.sub.k.sup.(j=j.theta.) =Z.theta..sub.k.sup.(j.theta.)
=Z.theta..sub.k.sup.(j.theta.) according to Equation (73);
Z.sub.k.sup.(j=jV) =ZV.sub.k.sup.(jV) =ZV.sub.k.sup.(jV) according
to Equation (74);
and the
Marginal conditions
P.sub.k.sup.(j=1)=P.sub.k * according to Equation (43);
.DELTA.X.sub.k.sup.(j=1) =.DELTA.X.sub.k * according to Equation
(42);
With the modified algorithms for the corrected navigation system,
the following changes and additions, respectively, result, on the
basis of the multiple bearings:
In deviation from Equations (53a) and (53b), one here obtains,
analogously to the procedure with Equations (72a) to (74), the
corrected discrete-time measurement (bearing) equations.
Corrected discrete-time position bearing equations:
with ##EQU57## and ##EQU58## and jP=1, . . . , NP,
P=1,2,3, . . .
i=1,2,3, . . .
CZN.sub.1.sup.(jP) =jp.sup.th deviation between corrected dead
reckoning
CZE.sub.1.sup.(jP) =position and jp.sup.th bearing position in the
north (N) and east (E) directions at time t.sub.1 =1T.sub.KO.
CRN.sub.1.sup.FOA =are according to Equations (54a) and (54b)
or
CRE.sub.1.sup.FOA =(55a) and (55b), respectively.
Corrected discrete-time course angle bearing equations:
with ##EQU59## and ##EQU60## as well as
CZ.theta..sub.1.sup.(j.theta.) is the j.theta..sup.th difference
between corrected course angle measurement signal and the
j.theta..sup.th course angle bearing value at time t.sub.1
=lT.sub.KO ; and
C.theta..sub.1 is according to Equation (51).
Corrected discrete-time velocity bearing equations:
with ##EQU61## ps and as well as
CZV.sub.1.sup.(jV), which is the jV.sup.th deviation between
corrected velocity measurement signal and the jV.sup.th velocity
bearing value at time t.sub.1 =1T.sub.KO ; and
CV.sub.1 is according to Equation (49).
Dead reckoning in the corrected navigation system is again
performed according to Equations (54a) and (54b) or Equations (55a)
and (55b), respectively.
The modified discrete-time Kalman filter algorithms will be given
below for the corrected naviagation system with the sequential
measurement (bearing) data processing appropriate here.
The basis for this is the recursive prediction (extrapolation)
algorithm for the a priori system error estimate according to
Equations (56) to (58).
Instead of Equations (59) to (68), the following relationships are
now obtained as recursive modified algorithms for correction of the
a priori system error estimate by the various measurements
(bearings).
Corrected amplification matrices CB.sub.k.sup.(j) at time t.sub.k
=kT.sub.KA : ##EQU62## where
Corrected a posteriori estimation errors y.sub.k.sup.(j+1) at time
t.sub.k =kT.sub.KA : ##EQU63##
Corrected a posteriori estimation error covariance matrices
CP.sub.k.sup.(j+1) at time t=kT.sub.KA : ##EQU64## with
V.sup.(j) =S.sup.(j) .multidot.V.sup.(j) .multidot.S.sup.(j)
according to Equation (89);
i=1, 2, 3, . . . ) according to Equation (28);
k=1, 2, 3, . . .
j=(jP), (j.theta.), (jV)=1, . . . , p;
jP=1, . . . , NP;
j.theta.=1, . . . , N.theta.;
jV=1, . . . , NV;
p=(NP+N.theta.+NV);
M.sup.(j) is according to Equation (85);
S.sup.(j) is according to Equation (87);
V.sup.(j) is according to Equation (90);
CZ.sub.k.sup.(j=jP) is according to Equation (96);
CZ.sub.k.sup.(j=j.theta.) is according to Equation (97);
CZ.sub.k.sup.(j=jV) is according to Equation (98); and the
Marginal conditions: CP.sub.k.sup.(j=1)=CP.sub.k * according to
Equation (56) ##EQU65##
Here, Equation (105) now defines the correction value vector at
time t.sub.k =k.multidot.T.sub.KA with the definitions according to
Equations (61) to (63) as well as Equations (65) and (66).
It will be understood that the above description of the present
invention is susceptible to various modifications, changes and
adaptations, and the same are intended to be comprehended within
the meaning and range of equivalents of the appended claims.
* * * * *