schmidt1966

Application of State-Space
Methods to Navigation Problems
S T A N L E Y F. S C H M I D T
Western Development
Laboratories
California
Philco Corporationy Palo Alto,
I.
Introduction
293
11.
Examples of N o m i n a l Trajectories
A . P o w e r e d - F l i g h t Trajectory
B. Translunar Trajectory
C. Earth-Entry Trajectory
294
295
295
296
III.
Equations of M o t i o n and N o t a t i o n
296
IV.
Guidance L a w s
A. A Tutorial Example
B. A n Interplanetary E x a m p l e
C. T h e Question of Linearity
299
299
307
313
Error Analysis
A. Definitions and N o t a t i o n
B. Propagation of Errors in Linear S y s t e m s
314
314
316
Determination of State
A . Derivative of V e c t o r Quantities
B. Least-Squares Fit to a Polynomial
C. Derivation of Kalman's Filter
318
319
320
322
Parameter Estimation
331
Effects of U n k n o w n Parameters
333
References
340
V.
VI.
VII.
I. Introduction
T h e d e s i g n of a n a v i g a t i o n s y s t e m is a p r o b l e m i n w h i c h t h e u s e of
a d v a n c e d t e c h n i q u e s is a l m o s t a n e c e s s i t y f o r d e r i v i n g s u i t a b l e g u i d a n c e
l a w s a n d u n d e r s t a n d i n g f u n d a m e n t a l b e h a v i o r . T h i s is a r e s u l t of t h e
f a c t t h a t t h e d e s c r i p t i o n of t h e p r o b l e m r e q u i r e s a l a r g e n u m b e r of
s t a t e s ; f o r e x a m p l e , t h r e e c o m p o n e n t s e a c h of l i n e a r p o s i t i o n a n d v e l o c i t y
a r e r e q u i r e d t o d e s c r i b e t h e m o t i o n of t h e c e n t e r of m a s s , t h r e e c o m p o n e n t s e a c h of a n g u l a r p o s i t i o n a n d v e l o c i t y t o d e s c r i b e r o t a t i o n s a b o u t
t h e m a s s c e n t e r . T h u s t o g a i n a n y u n d e r s t a n d i n g a n d f o r s i m p l i c i t y of
d e s c r i p t i o n , w e are forced t o u s e m a t r i x m e t h o d s for d e s c r i b i n g t h e
system.
293
294
STANLEY F. SCHMIDT
T h e p u r p o s e of t h i s c h a p t e r is t o p r o v i d e t h e r e a d e r w i t h m a t h e m a t i c a l
examples
and
concepts
p r o b l e m s associated
which
with
have
proved
navigation
very useful
systems. T h e
in
main
resolving
part
of
the
d o c u m e n t a s s u m e s k n o w l e d g e of l i n e a r d i f f e r e n t i a l e q u a t i o n s a n d t h e i r
solution using state-space methods.
T h e n a v i g a t i o n of a v e h i c l e c a n b e s u b d i v i d e d i n t o t h e f o l l o w i n g
five
tasks:
( 1 ) Navigation
The
instrumentation.
measurement
of
observables
w h i c h a r e r e l a t e d t o t h e s t a t e of t h e s y s t e m s . E x a m p l e s a r e r a d a r m e a s u r e ments, optical sighting on stars, inertial m e a s u r e m e n t s , etc.
( 2 ) Determination
of
This
state.
is
defined
as
the
mathematical
p r o b l e m of c a l c u l a t i n g t h e s t a t e of t h e v e h i c l e ( p o s i t i o n , v e l o c i t y , e t c . )
from t h e m e a s u r e m e n t s . T h i s p r o b l e m can involve relatively
complex
t e c h n i q u e s of " s m o o t h i n g " i n s o m e i n s t a n c e s .
( 3 ) Prediction
of
future
state
from
present
state.
The
successful
c o m p l e t i o n of t a s k s ( 1 ) , ( 2 ) , a n d ( 3 ) a l l o w s o n e t o d e t e r m i n e if a n y c o n t r o l
a c t u a t i o n is r e q u i r e d . T h a t i s , if f u t u r e s t a t e e q u a l s d e s i r e d f u t u r e s t a t e ,
n o a d d e d c o n t r o l is r e q u i r e d .
( 4 ) Application
of the guidance
law.
The
g u i d a n c e l a w is u s e d
to
calculate the control action required to make the future state the desired
state.
( 5 ) Application
of control
action
in accordance
with
the guidance
law.
T h i s task is, for e x a m p l e , a c h a n g e i n r u d d e r p o s i t i o n t o e s t a b l i s h a n e w
h e a d i n g f o r s h i p s ; t h e b a n k i n g a n d t u r n i n g of a n a i r c r a f t t o
change
h e a d i n g ; t h e a p p l i c a t i o n of a p r o p u l s i v e s y s t e m t o m o d i f y t h e d i r e c t i o n
of a s p a c e c r a f t ,
etc.
W e s h a l l b e c o n c e r n e d w i t h t h e s o l u t i o n of t h e s e c o n d , t h i r d , a n d f o u r t h
t a s k s . T h e p r e s e n t a t i o n w i l l first s h o w h o w t h e s t a t e - s p a c e c o n c e p t of
transition from
one state to another, w h e n applied to the
variational
equations along a nominal trajectory, allows one to derive
prediction
e q u a t i o n s a n d n o m i n a l g u i d a n c e l a w s . S e c o n d , t h e p r o b l e m of d e t e r m i n a t i o n of s t a t e b y s m o o t h i n g a l a r g e n u m b e r
of o b s e r v a t i o n s w i l l b e
covered.
I I . Examples of N o m i n a l Trajectories
A
nominal trajectory
is d e f i n e d
as a s o l u t i o n
of t h e e q u a t i o n s
of
m o t i o n b e t w e e n p o i n t s A a n d Β of F i g . 1 i n w h i c h all m i s s i o n c o n s t r a i n t s
a r e s a t i s f i e d . E x a m p l e s of n o m i n a l t r a j e c t o r i e s w i l l n o w b e g i v e n .
NAVIGATION
FIG.
1.
295
PROBLEMS
Nominal trajectory.
A. Powered-Flight Trajectory
T h i s is a t r a j e c t o r y s i m u l a t i n g t h e
t h e l a u n c h p a d at C a p e K e n n e d y a n d
to a circular orbit (approximately
" n o m i n a l " trajectory m u s t satisfy
such as:
Saturn booster, which starts from
arrives at c o n d i t i o n s c o r r e s p o n d i n g
8 km/sec) about the earth. T h i s
various intermediate constraints,
(1) S t r u c t u r a l l o a d i n g w i t h i n d e s i g n r e g i o n .
(2) T e m p e r a t u r e s w i t h i n d e s i g n r e g i o n .
(3) T r a j e c t o r y
within
range
safety
boundaries
at
the
Cape
and
beyond.
T h i s e x a m p l e is t y p i c a l of n o m i n a l t r a j e c t o r i e s f o r b o o s t e r s .
B. Translunar Trajectory
T h i s is a t r a j e c t o r y w h i c h s t a r t s f r o m a c i r c u l a r o r b i t ( a b o u t t h e e a r t h ) ,
a c c e l e r a t e s t o n e a r - p a r a b o l i c v e l o c i t y ( a p p r o x i m a t e l y 11 k m / s e c a t a n
a l t i t u d e of 1 8 0 k m a t t h e e a r t h ) , a n d c o a s t s (freefall m o t i o n ) t o t h e m o o n ,
a r r i v i n g i n s u c h a m a n n e r t h a t a m i n i m u m of r e t r o - m a n e u v e r is r e q u i r e d
to soft-land at s o m e crater o n t h e m o o n . C o n s t r a i n t s w h i c h s u c h a
n o m i n a l trajectory m u s t satisfy m a y i n c l u d e :
(1) O r b i t at e a r t h m u s t b e o n e w h i c h c a n b e a t t a i n e d u n d e r
such
powered-flight constraints as in S e c t i o n I I , A.
(2) T r a j e c t o r y m u s t p a s s over c e r t a i n t r a c k i n g s t a t i o n s d u r i n g e a r t h
orbit and acceleration phase.
(3) F l i g h t t i m e m a y b e r e q u i r e d t o b e w i t h i n c e r t a i n limits c a u s e d b y
payload or tracking considerations.
( 4 ) C e r t a i n l i g h t i n g c o n d i t i o n s o n t h e m o o n a t t h e t i m e of a r r i v a l
m a y b e d e s i r e d . ( T h i s c o n s t r a i n s t i m e of m o n t h f o r l a u n c h i n g . ) T h i s
e x a m p l e is t y p i c a l of t r a j e c t o r i e s t o i n t e r p l a n e t a r y t a r g e t s a l s o , if o n e
recognizes that s o m e w h a t - h i g h e r earth-injection vélocités are required.
T h e v e l o c i t y r e q u i r e m e n t s d e p e n d o n t h e t a r g e t , flight t i m e , a n d d a t e of
launch.
296
STANLEY F. SCHMIDT
C. Earth-Entry Trajectory
T h i s is a t r a j e c t o r y w h i c h s t a r t s a t a t m o s p h e r e e n t r y a n d l a n d s a t s o m e
l o c a t i o n o n t h e e a r t h . C o n s t r a i n t s for s u c h trajectories m a y i n c l u d e :
(1) A e r o d y n a m i c h e a t i n g m u s t b e k e p t b e l o w c e r t a i n d e s i g n
bound-
aries.
(2) D e c e l e r a t i o n
must
be kept within some structural
(or
human)
tolerances.
These
considerations
place constraints o n t h e initial c o n d i t i o n s
entry which m u s t be m e t by the approach trajectory to the earth.
at
The
d e t e r m i n a t i o n of t h e a l l o w a b l e v a r i a t i o n i n i n i t i a l c o n d i t i o n s w h i c h w i l l
p e r m i t a safe l a n d i n g w h i l e m e e t i n g t h e o t h e r c o n s t r a i n t s w o u l d b e p a r t
of a r e - e n t r y t r a j e c t o r y
T h e p r o b l e m of
study.
finding
n o m i n a l t r a j e c t o r i e s is b y n o m e a n s a s i m p l e
p r o b l e m . I t is a p r o b l e m w h e r e c o n c e p t s of s t a t e a n d s t a t e
allow
one
to
derive
digital-computer
programs
seek out t h e allowable solutions. I n t h e
which
material
which
transition
automatically
follows,
we
assume that this p r o b l e m has been solved.
I I I . Equations of Motion and N o t a t i o n
Prior
to
discussing
the
guidance
and
prediction
problems,
it
d e s i r a b l e t o r e v i e w t h e e q u a t i o n s of m o t i o n i n t h e f o r m i n w h i c h
is
we
shall b e treating t h e m . T h e n o m i n a l trajectories to w h i c h w e shall b e
r e f e r r i n g s a t i s f y a s e t of n o n l i n e a r v e c t o r d i f f e r e n t i a l e q u a t i o n s s u c h a s
*=/(*,u,o
(l)
T h e capital letter X will b e u s e d t o define a v e c t o r w h i c h r e p r e s e n t s t h e
s t a t e o f t h e n o m i n a l t r a j e c t o r y . C a p i t a l U is a v e c t o r w h i c h d e f i n e s a n y
control m o t i o n (forcing function) associated with this nominal trajectory.
T h e i n d e p e n d e n t v a r i a b l e is t, w h i c h is g e n e r a l l y t a k e n a s t i m e .
The
quantities w e are interested in observing or controlling are related
to
the state by
Y(t)
E x a m p l e s of Y(t)
could be the range, azimuth, elevation, range
e t c . , of a s p a c e v e h i c l e a s o b s e r v e d
represent
(2)
= G(X,t)
by a tracking station. It
altitude, range to target, or m a n y
other
rate,
might
quantities we
are
required to control to complete the navigation mission.
T h e s o l u t i o n e x p r e s s i n g Y(t)
i n c l o s e d f o r m is n o t g e n e r a l l y o b t a i n a b l e .
297
NAVIGATION PROBLEMS
I n t h e c a s e s of i n t e r e s t , o n e m a y c o m p u t e X(t)
a n d Y(t)
g i v e n X0
, the
i n i t i a l s t a t e , a n d U ( i ) , t h e n o m i n a l f o r c i n g f u n c t i o n , b y u s e of d i g i t a l ( o r
a n a l o g ) c o m p u t e r s . C o n s i d e r a t r a j e c t o r y X(t)y
s h o w n in Fig. 2.
The
X(t)Χ · + Χ · R E P R E S E N T S THE INITIAL STATE
y
X(t)-^
^
X(t) + X(t) R E P R E S E N T S THE STATE AT TIME
t
U ( t ) + U ( t ) R E P R E S E N T S THE CONTROL
NOMINAL TRAJECTORY
X(t)
P E R T U R B E D TRAJECTORY X ( t ) + X ( t )
FIG.
2.
N o m i n a l and perturbed trajectory. ( U in figure is equivalent to U in text.)
s m a l l l e t t e r s x0 , x(t)> a n d u(t)
trajectory.
To
find
the
represent
differential
deviations from
equations
the
relating the
nominal
deviations,
w e e x p a n d t h e n o n l i n e a r v e c t o r p r e s e n t e d in (1),
χ + χ = f(X
+ χ9
υ+
M,
(3)
t)
in a Taylor-series expansion:
0/ 1 - ,
and retain only the
first-order
Γ 0/
(4)
t e r m s of t h e s e r i e s . S u b t r a c t i n g ( 1 ) f r o m
(4) gives
*-[$•]*+[•&•
* = F(t)x
<>
5
+ B(t)u
(6)
F o r η s t a t e s a n d / c o n t r o l s,
/ '\
f
/
Λ \
W
1·
Γ
δ
/ι
Γ
.
dxx
dX
dxx
dxn
δ
/ι
4
.
=
J
B(t)
298
STANLEY F.
SCHMIDT
Similarly, e x p a n d i n g (2),
Y(t) + A*)
= G(X
+
x, t)
« G(X,
t) +
*(i)
(7)
a n d s u b t r a c t i n g (2) yields
y{t) =
brl
x{t)= H{t)x{t)
(8)
T h e m a t r i c e s F(t), B(t), a n d H(t) a r e g e n e r a l l y t i m e - v a r y i n g , s i n c e t h e
n o m i n a l t r a j e c t o r y X(t) is a f u n c t i o n of t i m e .
E q u a t i o n ( 6 ) is c o m m o n l y r e f e r r e d t o a s t h e p e r t u r b a t i o n o r v a r i a t i o n a l
differential e q u a t i o n associated w i t h t h e n o n l i n e a r v e c t o r e q u a t i o n (1).
T h e s o l u t i o n of ( 6 ) m a y ( f o r c o n s t a n t - c o n t r o l i n c r e m e n t s o v e r t h e i n t e r v a l
h ^ t < t2) b e e x p r e s s e d
*(**) =
; hWi) + W% ; hXh)
y(h) = muWt)
(9)
w h e r e φ is t h e t r a n s i t i o n m a t r i x of s e n s i t i v i t y c o e f f i c i e n t s r e l a t i n g t h e
d e v i a t i o n s t a t e x(t2) a t t i m e t2 t o t h e d e v i a t i o n s t a t e x(t^) a t t i m e t x , a n d U
is a m a t r i x of s e n s i t i v i t y c o e f f i c i e n t s r e l a t i n g a u n i t v a r i a t i o n of w ( ^ ) i n
t h e t i m e i n t e r v a l tx ^ t ^ t2 t o t h e d e v i a t i o n s t a t e x(t2) a t t i m e t2 .
T h e s e sensitivity coefficients are q u i t e useful for g u i d a n c e laws. T h e y
a l s o h a v e a g r e a t d e a l of u s e f u l n e s s i n e r r o r a n a l y s i s . E r r o r a n a l y s i s is
t h e c a l c u l a t i o n of t h e e r r o r s i n c o m p l e t i o n of s o m e o b j e c t i v e c a u s e d
b y c o m p o n e n t errors. T h e c o m p o n e n t s are devices s u c h as gyros,
accelerometers, a n d so on, w h i c h are u s e d to sense a n d correct t h e
t r a j e c t o r y t o m e e t t h e d e s i r e d o b j e c t i v e . T h e t h i r d i m p o r t a n t u s e is i n
t h e field of t r a j e c t o r y d e t e r m i n a t i o n . T h e
trajectory-determination
p r o b l e m is o n e of finding t h e b e s t e s t i m a t e of t h e s t a t e b a s e d u p o n
observations which are related to the state. T h e observations are generally
corrupted by errors and, therefore, m u s t be weighted in s o m e fashion
t o o b t a i n a " s m o o t h e d ' ' t r a j e c t o r y . A f o u r t h u s e of s e n s i t i v i t y c o e f f i c i e n t s
is i n t h e field of o p t i m i z a t i o n of t r a j e c t o r i e s , t h a t i s , i n t h e s e a r c h f o r
" n o m i n a l " trajectories which meet the e n d objective subject to certain
intermediate constraints on control or state or b o t h while maximizing or
m i n i m i z i n g s o m e payoff f u n c t i o n (for e x a m p l e , m a x i m i z i n g t h e p a y l o a d ) .
A s s o c i a t e d w i t h t h e v a r i a t i o n a l e q u a t i o n s (6) a r e t h e a d j o i n t e q u a t i o n s
λ(ί) =
-FT(t)X(t)
(10)
λ(<) = φ(ί; < 0)λ(< 0)
(")
w h o s e s o l u t i o n is
where
W ; Ό) =
[*-'('; Ό ) ]
Γ
Γ
= * (Ό; 0
(12)
NAVIGATION
PROBLEMS
299
A p r o p e r t y of t h e a d j o i n t w h i c h is q u i t e u s e f u l is t h a t t h e s o l u t i o n f o r
φ(Τ
— t; T) b y i n t e g r a t i o n of
^ = -FM
φ(Τ; Τ) = I
(13)
in negative time from the e n d - t i m e point Γ, yields the transition matrix
φ(Τ; Τ — t) b y t h e s i m p l e o p e r a t i o n of t r a n s p o s i n g t h e r e s u l t . T h i s m e a n s
t h a t w e m a y o b t a i n all t h e s e n s i t i v i t y c o e f f i c i e n t s ( t r a n s i t i o n m a t r i x )
r e l a t i n g d e v i a t i o n s x(t) t o e n d - p o i n t d e v i a t i o n s x(T) b y o n e c a l c u l a t i o n
on a digital c o m p u t e r .
I V . Guidance Laws
T o u n d e r s t a n d t h e p r i n c i p l e s i n v o l v e d i n u s i n g t h e s e n s i t i v i t y coefficients t o d e r i v e g u i d a n c e laws, a s i m p l e e x a m p l e will b e c a r r i e d t h r o u g h
in detail.
A. A Tutorial Example
A s s u m e that a m a s s has b e e n accelerated o n a frictionless surface a n d
is d i r e c t e d t o w a r d a m o v i n g t a r g e t s o m e d i s t a n c e a w a y ( F i g . 3 ) . T h e
γI
I
INTERCEPT T I M E - Τ I
100 Km
FIG.
3.
G e o m e t r y for example.
t a r g e t is k n o w n t o m o v e a t c o n s t a n t v e l o c i t y (vT) p a r a l l e l t o t h e Y a x i s
a n d a t t0 i n t e r c e p t s t h e X a x i s a t X = 1 0 0 k m . T h e m i s s i o n o b j e c t i v e
is t o c o l l i d e w i t h t h e t a r g e t . T h e n o m i n a l t r a j e c t o r y of t h e m a s s is a
s t r a i g h t l i n e a n d is a s s u m e d t o c o l l i d e w i t h t h e t a r g e t a t t i m e T.
W e a s s u m e t h e v e h i c l e is c o n s t r u c t e d w i t h a s m a l l j e t a n d a t t i t u d e control system, so a small velocity correction m a y b e m a d e in a n y
d i r e c t i o n . W e shall a s s u m e also t h a t t h i s little j e t t h r u s t s for s u c h a
300
STANLEY F. SCHMIDT
b r i e f i n t e r v a l t h a t a s t e p c h a n g e i n v e l o c i t y is a g o o d a p p r o x i m a t i o n t o
t h e c h a n g e i n t h e s t a t e of t h e m a s s . O u r p r o b l e m is t o d e t e r m i n e
magnitude
and
direction
the
of t h e s t e p c h a n g e i n v e l o c i t y r e q u i r e d
intercept the target (midcourse
to
maneuver).
E q u a t i o n s of m o t i o n :
X = 0
Ϋ = 0
(14)
D e f i n i n g t h e s t a t e s of t h e s y s t e m a s
X\
— X
yields the following
X2
first-order
— Y
X$ — X
X^ — Y
f o r m f o r t h e e q u a t i o n s of m o t i o n :
(14a)
M a k i n g a series expansion,
0
0
0
0
1 0 ^
0
0;
(15)
S u b t r a c t i n g X = f(X)
from (15) yields t h e variational e q u a t i o n s
0
0
1 0
* = i : i = io ο o \ w^\=
Fx
2
0
0
0
0
T h e adjoint differential e q u a t i o n s are
'
T
-F X
0
0
0
0
0
0
0
0
0
0
0
0
- 1
0
- 1
0
< >
16
301
NAVIGATION PROBLEMS
S o l u t i o n of t h e a d j o i n t e q u a t i o n s f r o m t i m e Τ t o t yields
but
T
(F f
T
=
(F f
so that
φ(ί; T)=I
+
0
0
0
0
0
0
0
0
-1
0
0
0
0
- 1 0
(t-T)
0
(17)
1
φ(ί; Τ)
0
0
0
0
1
ο
T - t
0
1
0
=
1
T - t
0
0
0
T h e t r a n s i t i o n m a t r i x t o t h e e n d p o i n t ( t i m e T)
φ(Τ; t) = φψ,
T)
=
is t h e r e f o r e g i v e n b y
1
0
T - t
0
1
0
0
0
1
0
0
0
0
1
N o w if o u r m e a s u r e m e n t a n d t r a j e c t o r y
0
T - t
(18)
d e t e r m i n a t i o n s y s t e m c a n tell
u s t h e d e v i a t i o n f r o m t h e n o m i n a l t r a j e c t o r y a t a n y t i m e t, w e c a n p r e d i c t
t h e d e v i a t i o n a t t i m e Τ u s i n g t h e t r a n s i t i o n m a t r i x φ(Τ;
x(T)
= φ(Τ;
t):
t)x(t)
(19)
1. G U I D A N C E L A W FOR F I X E D T I M E OF ARRIVAL
A n o b v i o u s w a y of m a k i n g t h e m a s s i m p a c t t h e t a r g e t is t o m a k e t h e
d e v i a t i o n of p o s i t i o n f r o m t h e n o m i n a l at t i m e Τ b e e q u a l t o z e r o :
(T)
Xl
= x2{T)
=
0
(19a)
I f t h e t r a n s i t i o n m a t r i x is p a r t i t i o n e d s o t h a t
pos(T)
(T)
Φ*(Τ; t)
Xl
νβΙ(Γ)
L
φ3(Τ;
**(T)
J
t)
x3(t)
L *«(0
(20)
302
STANLEY F. SCHMIDT
it follows t h a t
I n v e c t o r f o r m , t h e n , t h e e n d - p o i n t p o s i t i o n m i s s w i t h n o c o r r e c t i o n is
χ ( Γ ) = - Μ ί ) + < £ 2* ( ί )
(20b)
W e s e e t h a t <f>2 r e p r e s e n t s t h e s e n s i t i v i t y i n p o s i t i o n a t t i m e Τ d u e t o
v e l o c i t y d e v i a t i o n s a t t i m e t. I f w e c o n s i d e r e d a v e l o c i t y c h a n g e xg(t)y
the resultant position change at Τ w o u l d be
xg(T)
= W)
(21)
where
Letting our vector guidance correction be
, w e can d e t e r m i n e its
c o m p o n e n t s from t h e constraint e q u a t i o n (19a),
x(T) + xa(T)
= 0 = ^ , χ ( ί ) + φΜ)
+ ^ χ β( ί )
(22)
from which
i . ( 0 = -ΦΜΤ)
= - ^ V i x ( i ) - Mi)
(22a)
T h e t r a j e c t o r y b e f o r e a n d a f t e r c o r r e c t i o n is s h o w n i n F i g . 4 . S i n c e t h i s
guidance law p r o d u c e s a n e w trajectory which arrives at t h e target
a t t h e s a m e t i m e a s t h e n o m i n a l , it is k n o w n a s a
fixed-ttme-of-arrival
guidance law.
γ
TIME (T) IMPACT
100 Km
F I G . 4.
Trajectory before and after correction, (x in figure is equivalent to χ in text.)
303
NAVIGATION PROBLEMS
For the simple example problem,
1
0
T - t
0
κ(Τ)
1
T - t
T - t
=
0
(23)
x(t)-x(t)
T - t
I n s p i t e o f t h e c o m p l e x i t y of t h e e q u a t i o n s of m o t i o n i n a r e a l s y s t e m of
e q u a t i o n s for i n t e r p l a n e t a r y
flight,
t h e a b o v e p r o c e d u r e s m a y still b e
u s e d . T h e p r i n c i p a l d i f f e r e n c e is t h a t c l o s e d - f o r m e x p r e s s i o n s
cannot
b e d e r i v e d . All w e h a v e for t h e v a r i o u s m a t r i c e s a r e n u m b e r s o b t a i n e d
from digital-computer
calculations.
F r o m ( 2 3 ) w e c a n o b t a i n t h e m a g n i t u d e of t h e v e l o c i t y
v, = ( V * , )
correction
1 /2
(23a)
a n d t h e d i r e c t i o n (see F i g . 5)
(23b)
γ
FIG.
2.
5.
Φ
»
Velocity correction, ( χ in figure is equivalent to χ in text.)
G U I D A N C E L A W FOR VARIABLE T I M E OF ARRIVAL
A g u i d a n c e l a w w i t h t h e r e s t r i c t i o n of a r r i v i n g a t t h e t a r g e t a t t h e
s a m e t i m e a s t h e n o m i n a l t r a j e c t o r y is n o t t h e o n l y g u i d a n c e l a w p o s s i b l e .
A s a m a t t e r of f a c t , it m a y p l a c e u n d u e r e q u i r e m e n t s o n t h e m a g n i t u d e
of v e l o c i t y c o r r e c t i o n r e q u i r e d t o t h e t a r g e t . W e s h a l l t h e r e f o r e d e t e r m i n e
a l a w i n w h i c h t h e a r r i v a l t i m e is f r e e .
Consider
x
T
C, y
T C
the
motion
of
the
mass
in
target-centered
coordinates
( F i g . 6). I n this reference frame w e see f r o m Fig. 7 t h a t t h e
n o m i n a l trajectory a p p r o a c h e s t h e target at (0, 0) coordinates along a
straight line. P e r t u r b e d
trajectories
would tend to be nearly
parallel
t o t h e n o m i n a l t r a j e c t o r y (for s m a l l p e r t u r b a t i o n s ) a n d t h e r e f o r e w e c a n
c o n s i d e r a l i n e of m i s s n o r m a l t o t h e n o m i n a l a n d t h r o u g h t h e c e n t e r
of t h e t a r g e t . A t t i m e Τ f o r t h e n o m i n a l t r a j e c t o r y t h e m a s s is a t t h e
c e n t e r of t h e t a r g e t . F o r t h e p e r t u r b e d t r a j e c t o r y it m i g h t b e a t t h e p o i n t
304
STANLEY F.
SCHMIDT
C o o r d i n a t e s in t a r g e t - c e n t e r e d
coordinate system:
X
=
TC
=
JTC
=
XTC
=
X t
y - y t
X *t
y - y t
x
c-100
'TC
MASS
TARGET
TC
m
100
FIG.
6.
KM
M o t i o n of mass in target-centered coordinates.
o n t h e d a s h e d line s h o w n in F i g . 7. T h e
fixed-time-of-arrival
system
w o u l d p r e d i c t t h e m i s s (XITC{T),
^ 2 r c ( ^ ) ) - W e can see from t h e
figure
t h a t o u r m i s s of t h e t a r g e t w i t h a v a r i a b l e t i m e of a r r i v a l w o u l d i n g e n e r a l
b e a s m a l l e r v a l u e t h a n f o r t h e fixed t i m e of a r r i v a l a n d i n a d i r e c t i o n
a l o n g t h e u n i t v e c t o r T.
PREDICTED M A S S
POSITION AT T I M E T .
x 1 T( CT ) ,
(-
x 2 T Cai)
*TC
Λ
LINE O F MISS
TRUE
Λ
s a τ
TRAJECTORY
(
NOMINAL
FIG.
7.
TRAJECTORY
M o t i o n in target-centered-coordinates reference frame.
UNIT
VECTORS
NAVIGATION
305
PROBLEMS
Λ
i
FIG.
8.
T h e T, S coordinate system.
A r e a s o n a b l e w a y of h a n d l i n g t h e p r o b l e m t h e n is t o t r a n s f o r m t h e
d e f i n i t i o n of m i s s f r o m t h e (x, y) s y s t e m t o t h e T, S s y s t e m ( F i g . 8 ) .
(t\
/—sin 0
W
cos0\/i\
= ( - « . «
-sin
, _ ,42χ
Jl/)
<>
I f w e l e t t h e c o m p o n e n t of m i s s a l o n g Τ e q u a l Β a n d t h e c o m p o n e n t o f
m i s s a l o n g S e q u a l A, t h e n
,B\
_
/-sin θ
cosö
\A)-\
- c o s θψ(Τ)\
_
,x(T)\
m
)
-sin e)\y(T)) - \y(T))
w h e r e χ a n d y a r e t h e m i s s c o m p o n e n t s i n t h e (x, y) c o o r d i n a t e f r a m e .
For our example,
0
-iilZZi
=
t a n
=
X
-iilÇ
xTc
t a n
w h e r e yTC a n d XYQ a r e t a k e n a s t h e n o m i n a l v a l u e s .
T h e r e f o r e , s u b s t i t u t i n g ( 2 0 b ) for t h e m i s s c o m p o n e n t s yields
Q = miUT; t)x(t)
+ φ2(Τ;
ί)χ(ί)]
S o l v i n g (26) in t h e m a n n e r u s e d in t h e p r e v i o u s section yields a
t i m e - o f - a r r i v a l g u i d a n c e l a w w h i c h is e q u i v a l e n t t o ( 2 2 a ) :
ig
= - [ 7 W 2] - i Q
(26)
fixed-
(27)
( 2 5
306
STANLEY F. SCHMIDT
I t is e v i d e n t t h a t t h e m i s s a l o n g t h e S v e c t o r is r e l a t e d t o t i m e of i m p a c t ,
b e c a u s e , as l o n g as Β = 0, w e shall i m p a c t t h e target. T h u s a g u i d a n c e
law o n e c o u l d u s e w o u l d b e t o c o m p u t e xG w i t h A = 0 ( n o c o r r e c t i o n
along S miss), which
yields
(28)
T h i s w o u l d b e a variable-time-oj-arrival
system in t h e sense t h a t
the
t i m e of a r r i v a l w o u l d b e d i f f e r e n t f r o m t h e n o m i n a l .
S i n c e w e s a i d t h a t w e r e a l l y d o n o t c a r e w h a t t h e t i m e of a r r i v a l i s ,
it w o u l d s e e m w i s e t o c o n s i d e r it a s a f r e e p a r a m e t e r a n d c h o o s e i t s
value so t h a t
is m i n i m i z e d . L e t
(29)
Substituting
(30)
T a k i n g t h e d e r i v a t i v e of ( 3 0 ) w i t h r e s p e c t t o A a n d s e t t i n g t h e r e s u l t a n t
equation equal to zero gives
S o l v i n g for A w e o b t a i n
A = -
2 2
(^11^12 + 021^22)
K
2
+
' "f" ' Β
2 2)
(31)
ö
I f o n e u s e s t h e v a l u e of A f r o m ( 3 1 ) i n ( 3 2 ) , t h e m i n i m u m v a l u e of t h e
g u i d a n c e c o r r e c t i o n is o b t a i n e d :
i, =
- [ Ï W J -
T h i s m i g h t b e c a l l e d a variable-time-of-arrival
midcourse
correction.
1
(32)
Q
guidance
law for
minimum
NAVIGATION
PROBLEMS
W i t h reference to the example, the two guidance laws would
307
give
trajectories (exaggerated) as s h o w n in F i g . 9.
COLLISION AT TIME (T )
X
FIXED T I M E OF ARRIVAL
COLLISION LATER THAN TIME (Tl
COLLISION EARLIER THAN TIME (Tl
VARIABLE TIME OF ARRIVAL
MINIMIZING MIDCOURSE CORRECTION
FIG.
9.
G u i d a n c e - l a w trajectories.
B. An Interplanetary Example
S u p p o s e o u r p r o b l e m w a s to d e r i v e a g u i d a n c e law for m i s s i n g s o m e
planet by a prescribed distance. W e assume that a nominal trajectory
has been found which misses the planet by the desired distance and
a r r i v e s a t t h e r e f e r e n c e p o i n t p e r i a p s i s ( p o i n t of c l o s e s t a p p r o a c h ) a t
t i m e T. T h e n o m i n a l t r a j e c t o r y f o r t h e m o o n a s t h e t a r g e t m i g h t l o o k
a s s h o w n i n F i g . 10.
L e t us say that studies have s h o w n that b y t h e t i m e w e w o u l d have
reached Β (on the nominal trajectory), tracking data from earth-based
tracking stations (e.g., t h e D e e p S p a c e I n s t r u m e n t a t i o n Facility, D S I F )
have been smoothed and have accurately determined the true trajectory
of t h e s p a c e c r a f t . I t is o u r p r o b l e m t o find t h e d i r e c t i o n a n d m a g n i t u d e
of t h e m i d c o u r s e c o r r e c t i o n , s o t h a t t h e t r u e t r a j e c t o r y m i s s e s t h e t a r g e t
( m o o n ) b y t h e s a m e d i s t a n c e as t h e n o m i n a l . A w o r d of c a u t i o n s h o u l d
308
STANLEY F. SCHMIDT
F I G . 10.
N o m i n a l trajectory for the m o o n as the target.
be noted here: J u s t because the true trajectory goes t h r o u g h the nominal
position at t i m e
A
Τ d o e s n o t m e a n it w i l l n e c e s s a r i l y m i s s t h e
tremendous
deviation
from
the
nominal
would
have
to
moon.
occur
b e f o r e t h e s i t u a t i o n s h o w n i n F i g . 11 c o u l d t a k e p l a c e . H o w e v e r ,
p r e v e n t t h e o c c u r r e n c e of s u c h a s i t u a t i o n , w e d e r i v e t h e
to
fixed-time-of-
a r r i v a l g u i d a n c e l a w as s h o w n i n F i g . 12. W e m a k e t h e g u i d a n c e l a w
so t h a t t h e p o s i t i o n of t h e t r u e t r a j e c t o r y a g r e e s w i t h t h e n o m i n a l
s o m e t i m e ( p o i n t D)
b e f o r e t h e m o o n is e n c o u n t e r e d . W e t h e n
a s e c o n d v e l o c i t y c o r r e c t i o n ( a t p o i n t D)
so t h a t t h e velocity
with t h e nominal trajectory.
/
PERTURBED '
TRAJECTORY
F I G . 11.
F I G . 12.
G e o m e t r y for a deviation trajectory.
G e o m e t r y for
fixed-time-of-arrival
guidance law.
at
make
agrees
309
NAVIGATION PROBLEMS
1. EQUATIONS OF M O T I O N
F o r simplicity we write t h e equations
(33)
The
functions /
involve t h e inverse-square
law for t h e n u m b e r
of
b o d i e s w h o s e g r a v i t a t i o n a l a t t r a c t i o n is s i g n i f i c a n t e n o u g h t o affect t h e
t r a j e c t o r y , a s w e l l a s g r a v i t a t i o n a l a n o m a l i e s ( o b l a t e n e s s of e a r t h , e t c . )
a n d possibly external forces (solar pressure, etc.). F o r trajectories
in
the region from t h e earth to moon, studies have indicated that only t h e
a t t r a c t i o n s of t h e e a r t h , m o o n , a n d s u n n e e d b e c o n s i d e r e d . T h e f u n c t i o n s / a r e not given here, since they are complex a n d would only confuse
the concept being presented.
T h e y are known,
however, and
partial
dervatives m a y b e taken to form t h e variational equations
* =
a n d t h e adjoint
(34)
F(t)x
equations
λ = - ,F (t)X
T
W e cannot
find
(35)
( i n c l o s e d f o r m ) s o l u t i o n s of t h e d i f f e r e n t i a l
equations
( 3 3 ) t o ( 3 5 ) , s o a d i g i t a l c o m p u t e r is p r o g r a m m e d t o s o l v e ( 3 3 ) a l o n g w i t h
six sets
of t h e v a r i a t i o n a l
equations
(35) for b a c k w a r d
integration.
Solutions ( n u m b e r s ) from t h e c o m p u t a t i o n give u s at discrete
X(t)y
the nominal
trajectory
trajectory;
to injection
a n d <f>(t; t0),
deviations.
In
times:
t h e sensitivities along t h e
particular,
</>(TD; t)
equals
the
sensitivities of a m i s s at p o i n t D d u e t o a n y deviation f r o m t h e n o m i n a l
a t a n e a r l i e r t i m e t.
2.
G U I D A N C E L A W FOR F I X E D T I M E OF ARRIVAL
W e can derive the
fixed-time-of-arrival
guidance law in exactly t h e
s a m e m a n n e r as w a s d o n e previously.
P a r t i t i o n </>(TD; tB) t o g i v e
Φ(ΤΒ;1Β)
= [- Φι
Φ
(36)
(37)
310
STANLEY F. SCHMIDT
S i n c e φ2 is t h e s e n s i t i v i t y of p o s i t i o n a t t i m e TD t o a v e l o c i t y d e v i a t i o n
a t t i m e t B w e a d d φ2(ΤΒ;
tB) x(tB)
t o e a c h s i d e of ( 3 7 ) , s e t t h e r e s u l t
e q u a l t o z e r o , a n d s o l v e f o r xg :
iy(h)
; tB)x(TD)
= -ΦΙ\ΤΒ
Note that immediately
after
= -^Vix(^) -
t h e velocity correction
(38)
Mh)
a t t i m e (tB)
the
d e v i a t i o n s t a t e χ is
(39)
If w e desire t o c o m p u t e t h e velocity c o r r e c t i o n r e q u i r e d at p o i n t D it is
MTD)
= -[Φ&αΜ+ΦΜϊΒ)]
w h e r e <f>3 a n d </S4 a r e t h e o t h e r t w o 3 x 3
t r a n s i t i o n m a t r i x <f>(tD; tB)y
=
(40)
-MTD)
m a t r i c e s of t h e p a r t i t i o n e d
s h o w n in (36).
E q u a t i o n ( 3 8 ) is t h e g u i d a n c e l a w f o r p o i n t Β a n d ( 4 0 ) is t h e g u i d a n c e
l a w f o r p o i n t D. T h e u s a g e of t h e s e t w o g u i d a n c e l a w s p e r m i t s t r a v e l
a r o u n d t h e m o o n o n t h e s a m e trajectory ( e x c e p t for g u i d a n c e
as t h e n o m i n a l
errors)
trajectory.
3 . GUIDANCE L A W FOR VARIABLE T I M E OF ARRIVAL
A s w a s t h e case for t h e s i m p l e e x a m p l e , a
variable-time-of-arrival
s y s t e m m a y b e d e r i v e d if a c e r t a i n t r a n s f o r m a t i o n t o
target-centered
c o o r d i n a t e s is m a d e . O n l y t h e p r i n c i p l e s w i l l b e g i v e n h e r e .
A trajectory
a p p r o a c h i n g a celestial b o d y generally h a s
hyperbolic
velocity relative t o t h e b o d y ( F i g . 13). T w o e x c e p t i o n s t o t h i s a r e w h e n
HYPERBOLIC
F I G . 1 3 . Approach trajectory to a celestial body.
NAVIGATION
311
PROBLEMS
t h e t a r g e t is t h e s u n a n d w h e n t h e t a r g e t is t h e e a r t h for a r e t u r n t r i p
f r o m t h e m o o n . W e c a n d e f i n e a p l a n e of m i s s , s i m i l a r t o t h e p l a n e o f
m i s s of t h e e x a m p l e , a s b e i n g t h r o u g h t h e t a r g e t c e n t e r a n d n o r m a l
to t h e approach asymptote. T h e points in this plane pierced b y t h e
a s y m p t o t e a n d t h e actual trajectory a r e s h o w n i n F i g . 14. T h e Β v e c t o r
Α
Ε
ΝΟΤΜΓ "
TO
» *
^
§ I S OUT OF PAPER
PLANE
ACTUAL
TRAJECTORY
PLANE OF MISS
FIG.
shown
1 4 . M i s s geometry. ( B in figure is equivalent t o Β in text.)
is t h e m i s s of t h e a s y m p t o t e .
T w o orthogonal
unit
vectors,
R a n d T, m a y b e c h o s e n i n a n y c o n v e n i e n t m a n n e r a n d t h e m i s s o f t h e
target defined b y
( β · A \ = miss
VlTlt
(41)
= (F»-2,i/Ä)V»
(42)
μ is t h e g r a v i t a t i o n a l c o n s t a n t of celestial b o d y a n d V is t h e velocity a t
r a d i u s R f r o m t h e p l a n e t . E q u a t i o n (42) is for t h e velocity a t a n infinite
d i s t a n c e f r o m t h e t a r g e t . I t c a n b e s h o w n t h a t if t h e s e t h r e e p a r a m e t e r s
a r e h e l d a t t h e s a m e values as o n t h e n o m i n a l trajectory, t h e d i s t a n c e of
1
closest a p p r o a c h will r e m a i n u n c h a n g e d b y small trajectory v a r i a t i o n s .
Let
/
\
T
δ Β
j δ Β · Ê I = deviations from n o m i n a l
W
l nf
Now
Β · f = F,(I(JT))
1
Β · A =
F2(X(T))
(43)
A s a matter of fact, the distance of closest approach is primarily governed b y Β · Τ
and Β · i?, since small midcourse corrections have very little influence o n Vint.
312
STANLEY F. SCHMIDT
a n d o n e m a y t a k e p a r t i a l d e r i v a t i v e s of t h e a b o v e e x p r e s s i o n s t o f i n d
dFt
8Χ,(Τ)
dFx
dXx(T)
dF1
8X3(T)
dF2
dXJT)
8Fl
3Xa(T)
dF,
exe(T)
dVlnt
8Vint
dxt{T)
8Xe(T)
=
C(T)x(T)
(44)
which may be written
/δΒ - 1 \
δΒ · Ê
= C(T)x(T)
= 0(Τ)φ(Τ;
(44a)
t)x(t)
Inf
w h e r e t h e p a r t i a l s C(T)
a r e e v a l u a t e d f o r t h e n o m i n a l t r a j e c t o r y a t t i m e T.
S i n c e C is a 3 X 6 a n d φ a 6 X 6, E q . ( 4 4 a ) m a y b e w r i t t e n
δΒ * ί \
δΒ - Ä) =
( £ j ^ ) «(0 =
3
3
3
3
Z) (0 + D2x(t)
(45)
lX
where
A s b e f o r e , if w e a d d D2ig
t o b o t h s i d e s of ( 4 5 ) a n d s e t t h e r e s u l t a n t e q u a l
t o z e r o , t h e n t h e s o l u t i o n f o r xg(t)
is
/δΒ · 7 \
xg(t)
Equation
energy
= -D?
δ Β · A ) = -(D^DXt)
( 4 6 ) i s a variable-time-of-arrival
+ x(0)
guidance
( o r v e l o c i t y a t a g i v e n r a d i u s ) relative
(46)
law for
to the target.
constant
A guidance
l a w of t h i s t y p e i s q u i t e u s e f u l f o r s p a c e m i s s i o n s w h e r e o n e p l a n s t o
expend
additional
e n e r g y (after
arriving near t h e target) to go
into
o r b i t , o r l a n d , o r b o t h . T h e r e a s o n is t h a t t h e e n e r g y r e q u i r e d for t h e s e
a d d e d m a n e u v e r s is u n a f f e c t e d
4.
by midcourse
correction.
G U I D A N C E L A W FOR M I N I M U M MIDCOURSE MANEUVER
W e m a y a l s o u s e t h e f a c t t h a t δ Vint h a s l i t t l e i n f l u e n c e o n t a r g e t m i s s
t o d e r i v e a g u i d a n c e l a w w h i c h c h o o s e s t h e v a l u e SVint
to minimize
313
NAVIGATION PROBLEMS
F r o m (46),
/SB •
S B · f>
δ Β · £ |
W
χ/χβ =
l
nf
31
(47)
"32 "33"· 8Vm
( α η δ Β · t + α 1 2δ Β · & +
+ ( « 2 1δ Β · f + « 2 2δ Β · Ê +
al38ViDtf
ß
23^1nf)
2
Λ
+ (<ζ 3 1δΒ · 7 + a 3 2S B · R + o « 8 F I l l )f »
(47a)
T a k i n g t h e p a r t i a l d e r i v a t i v e o f ( 4 7 a ) w i t h r e s p e c t t o SVinf a n d s e t t i n g
the resultant zero gives
0 =
2(αηδΒ
· Τ + a12SB
· Ê +
a138Vlnt)a13
+ 2 ( α 2 1δ Β · f + Λ 2 2δ Β · Ê + α 2 3δ Γ 1 η Γ) α 2 3
+ 2 ( * 3 1δ Β · f + α 3 2δ Β · R + α^ν1ηί)α33
S o l u t i o n o f ( 4 8 ) f o r 8Vint
_
, n f
_
(^11^13 +
yields
a
a a
+ Sl Zz)8B
~
' Τ + ( f l 1 f2 l 13 + a22 23
+
a
fl
32 33)SB ' ^
«?a + «la + «la
I f t h e v a l u e of 8Vint
arrival
flfl
21 23
(48)
guidance
f r o m (49) is p u t i n (47), w e h a v e a
law for minimum
midcourse
(49)
variable-time-of-
correction.
M a n y other guidance laws could b e derived using t h e principles we
h a v e g i v e n . T h e s e l a w s , of c o u r s e , a r e f o r t h e m i d c o u r s e p h a s e s of
flight
a n d a r e t h e easiest to obtain.
C. The Question of Linearity
Before s u c h g u i d a n c e laws as have b e e n described are used in practice,
o n e s h o u l d a l w a y s t e s t t h e s o l u t i o n s t o s e e if t h e d e v i a t i o n s
expected
a r e s m a l l e n o u g h . T h e t e s t o n e m a k e s is t o t a k e t h e c o r r e c t i o n
calcu-
l a t e d , a d d it t o t h e v e l o c i t y ( w i t h c o r r e c t s i g n ) , a n d i n t e g r a t e t h e n o n linear equations to t h e target. O n e t h e n calculates t h e target miss to see
if i t i s c l o s e e n o u g h t o t h e d e s i r e d ( n o m i n a l ) m i s s .
N o t e a l s o t h a t if a l a r g e c o m p u t e r i s a v a i l a b l e , o n e m a y a l w a y s i n t e g r a t e
the trajectory to t h e target even in real-time situations. T h i s
means
that t h e following iterative p r o c e d u r e could b e developed which would
y i e l d a n e x a c t s o l u t i o n . S e t xg =
0:
(1) I n t e g r a t e t h e t r u e t r a j e c t o r y t o t h e t a r g e t t o d e t e r m i n e d e v i a t i o n s
from the nominal.
314
STANLEY F. SCHMIDT
( 2 ) U s e t h e s e d e v i a t i o n s w i t h t h e l i n e a r g u i d a n c e l a w t o c o m p u t e x^.
( 3 ) L e t xg
total =
xg
f r o m s t e p (2) p l u s p r e v i o u s v a l u e ( f r o m
last
t i m e t h r o u g h ) ; r e t u r n to s t e p (1).
I f o n e g o e s t h r o u g h t h e s e s t e p s a s u f f i c i e n t n u m b e r of t i m e s
[until
t h e xg c o m p u t e d i n s t e p ( 2 ) is n e g l i g i b l e ] , t h e n t h e e x a c t ( f o r all p r a c t i c a l
p u r p o s e s ) v a l u e of t h e g u i d a n c e c o r r e c t i o n is f o u n d . P r a c t i c a l e x p e r i e n c e
w i t h s u c h i t e r a t i v e s c h e m e s a s t h i s s h o w s t h a t c o n v e r g e n c e is a l m o s t
always obtained.
V . E r r o r Analysis
T h e s u c c e s s of m o s t m i s s i o n s r e s t s o n w h e t h e r t h e c o m p o n e n t s
and
the m a n n e r in which they are used in the system can be chosen in s u c h
a w a y t h a t t h e e x p e c t e d deviation f r o m t h e desired objective at t h e e n d
of t h e m i s s i o n is w i t h i n a l l o w a b l e b o u n d s .
On
a space mission,
for
e x a m p l e , t h e a c c u r a c y of t h e l a u n c h - v e h i c l e g u i d a n c e s y s t e m c o n t r i b u t e s
t o t h e fuel r e q u i r e m e n t s for a m i d c o u r s e m a n e u v e r . T h e l o w e r t h e a c c u r a c y of t h e g u i d a n c e
system,
the larger the
midcourse
maneuver
is
likely t o b e . O n e usually desires t o k n o w t h e relative i m p o r t a n c e of t h e
v a r i o u s e r r o r s o u r c e s of t h e l a u n c h - v e h i c l e s y s t e m . B y a p p r o p r i a t e u s a g e
of t h e s e n s i t i v i t y c o e f f i c i e n t s m e n t i o n e d e a r l i e r , o n e c a n r e l a t e e a c h e r r o r
source to s o m e objective s u c h as m i d c o u r s e m a n e u v e r or miss at t h e
t a r g e t . I n t h i s m a n n e r o n e is a b l e t o d e t e r m i n e t h e t r a d e o f f s
between
i m p r o v e m e n t s i n s y s t e m d e s i g n a n d o b j e c t i v e s of t h e m i s s i o n . O n e c a n
also focus o n i m p r o v i n g those error sources w h i c h c o n t r i b u t e t h e m o s t
t o e r r o r s i n a t t a i n i n g t h e o b j e c t i v e s of t h e m i s s i o n .
To
perform
error analysis a n d apply t h e m e t h o d
g i v e n for
deter-
m i n a t i o n of s t a t e f o l l o w i n g t h i s s e c t i o n , s o m e b a c k g r o u n d i n s t a t i s t i c s is
required. T h i s section, therefore, introduces some additional
and fundamental
notation
definitions.
A. Definitions and Notation
T h e e x p e c t e d v a l u e of t h e s c a l a r f u n c t i o n f(x)
is g i v e n b y
— 00
w h e r e p(x)
is t h e p r o b a b i l i t y d e n s i t y f u n c t i o n , w h i c h h a s t h e p r o p e r t i e s
— 00
315
NAVIGATION PROBLEMS
and
J
p(x) dx = p r o b a b i l i t y t h a t χ lies in t h e interval A < x < B
F o r a Gaussian
o r normal
d i s t r i b u t i o n t h e d e n s i t y f u n c t i o n p(x)
is g i v e n
by
p(x)
1
=
(2π)!/2 0
Γ (*-*) i
ί
2σ J
2
exp
2
2
w h e r e χ = E(x)
i s t h e m e a n o r a v e r a g e v a l u e o f χ, σ
2
the variance, and σ =
1
2
= E(x )
2
— x
2
( σ ) / is t h e s t a n d a r d of d e v i a t i o n .
F o r t h e case of a v e c t o r f u n c t i o n of t h e v e c t o r
x,
7i(«)
k
T h e Gaussian
o r normal
E(Ux))
£(/(*)) =
/(*) =
/,(*)/
W„(*))y
d e n s i t y f u n c t i o n for t h e vector
χ =
is g i v e n b y
p(Xl,
*2 ,
xn)
= [(2π)*/2| Ρ |V2]-i e x p [ - ± ( * -
*)*(/«X* - *)]
where
/ E(Xl)
a n d Ρ is t h e c o v a r i a n c e
T
Ρ = E(xx )
2
~(E(Xl )
xx
matrix
T
— χ*)
{Ε(χΛχ2)
(Ε(χ2η-χ2η
;
{Ε(χχχη)
\
—
χλχη)
— xxx2)
—
{E(xxxn)
—
χλχη)
m
(Ε(χη*)-χηη
J
is
316
STANLEY F. SCHMIDT
Ρ is a s y m m e t r i c m a t r i x , i.e., (P^
=
P ^ ) . A l o n g t h e d i a g o n a l of Ρ
are
t h e v a r i a n c e s of e a c h of t h e c o m p o n e n t s of x. T h e o f f - d i a g o n a l t e r m s a r e
E x x
— *n*m 4
( n m)
σ σ
Ρητη η τη > w h e r e
P
is t h e c o r r e l a t i o n b e t w e e n t h e
nm
η a n d m c o m p o n e n t s of x. T h e c o r r e l a t i o n r a n g e s b e t w e e n t h e v a l u e s
~~ 1 ^
Pnm ^
+1 · \P \
ι
ls
^
d e t e r m i n a n t of t h e c o v a r i a n c e m a t r i x P .
B. Propagation of Errors in Linear Systems
S u p p o s e one has t h e linear system
+
χ = F(t)x
(50)
B(t)u(t)
whose solution m a y be written
(51)
f o r u(t0)
c o n s t a n t in t h e interval t0 ^
t ^
tt.
Assume
E(x(t0))
=
E{(x(tu)
-
*((„)
x{t0))(x(t0)
£("ί(Ό)**(ίο)) =
E(u(t0))
-
0
x(t0))T}
for
=
all / a n d
P(t0)
(53)
(54)
k
= 0
E(u(t0)uT(t0))
Problem.
(52)
D e t e r m i n e E(x(t1))
(55)
=
(56)
Q
and
= x(t^)
F r o m (51) w e see t h a t
£(*('i)) = *('i) =
; t0)E(x(t0))
+
;
t0)E(u(t0))
w h i c h a s a r e s u l t of ( 5 2 ) a n d ( 5 5 ) is
(57)
Let
X =
X — X
T h e n f r o m (57) a n d (51) w e see t h a t
*&) = Φ(*ι î W o )
+ U(ti ; *o)«(*o)
(58)
NAVIGATION
317
PROBLEMS
F r o m (58)
+ <f,E(x(t0)uT(t0))lF
r
+
UE(u{t0)XT(t0W
T
+ C / £ ( « ( < » ) « ( i 0) ) ^ = P{h)
(59)
As a result of (54) t h e cross t e r m s a r e z e r o a n d o u r result is, u s i n g (53)
a n d (56),
τ
(60)
Ρ(ίι)=φΡ(ί0)φτ+υρυ
W i t h n o n l i n e a r p r o b l e m s t h e p r o c e d u r e i s t h e s a m e if w e v i e w ( 5 0 ) a s t h e
variational equation a n d (51) as t h e solution along a nominal trajectory.
Example
Problem:
Suppose y o u were given t h e covariance matrix
of d e v i a t i o n f r o m t h e n o m i n a l t r a j e c t o r y a t i n j e c t i o n f o r a l u n a r s p a c e craft. T h i s w o u l d c o r r e s p o n d t o p o i n t A of F i g . 10. Y o u r p r o b l e m is t o
d e t e r m i n e t h e r o o t - m e a n - s q u a r e ( r m s ) velocity correction required at
p o i n t Β ( F i g . 1 0 ) f o r t h e fixed-time-of-arrival
guidance law [ E q . (38)]:
xg(tB)
= —$ï\TD
; tB)<f>i(TD ; tB)x(tB)
-
x(tB)
w h e r e t B i s t h e t i m e o f t h e v e l o c i t y c o r r e c t i o n a n d TD i s t h e t i m e t h e
n o m i n a l trajectory arrives at p o i n t D ( F i g . 12). E q u a t i o n (38) m a y b e
written
(61)
If o n e a s s u m e s that
(that is, t h e m e a n value of t h e d e v i a t i o n f r o m t h e n o m i n a l
a t i n j e c t i o n i s z e r o ) a n d if w e a r e g i v e n
T
E(x(tA)x (tA))
= P(tA)
= covariance m a t r i x of deviations
from n o m i n a l at injection
trajectory
(62)
a n d if t h e r e a r e n o f o r c i n g f u n c t i o n s ,
(63)
t h e n b y using (60) for Q = 0,
(64)
318
STANLEY F.
SCHMIDT
F r o m (61) w e see t h a t
£(*„*/)
=
CP(tB)CT
(65)
a n d i n t r o d u c i n g (64) gives
(66)
w h i c h i s t h e c o v a r i a n c e m a t r i x of m i d c o u r s e v e l o c i t y - c o r r e c t i o n r e q u i r e ments.
T h e m e a n - s q u a r e velocity r e q u i r e m e n t s are
E(ig\)
= E{*1)
+ £(4,)
= trace of
+
E(*l)
T
E(xgxg )
(67)
T h u s t h e r m s v e l o c i t y r e q u i r e m e n t s a r e t h e s q u a r e r o o t of ( 6 7 ) ,
r m s velocity = [trace of
12
E^x/)] !
(68)
T h i s e x a m p l e i l l u s t r a t e s t h e p r i n c i p l e s of e r r o r analysis a n d h o w t h e
c o n c e p t s of s t a t e a n d s t a t e t r a n s i t i o n a r e q u i t e u s e f u l i n d e r i v i n g t h e
e q u a t i o n s f o r c a l c u l a t i o n of i m p o r t a n t q u a n t i t i e s . T o b e efficient i n
obtaining s u c h calculations, relatively complicated d i g i t a l - c o m p u t e r
p r o g r a m s a r e r e q u i r e d . W i t h s u c h t o o l s , h o w e v e r , t h e a n a l y s i s of v e r y
c o m p l i c a t e d m i s s i o n s is p o s s i b l e .
V I . Determination of S t a t e
I n m a n y s y s t e m s t h e m e a s u r e m e n t s of o b s e r v a b l e q u a n t i t i e s a t o n e
p o i n t i n t i m e is i n s u f f i c i e n t t o d e t e r m i n e t h e s t a t e . A l s o , t h e a c c u r a c y of
o b s e r v a t i o n s m a y n o t allow o n e t o d e t e r m i n e t h e s t a t e of t h e s y s t e m t o
t h e d e s i r e d a c c u r a c y . T h e b e s t a p p r o a c h t o t h e s o l u t i o n of s u c h p r o b l e m s
is t o i n t r o d u c e t h e k n o w l e d g e of e q u a t i o n s of m o t i o n a n d h a v e a m e a n s
of s m o o t h i n g t h e o b s e r v e d d a t a . T h a t i s , find a s o l u t i o n of t h e e q u a t i o n s
of m o t i o n w h i c h p r o v i d e s a " b e s t " fit t o t h e d a t a .
K a l m a n (7) d e r i v e d a m e t h o d for s o l v i n g s u c h p r o b l e m s w h e n t h e
e q u a t i o n s of m o t i o n a r e l i n e a r . T h e a u t h o r (et al.) (2-4) s h o w e d t h a t t h e
m e t h o d c a n b e u s e d for n o n l i n e a r p r o b l e m s p r o v i d e d o n e p e r f o r m s t h e
,,
l i n e a r i z a t i o n r e q u i r e d a r o u n d t h e " b e s t e s t ί m a t e of t h e s t a t e of t h e
n o n l i n e a r s y s t e m . T o a p p l y t h e m e t h o d , a n i n i t i a l e s t i m a t e of t h e s t a t e
of t h e n o n l i n e a r s y s t e m a n d t h e c o v a r i a n c e m a t r i x of e r r o r s i n t h i s
e s t i m a t e m u s t b e a v a i l a b l e . A r e a s o n a b l e w a y of o b t a i n i n g t h i s is b y u s e
of t h e " l e a s t - s q u a r e s " fit t o a p o l y n o m i a l .
NAVIGATION
319
PROBLEMS
I n t h i s s e c t i o n w e s h a l l first g i v e a f e w m o r e m a t h e m a t i c a l f o r m u l a s
r e q u i r e d to u n d e r s t a n d t h e derivations; second, w e shall derive t h e
f o r m u l a f o r t h e l e a s t - s q u a r e s fit t o a p o l y n o m i n a l a n d t h e c o v a r i a n c e
m a t r i x of t h e e r r o r i n t h i s e s t i m a t e ; t h i r d , w e s h a l l d e r i v e K a l m a n ' s
m e t h o d for w h i c h t h e d e r i v a t i o n s g i v e n a r e c o n s i d e r a b l y s i m p l e r b u t
m o r e r e s t r i c t i v e t h a n t h e o r i g i n a l (7). F i n a l l y , w e s h a l l s h o w t h e a p p l i c a t i o n of t h e m e t h o d t o t h e t r a j e c t o r y - d e t e r m i n a t i o n p r o b l e m .
A. Derivative of Vector Quantities
T o find t h e m a x i m u m a n d m i n i m u m of a s c a l a r f u n c t i o n of a s i n g l e
variable o n e differentiates, sets t h e resultant equal to zero, a n d solves
t h e r e m a i n i n g e q u a t i o n . F o r t h e c a s e of a f u n c t i o n of a v e c t o r v a r i a b l e
a s i m i l a r o p e r a t i o n t a k e s p l a c e if w e u s e t h e g r a d i e n t , V .
gradient = V x = ( —
)
T h e g r a d i e n t of a s c a l a r f u n c t i o n o f t h e v e c t o r x, f(x),
3/
7
w h e r e x t , x2 ,
xn
df
'
\
dxn J
a r e t h e c o m p o n e n t s o f x. F o r / b e i n g a v e c t o r ,
g/x
ffi\
Ι
/=i/.|
8/
' 8x,
is
dxi
v . / = |_%_
eft
...
if
&c2
dxn
S u p p o s e y o u w i s h t o find VAy,
where A represents a constant
transformation. First, you partition A by rows such that
w h e r e a^yy
T
a2 y,
linear
etc., are scalars:
Va/y
VAy
= auVyi
+ a12Vy2
-\
h
alnVyn
= a/Vy
(69)
= A(Vy)
(70)
320
STANLEY F.
T
S u p p o s e y o u w i s h t o find ^y Qy,
Let
SCHMIDT
w h e r e Q is a c o n s t a n t s q u a r e m a t r i x .
Then
yTQy
= yiqTy
y^Ty
+
...
+
+
y^Ty
D i f f e r e n t i a t i o n of ( 7 1 ) g i v e s
y&^y
+
T
ynqn Vy
qfy^yA
+
qn yVy\
T
T
N o t e t h a t qn y is a s c a l a r a n d m a y b e w r i t t e n y qn
C o l l e c t i o n of t e r m s i n ( 7 2 ) g i v e s
.
T
T
T
(72)
T
v(y Qy) = y QVy + y Q^y
(73)
T
I f Q is s y m m e t r i c , i.e., Q = Q > t h e n
VyTQy
T
=
(74)
2y QVy
B. Least-Squares Fit to a Polynomial
S u p p o s e y o u a r e g i v e n η o b s e r v a t i o n s of t h e s c a l a r q u a n t i t y x:
x
(h)y
x
(h\
x
(h)>
-»*(*n)
Y o u w i s h t o o b t a i n a n / t h - o r d e r p o l y n o m i a l fit t o t h e o b s e r v a t i o n s
(/ < n) s u c h t h a t t h e s u m of t h e s q u a r e s of t h e d e v i a t i o n s of t h e d a t a
f r o m t h e p o l o n o m i a l is m i n i m i z e d . M a t h e m a t i c a l l y w e w i s h t o d e t e r m i n e
t h e c o e f f i c i e n t s yt of
*(t)=y0+y1t+yJ*
+
ι\ * '
y
= (i t
»>+ylt')
y
(75)
NAVIGATION
321
PROBLEMS
W h i c h f o r all t h e o b s e r v a t i o n s g i v e s
1
x(t2)
1
o r χ = Ay,
t
1
X(tn)
..
..
h
h
2
t
/ι
/ι
~ yo "
yi
tj
.yi
(76)
-
and we wish to minimize
T
Ay) {x
(* -
— Ay)
= L
(77)
E x p a n d i n g (77) o n e o b t a i n s
T
L = xx
T T
which, since y A x
T T
— yAx
T
T
-
x Ay
T T
- y A Ay
(78)
gives
= x Ay,
T
L = xx
T
— 2x Ay
T T
- y A Ay
(79)
T a k i n g t h e g r a d i e n t of ( 7 9 ) w i t h r e s p e c t t o y a n d s e t t i n g t h e r e s u l t a n t
zero gives
VL = 0 =
T
T T
+ 2y A AI
-2x AI
(80)
= / ) . S o l v i n g ( 8 0 ) f o r y, o n e o b t a i n s
w h e r e / is t h e i d e n t i t y m a t r i x ÇVyy
τ
χ τ
y = (Α Α)- Α χ
(81)
T h e m a t r i x A is m a d e of p o w e r s of t h e i n d e p e n d e n t v a r i a b l e , t i m e , a s
s e e n f r o m ( 7 6 ) . T h u s g i v e n t h e o b s e r v a t i o n s χ(ί{),
one can compute
t h e c o e f f i c i e n t s of t h e p o l y n o m i a l y w h i c h p r o v i d e t h e b e s t fit i n a l e a s t squares sense.
O n e m a y a s k h o w t o d e t e r m i n e t h e c o v a r i a n c e m a t r i x of t h e e r r o r s i n
this estimate caused by errors in the observations. ( W e assume that
sufficient t e r m s a r e t a k e n i n t h e p o l y n o m i a l s u c h t h a t t r u n c a t i o n e r r o r s
are n o n e x i s t e n t ) . W e state this p r o b l e m as follows. G i v e n
find
F r o m (81) w e see that
(82)
N o w l e t u s a s s u m e t h a t Px is a d i a g o n a l m a t r i x w i t h all d i a g o n a l t e r m s
e q u a l . T h i s is e q u i v a l e n t t o h a v i n g t h e m e a s u r e m e n t e r r o r s all w i t h t h e
s a m e v a r i a n c e a n d u n c o r r e l a t e d i n t i m e . Px m a y t h e n b e w r i t t e n a s
(83)
322
STANLEY F.
SCHMIDT
Placing (83) in (82) gives
τ
Py =
σ*{Α Α)-*
(84)
E q u a t i o n s ( 8 4 ) a n d ( 8 1 ) c a n b e u s e d t o find a n i n i t i a l e s t i m a t e of a n o n l i n e a r s t a t e v e c t o r of t h e e q u a t i o n s of m o t i o n a n d t h e e r r o r i n t h i s
e s t i m a t e w i t h t h e h e l p of a d d i t i o n a l i n f o r m a t i o n ( e q u a t i o n s ) r e l a t i n g
observations to the state.
C. Derivation of Kalman's Filter
T h e e r r o r - a n a l y s i s s e c t i o n of t h i s c h a p t e r i l l u s t r a t e s t h e m a n n e r i n
which errors propagate t h r o u g h linear systems. T h u s , given an estimate
of t h e s t a t e v e c t o r a n d t h e c o v a r i a n c e m a t r i x of t h e e r r o r s i n t h i s e s t i m a t e
a t a n y o n e t i m e i n t h e s o l u t i o n , o n e m a y find t h e s a m e q u a n t i t i e s a t a n y
later t i m e . I n t h e general p r o b l e m o n e m a k e s observations at discrete
t i m e s along t h e trajectory, as illustrated in Fig. 15. Since w e m a y
F I G . 15.
Observation times.
propagate the estimate and error in the estimate between such time
p o i n t s , t h e r e m a i n i n g p r o b l e m is h o w t o i m p r o v e t h e e s t i m a t e d u e t o
t h e observations at t h e discrete t i m e points. It s h o u l d b e o b v i o u s t h a t
if e a c h o b s e r v a t i o n is u s e d i n a n o p t i m a l f a s h i o n t o o b t a i n a n e w e s t i m a t e ,
t h e n all d a t a p o i n t s a r e u s e d i n a n o p t i m a l f a s h i o n . T h e e s t i m a t e o b t a i n e d
is o p t i m u m , t h e r e f o r e , if o n e s e q u e n t i a l l y p r o c e s s e s t h e o b s e r v a t i o n s
in an o p t i m a l way. W e f o r m u l a t e o u r p r o b l e m m a t h e m a t i c a l l y as follows.
Given
jc(ij) = e s t i m a t e of χ
P(tx)
7
= E[(x — x)(x — x) ]
= covariance m a t r i x of t h e e r r o r in t h e estimate
ç(ij) = random error in measurement of yfa)
Γ
£[?(Ί)? (ίι)]
=0
NAVIGATION
F i n d a n e w e s t i m a t e xn(t)
o f x(tx)
323
PROBLEMS
such that
(85)
L=E[(x-xnY{x-xn)]
is m i n i m i z e d . T w o d e r i v a t i o n s of t h e s o l u t i o n a r e g i v e n . T h e first
requires t h e assumption that t h e r a n d o m variables are Gaussian a n d
t h e s e c o n d r e q u i r e s t h e a s s u m p t i o n of a l i n e a r filter.
1. DERIVATION O N E
T h e loss f u n c t i o n [ E q . (85)] m a y b e w r i t t e n
T
L = j (x — xn) (x
— xn)p(x
I y, x) dx
(86)
T a k i n g t h e g r a d i e n t o f ( 8 5 ) w i t h r e s p e c t t o xn a n d i n t e r c h a n g i n g o r d e r
of d i f f e r e n t i a t i o n a n d i n t e g r a t i o n g i v e s
T
VL = j 2(x - xn) p(x
I y, x) dx
(87)
S i n c e xn i s a c o n s t a n t , w e m a y i n t e g r a t e ( 8 7 ) t o g i v e
VL -
T
T
2 J x p(x
I y> x) dx - 2xn
By definition, t h e t e r m u n d e r t h e i n t e g r a l is t h e c o n d i t i o n a l
S e t t i n g V L e q u a l t o z e r o a n d s o l v i n g f o r xn w e o b t a i n
(88)
mean.
(89)
xn = E{x\y,x)
E q u a t i o n ( 8 9 ) s h o w s t h a t t h e c o n d i t i o n a l m e a n is t h e o p t i m u m e s t i m a t e
f o r t h e l o s s f u n c t i o n of ( 8 5 ) .
By making t h e assumption that t h e r a n d o m variables are Gaussian,
w e c a n d e t e r m i n e t h e p r o b a b i l i t y d e n s i t y f u n c t i o n p(x \ y, x). F o r a
G a u s s i a n r a n d o m v a r i a b l e t h e m e a n is a t t h e m a x i m u m o f t h e d e n s i t y
function. T h u s w e shall b e able t o d e t e r m i n e t h e e q u a t i o n s for t h e
o p t i m u m e s t i m a t e b y s e t t i n g t h e g r a d i e n t of t h e e x p o n e n t o f e e q u a l
to zero.
F r o m t h e given information,
*(*) =
n)^\P(
{2
tl)\^
e x
Ι Ρ
1
P { - * ( * - *) ΐ " (ίι)](* - *)}
(90)
324
STANLEY F. SCHMIDT
where η =
n u m b e r of s t a t e s a n d P(t)
is t h e c o v a r i a n c e m a t r i x of
x.
T h e d e v i a t i o n of t h e o b s e r v a t i o n y f r o m i t s m e a n is
y — y = Η (χ — x) + q
T h e c o v a r i a n c e m a t r i x of t h i s d e v i a t i o n is
my - y){y - yf]
7
s i n c e f r o m t h e a s s u m p t i o n s E{(x
p
(
)y
(n =
=
e
+ Qiv*
( i ^ H P W
— x)^ ]}
P<-^ -
(9i)
0,
=
x
+Q
= HPH^
T
m n P H
+
QTHy - y)} (92)
n u m b e r of o b s e r v a t i o n s ) ,
P(y
I *) =
P{q) =
( 2 7 )rn / 2 |
Q |i/2 « P ( - f c ß " ^ )
r
B y u s e of B a y e s ' e q u a t i o n w e m a y u s e ( 9 0 ) , ( 9 1 ) , a n d ( 9 2 ) t o
obtain
x
p(*\y> )=
)
p
{
y
T
= A exp{-£[(* T
-
(y - y) (HPH
x) P-\x
T
- x ) +
+ Q)-\y
T
x
q Q~ q
(93)
- y)]}
where
A
_
\HPH
T
+
2
Q\V*
(2π)"/ | Q | ! / 2 | ρ |l/2
T a k i n g t h e g r a d i e n t of t h e e x p o n e n t w i t h r e s p e c t t o χ a n d s e t t i n g t h e
resultant χ equal to &n , we obtain
V ( e x p o n e n t ) = -[(xn
S i n c e y = Hx
-
T
x) P~^
-
T
+ QY^x{y
- y)}
(94)
-\- q>
Vxy
=
H
S e t t i n g ( 9 4 ) e q u a l t o z e r o a n d s o l v i n g f o r xn
T
T
xn = S + PH (HPH
where y =
T
(y - y) (HPH
gives
+ Q)-\y
(95)
- y)
Η χ a n d y is t h e o b s e r v a t i o n . E q u a t i o n ( 9 5 ) s h o w s t h e o p t i -
m u m n e w e s t i m a t e . W e m u s t d e t e r m i n e t h e c o v a r i a n c e m a t r i x of
the
error in this estimate.
E[(x -
xn)(x
-
χ„Υ]
= E{[{x - x ) - K(y
- j>)][(* -
x) -
K{y - y)Y}
(96)
NAVIGATION PROBLEMS
325
where
T
T
Κ = PH (HPH
+ Q)-
1
E x p a n d i n g (96) w e obtain
E[(x
-
*„)(*
-
xnY]
=
E[(x
-
x)(x
-
-
χ)*]
E[(x
-
x)(y
-
yffL*}
- E[K(y - y){x - xf] + E[K(y - y)(y - $γκ?]
Letting y — y =
H(x
t h e t e r m s of (97) are
— χ) +qf
E[(x — x)(x
(97)
-x)]=P
T
T
= E{[(x - x)][(x - x) H
E[(x - x)(y - yY]K
T
T
T
T
+
q ]}K
T
T
since E[(x - x)(q) ] = 0
= PH K ,
T
7
E[K(y - y){x - x) ] = KE[(H(x - x) + q){x - χ) ] = KEP
T
T
T
E[K(y - y)(y - y) K ]
= K[HPH
+
Q]K
(98)
T
S u b s t i t u t i o n of t h e v a l u e of Κ i n ( 9 8 ) g i v e s
T
PH K
T
T
T
= PH (HPH
+ QY^HP
T
T
+ Q)~ EP
T
T
+ Q)-\HPH
T
T
+
X
KEP = PE (EPE
K(EPE
T
+ Q)K
T
= PE (EPE
= PE (EPE
T
T
+ Q)(EPE
+
X
QY EP
X
QY EP
T h u s (97) m a y b e w r i t t e n
E[(x - xn)(x - * η ) η = P
T
n
= P - PE (EPE
T
+ QY^EP
T h e t w o results, (95) a n d (99), are r e p e a t e d b e l o w for c o n v e n i e n c e :
T
xn = S + PE (EPE
T
Pn=P-
T
PE (EPE
T
+ QY\y
- y)
+ QY^EP
(95)
(99)
E q u a t i o n ( 9 5 ) s h o w s h o w t h e e s t i m a t e is m o d i f i e d b y e a c h n e w o b s e r v a t i o n y, a n d ( 9 6 ) s h o w s h o w t h e c o v a r i a n c e m a t r i x o f t h e e r r o r i n t h e
e s t i m a t e is r e d u c e d b y e a c h n e w o b s e r v a t i o n .
2.
DERIVATION
Two
I n t h i s d e r i v a t i o n w e a s s u m e t h a t t h e e s t i m a t e w e d e s i r e is l i n e a r , i . e . ,
x
n
=
x
+
A(y-y)
(100)
326
STANLEY F. SCHMIDT
I n ( 1 0 0 ) , y is t h e o b s e r v a t i o n , χ is t h e o l d e s t i m a t e of x, a n d y is t h e v a l u e
of y c o m p u t e d f r o m x, i.e., y = Hx.
I n t h i s c a s e w e w i s h t o find A
which
m i n i m i z e s t h e l o s s f u n c t i o n of ( 8 5 ) . N o t e t h a t
T
E[(x — xn) (x
= trace E[(x — xn)(x
— xn)]
T
—
xn) ]
*) -
A{y
- y)(x
-
T h e covariance matrix
E[(x -
xn)(x
T
-
xn) ]
= E{[(x
- x ) - A(y
= E[(x Letting y - y =
H(x
T
x) ]
x)(x -
E[(x -
- y)][(* -
E[A(y
T
x)(y
- y) A^\
+ E[A(y
y)] )
xf]
- y)(y
+ ? i n t h e a b o v e a n d n o t i n g E[(x
-A)
T
-
-
yfA*]
- χ)α^\
=
0,
we obtain
E[(x -
&n)(x -
T
xn) ]
= P -
ΑΗΡ
T T
-
PH A
a n d o u r p r o b l e m is t o d e t e r m i n e A
minimum.
Note
that
if A, Py a n d
+ A(HPH
T
+ Q)A
T
(101)
s u c h t h a t t h e t r a c e of ( 1 0 1 ) is a
are scalars a n d
Η
one takes
the
d e r i v a t i v e of ( 1 0 1 ) w i t h r e s p e c t t o A a n d s e t s t h e r e s u l t a n t e q u a l t o z e r o ,
then
T
T
-2PH
+ 2A(HPH
+
Q)=0
or
T
A = PH (HPH
T
+ Q)-
1
(102)
T h u s w e s h a l l a s s u m e t h a t t h e s o l u t i o n is g i v e n b y ( 1 0 2 ) a n d
prove
t h a t t h e r e s u l t is g e n e r a l .
Let
C = A -
T
T
PH (HPH
+ Q)-
1
T
T
+ ρ)"
A = C + PH (HPH
1
T h e n t h e t r a c e of ( 1 0 1 ) m a y b e w r i t t e n
Tr{[E(x
-
= Tr[P]
-
xn)(x
T
-
xn) ]}
T
T r [ ( C + PH (HPH
T
Tr[PH (C
T
T
+ PH (HPH
T
T
+ T r { [ C + PH (HPH
T
+
QY^HP]
1 7
+
Q)- ) ]
1
T
+ Q)- ][(HPH
T
+ Q)][C
T
+ (HPH
+
ρ^ΗΡ]}
(103)
NAVIGATION
327
PROBLEMS
W e w i s h t o c h o o s e C s u c h t h a t ( 1 0 3 ) is a m i n i m u m . A n u m b e r o f t h e
t e r m s cancel in (103), leaving
T r E[(x - xn)(x
T
- xn) ]
T
= Tr[P - PH (HPH
T
+ Q^HP
T
+ C(HPH
+
Q)C
T
(104)
T
S i n c e (HPH
+ Q) is s y m m e t r i c a n d p o s i t i v e - d e f i n i t e (it is t h e c o v a r i a n c e
m a t r i x of t h e d e v i a t i o n b e t w e e n t h e o b s e r v e d a n d c o m p u t e d v a l u e s of
7
T
T
+
Q)C
t h e o b s e r v a t i o n , E[(y - y) (y - j ) ) ] , t h e n c l e a r l y C(HPH
m u s t b e p o s i t i v e . T h e r e f o r e , t h e b e s t c h o i c e of C f o r m i n i m i z i n g t h e
7
t r a c e E[(x — £n) (x — J c J ] is C = 0 , o r
T
T
1
A = PH (HPH
(105)
+ Q)-
W i t h C = 0 i n ( 1 0 4 ) , w e c a n s e e t h a t t h e c o v a r i a n c e m a t r i x of t h e e r r o r
i n t h e e s t i m a t e is
E[(x -
xn)(x
-
xny]
T
T
= Ρ — PH (HPH
+ Q^HP
(106)
T h i s r e s u l t is e q u i v a l e n t t o t h a t g i v e n p r e v i o u s l y i n ( 9 9 ) .
T h e s e t w o p r o o f s h a v e s h o w n t h a t t h e o p t i m u m e s t i m a t e is l i n e a r
for G a u s s i a n r a n d o m variables ( d e r i v a t i o n o n e ) ; a n d t h a t t h e o p t i m u m
linear e s t i m a t e for a n y p r o b a b i l i t y d e n s i t y f u n c t i o n , given m e a n s ,
v a r i a n c e s , a n d c o r r e l a t i o n f a c t o r s , is i d e n t i c a l t o t h a t f o r t h e G a u s s i a n
r a n d o m variable (derivation two).
Example
Problem:
A s s u m e a h e a v y r i n g is s l i d i n g o n a
frictionless
RING
FIG. 1 6 .
G e o m e t r y for e x a m p l e problem.
b a r ( F i g . 16). F o u r o b s e r v a t i o n s of t h e d i s t a n c e χ f r o m t h e
point are made. T h e s e measurements are:
Time
reference
χ (meters)
0
1.1
1
2.0
2
3.2
3
3.8
O t h e r s t u d i e s of t h e p r o b l e m h a v e e s t a b l i s h e d t h a t a t t i m e z e r o t h e
m e a n v a l u e of x(0) = E(x) = 0 a n d t h e m e a n v a l u e of t h e v e l o c i t y
328
*(0) =
STANLEY F. SCHMIDT
E(x)
E(x(0)x(0))
= 0
2
and
=
E(x (0))
2
(m) ,
2
E(x (0))
=
2
10 ( m / s e c ) ,
= 0 . T h e m e a s u r i n g i n s t r u m e n t is k n o w n t o h a v e a r a n d o m
e r r o r w i t h a v a r i a n c e of 0 . 1 ( m )
Problem:
10
Formulate
the
2
a n d a m e a n v a l u e of z e r o .
problem
mathematically
in
the
state
n o t a t i o n a n d c a l c u l a t e t h e e s t i m a t e a n d c o v a r i a n c e m a t r i x of t h e e r r o r i n
e s t i m a t e after each o b s e r v a t i o n .
T h e m a t h e m a t i c a l f o r m u l a t i o n is a s f o l l o w s . S i n c e t h e b a r is f r i c t i o n less,
χ =
0
Let
=
*
*
x2 = χ
lA = χ
X
and
\ Ä ; 2/
Then
C;) - [? ft
T h e o b s e r v a t i o n is a d i r e c t m e a s u r e m e n t o f p o s i t i o n , s o
y = (o i ) Q ) + ç = # x + ?
2
T h e v a r i a n c e o f q, E(q ),
o f x2 is £ 2 ( 0 ) =
= 0 . 1 . T h e s t a r t i n g e s t i m a t e of xx is ^ i ( 0 ) =
0,
0 and
£[(x-x)(x-i)fJ
= P(0) = ( ^
$
S i n c e t h e e q u a t i o n s of m o t i o n a r e l i n e a r , w e m a y w r i t e t h e
general
solution
x ( i ) = φ(ί;
T h e t r a n s i t i o n m a t r i x φ(ί\
t0)
t0)x(t0)
m a y be determined by direct integration
to be
«;'<»> = [I_, ?]
0
A s for t h e calculations,
o n l y t h e c a l c u l a t i o n s f o r t h e first d a t a p o i n t
are shown here. T h e equations to be used are:
At the
observation:
T
xn = χ + PH (HPH
T
+ Q)-\y
- y)
Pn = Ρ -
T
PH (HPH
T
+
X
Q)- HP
NAVIGATION
329
PROBLEMS
Between observations:
m = w\ Wo)
p(t)=κ*,
< o W f t ίο)
F o r t h e first o b s e r v a t i o n ,
' - r c
= r
ιοί-1"
<
,aa+<p < >a
°i
10
L0
i3ci[IP
0.099J
h
1
x-datum
points
Slope of estimate
lines indicate estimate
of velocity (JT,)
I
0
FIG. 17.
1
I
2
Time, sec
U p d a t i n g of state estimate for i n p u t of observations.
I
3
330
STANLEY F. SCHMIDT
P r i o r t o i n c l u d i n g t h e d a t a p o i n t a t 1 s e c t h e e s t i m a t e is
«•>-(! M - P
a n d t h e c o v a r i a n c e m a t r i x of t h e e r r o r e s t i m a t e is
K l
10.5
F I G . 18.
h
mo
0.099.1 lo
îJ
Lio
10.09J
R
Change of velocity and position variances with observation time.
331
NAVIGATION PROBLEMS
T h e r e s u l t s of t h e c a l c u l a t i o n s f o r t h e f o u r o b s e r v a t i o n s a r e s h o w n i n
F i g s . 17 a n d 1 8 . F i g u r e 17 s h o w s h o w e a c h m e a s u r e m e n t c h a n g e s t h e
e s t i m a t e of p o s i t i o n a n d v e l o c i t y ( s l o p e of t h e l i n e ) a t t h e t i m e of o b s e r +
v a t i o n . T h e final e s t i m a t e ( a t t = 3 ) c a n b e s e e n t o e f f e c t i v e l y s p l i t
the d a t u m points, leaving an error (residual) which takes on b o t h
p o s i t i v e a n d n e g a t i v e v a l u e s . F i g u r e 18 s h o w s h o w t h e v a r i a n c e s of t h e
e r r o r i n t h e e s t i m a t e s of p o s i t i o n a n d v e l o c i t y c h a n g e w i t h a n d b e t w e e n
t h e o b s e r v a t i o n s . T h e r m s p o s i t i o n e r r o r a t t h e e n d of f o u r o b s e r v a t i o n s
1 2
1 /2
= 0 . 0 7 / ^ 0.26 m, a n d t h e r m s velocity error = 0 . 0 2
^ 0.14 m/sec.
V I I . P a r a m e t e r Estimation
F r e q u e n t l y one desires to estimate u n k n o w n parameters in addition
t o t h e p o s i t i o n a n d v e l o c i t y of t h e v e h i c l e . T h e s e p a r a m e t e r s m a y b e
u n k n o w n s i n t h e e q u a t i o n s of m o t i o n , s u c h a s g r a v i t a t i o n a l a n o m a l i e s ,
or t h e y m a y b e u n k n o w n s in t h e m e a s u r e m e n t s , s u c h as biases or
station location u n k n o w s . T h i s section deals with the p r o b l e m f o r m u lation for t h e a b o v e t o p i c s .
T h e e q u a t i o n s of m o t i o n m a y i n g e n e r a l b e w r i t t e n
(107)
X=F(X,V1t)
w h e r e X is t h e v e c t o r of p o s i t i o n s a n d v e l o c i t i e s a n d U i s t h e v e c t o r of
f o r c i n g f u n c t i o n s p l u s u n k n o w n p a r a m e t e r s i n t h e e q u a t i o n s of m o t i o n .
T h e observations or m e a s u r e m e n t s are in general related to X b y
Y = G(X,
V, t) + *(X,
q
V, t)
(108)
w h e r e F is a v e c t o r of u n k n o w n p a r a m e t e r s a n d q* a r e r a n d o m e r r o r s
i n m e a s u r e m e n t . L i n e a r i z a t i o n of ( 1 0 7 ) a n d ( 1 0 8 ) a b o u t a n o m i n a l
trajectory gives
'-[·&]"•»-[•&]"
»-tS'+[4r> *>
+
<'°"
>
(ll0
T h e r a n d o m error in m e a s u r e m e n t has b e e n called q in (110). T h e
m a g n i t u d e of t h e r a n d o m e r r o r is, i n g e n e r a l , d e p e n d e n t o n s t a t e ;
h o w e v e r , t h i s is u s u a l l y c o n s i d e r e d i n a s e p a r a t e s t a t e m e n t of t h e p r o b l e m .
N o g e n e r a l i t y is l o s t b y t h e a b o v e s i m p l i c a t i o n .
I n ( 1 0 9 ) a n d ( 1 1 0 ) u a n d ν a r e c o n s i d e r e d a s s m a l l v a r i a t i o n s of U
a n d V, r e s p e c t i v e l y . B y w r i t i n g t h e e q u a t i o n s i n t h i s m a n n e r w e s h a l l b e
a b l e t o find n e w e s t i m a t e s of U a n d V a s o b s e r v a t i o n s a r e i n t r o d u c e d .
332
STANLEY F. SCHMIDT
N o t e t h a t a c o n s t a n t c o b e y s t h e differential
equation
(111)
T h u s w e m a y define an e x p a n d e d state vector,
a n d v's
y
s u c h t h a t all t h e
us
are included:
ζ = lu\ Im
=
X
(112)
l]
\n χ 1/
\vl
T h e differential e q u a t i o n s for ζ a r e
6x6
ζ
=
6
X
m
dF
dF
ex
au
(m + η) x 6
0
(w + η)
6
X
n"
0
(113)
(m + n)
X
0
T h e s o l u t i o n of (113) m a y b e w r i t t e n
6x6
6 χ m
6
*(ί)
=
x n
0
φ(ί; to)
(m + η) X 6
(m + η) X (m + n)
0
7
* ( ί 0 ) = Φ(<; ί 0 ) * ( ί 0 )
(114)
W i t h t h i s e x p a n d e d d e f i n i t i o n of s t a t e o n e m a y i n c l u d e a s m a n y a d d i t i o n a l
u n k n o w n s (in p r i n c i p l e ) as h e desires. W e m i g h t n o t e in p a s s i n g t h a t
w e c o u l d h a v e c o n s i d e r e d u a n d ν a s s o l u t i o n s of a n y a u x i l i a r y s e t of
differential
equations.
The
(m + ri) X (m + ή)
would be replaced by a time-dependent
null
matrix
of
(or constant) m a t r i x in
(113)
this
case.
T h e s o l u t i o n of t h e p r o b l e m is t h u s t h e s a m e a s w a s g i v e n p r e v i o u s l y ;
t h a t is,
2» =
F7 η
|u"j
= Î + Ρ,Η/ίΗ,Ρ,Η/
Pz -
P,H/(H,PZH/
+ Q)-\Y
-
Ϋ(Χ,
V, t))
(115)
Ê{t)
= Ê(t0)
+
+
Q)^HZP..
f
M
J
(Jü(*,U,t)|<ft
L <o ι
P , ( f ) = Φ ( ί ; to)Pz(tO)0T(t;
g
t0) + Β
(116)
333
NAVIGATION PROBLEMS
E q u a t i o n s (115) are for t h e i m p r o v e m e n t in e s t i m a t e a n d t h e c o v a r i a n c e
m a t r i x of t h e e r r o r i n e s t i m a t e a s a r e s u l t o f t h e o b s e r v a t i o n
e q u a t i o n s (116) a r e for
updating the estimate and
b e t w e e n t h e o b s e r v a t i o n s . Bz
functions
estimate
is f o r i n c l u s i o n of a n y r a n d o m
forcing
w h i c h h a v e o c c u r r e d i n t h e t i m e i n t e r v a l (t0 —>• t)
derivation
of ( 6 0 ) ] . Φ
is c a l c u l a t e d
by
numerical
[see
integration
v a r i a t i o n a l e q u a t i o n s a l o n g t h e c u r r e n t b e s t e s t i m a t e of X
and
Y,
error in
the
of
the
U.
and
Effects of Unknown Parameters
I t is c l e a r f r o m t h e p r e v i o u s d e r i v a t i o n t h a t n o t h e o r e t i c a l
are
introduced
by
parameter
estimation.
There
may
be
difficulties
some
real
p r a c t i c a l difficulties, s i n c e o n e m a y n o t b e a b l e t o o b t a i n a s o l u t i o n w h i c h
c o n v e r g e s w h e n a l a r g e n u m b e r of u n k n o w n s a r e i n t r o d u c e d . T h i s c a n
h a p p e n w h e n t h e u n k n o w n s (states) are not linearly i n d e p e n d e n t
t h e n u m b e r of s i g n i f i c a n t
the particular
flight
figures
for
retained in numerical calculation
for
u n d e r i n v e s t i g a t i o n . A l s o it is a p p a r e n t t h a t
the
s i z e of t h e d i g i t a l c o m p u t e r
required can be excessive w h e n a
large
n u m b e r of u n k n o w n p a r a m e t e r s a r e a d d e d .
F o r t h e s e r e a s o n s o n e w o u l d l i k e t o i n c l u d e t h e effects of u n k n o w n
parameters
without
in the
sense that
they
actually carrying t h r o u g h
deteriorate
the
estimate
all t h e c a l c u l a t i o n s f o r
of
state,
estimating
t h e m . T h e m a n n e r i n w h i c h t h i s is d o n e f o r t h e t w o t y p e s of u n k n o w n
parameters—equation
described
of m o t i o n a n d m e a s u r e m e n t
of
observables—is
subsequently.
1. E Q U A T I O N - O F - M O T I O N T Y P E OF U N K N O W N
F o r t h i s t y p e of u n k n o w n o n e w o u l d l i k e t o i n c l u d e i n t h e w e i g h t i n g
f a c t o r of t h e o b s e r v a t i o n s t h e i n f l u e n c e of t h e u n k n o w n p a r t of t h e
U
of (107).
Let U =
U0 +
w, w h e r e U 0 is t h e e s t i m a t e of t h e p a r a m e t e r . U 0 w i l l
r e m a i n c o n s t a n t t h r o u g h o u t t h e p r o c e s s of s o l u t i o n . F r o m ( 1 0 9 )
and
( 1 1 6 ) it is c l e a r t h a t t h e o n l y i n f l u e n c e of u w i l l b e d u r i n g t h e p r o p a g a t i o n
of t h e c o v a r i a n c e m a t r i x of e r r o r s i n t h e e s t i m a t e b e t w e e n o b s e r v a t i o n s .
L e t u s define t h e p r o b l e m m a t h e m a t i c a l l y as follows. G i v e n
(1)
E[(x -
(2)
x(t)
(3)
E[(x -
T
x)(x -
x) ]
= φ(ί; t0)x(to)
x)Ur\
=
= P(t0)
+ U(t;t0)u(t0)
C(t0)
(4)
(5)
E(u)=0
T
E(uu )
=
D
334
STANLEY F. SCHMIDT
Find
(ί)
p(t)
= E{[X(t)
-
(2)
C(t)
= E{[x(t)
-
χ(φτ}
= φ(ΐ; t0)x{t0)
+ U(t;
S o l u t i o n : L e t χ = χ — x\
t0) a n d U =
U(t;
T
P(t)
t0)u
t0):
τ
= E[x(t)x (t)]
= φΕ[χ{ί0)χ (ί0)ψ
T
= φΡ(ί0)φ
τ
τ
φΟ(ί0)υ
+
T
C(t) = E[x(t)uT]
= φΟ(ί0)
= Ε[(φχ(ί0)
+
+
UEluu^U
+
T
U
7,
+
UC (t0W
+
T
+ φΕ[χψ0)ηη
+ UE[ux {tQ)^
P(t)
m}
then
x(t)
L e t φ = φ(ί\
T
*(*)][*(') -
UDU
T
ϋη)ηη
UD
A t a n o b s e r v a t i o n t h e c h a n g e i n t h e c o v a r i a n c e m a t r i x Ρ is g i v e n
t h e p r e v i o u s d e r i v a t i o n [ E q . (99)] u n l e s s t h e r e a r e u n k n o w n
by
parameters
i n t h e m e a s u r e m e n t ( w h i c h is c o v e r e d i n t h e f o l l o w i n g s e c t i o n ) .
The
c o r r e l a t i o n f a c t o r C does c h a n g e a t a n o b s e r v a t i o n . T h e n e w c o r r e l a t i o n
f a c t o r , Cn , is f o u n d
E(x -
xn)u
T
by
T
T
= E[(x — χ — PH (HPH
T
Cn = C -
PH (HPH
T
+ Q)-\H(x
+
-
x) +
T
q))u ]
Q^HC
I n s u m m a r y t h e e q u a t i o n s are as follows:
Between observations:
+
Jt(t)
= i(t0)
P(t)
= φΡ(ί0)φ
C(t)
= φΟ(ΐ0)
Î ' f ( * , U o , O *
J
to
At an observation
Xn
τ
+
+
τ
φΟ(ί0)υ
T
T
+ UC (t^
+ UDU
(117)
UD
Y:
T
T
+ Q)-\Y
T
T
+ QY\HP)
= X + PH (HPH
Pn=P
-
Cn = C -
PH (HPH
T
PH (HPH
T
+
-
Y(X,
t))
(118)
Q^HC
A s c a n b e s e e n f r o m ( 1 1 7 ) , c o r r e l a t i o n e x i s t s f o r a n y v a l u e of t i m e o t h e r
t h a n t 0 , e v e n w h e n C(t0)
= 0 . T h i s is a r e s u l t of t h e f a c t t h a t t h e e s t i m a t e
of t h e s t a t e χ is d e p e n d e n t u p o n t h e u n k n o w n p a r a m e t e r s i n t h e e q u a t i o n s
of m o t i o n .
335
NAVIGATION PROBLEMS
2.
U N K N O W N PARAMETERS I N THE MEASUREMENT
A s can b e seen from e q u a t i o n (108), quantities s u c h as m e a s u r e m e n t
b i a s e r r o r s , s t a t i o n l o c a t i o n e r r o r s , e t c . , affect t h e o b s e r v a t i o n . U t i l i z a t i o n
of m e a s u r e m e n t s w i t h t h e s e t y p e s of e r r o r s w o u l d i n f l u e n c e t h e e s t i m a t e
of t h e p o s i t i o n a n d v e l o c i t y . T h e e r r o r s w o u l d c a u s e offsets ( o r b i a s e s )
in the
estimate in some
manner
as observations
are included.
One
q u e s t i o n s h o w t o i n c l u d e t h e effects of s u c h p a r a m e t e r s o n t h e e s t i m a t e
a n d c o v a r i a n c e m a t r i x of t h e e r r o r i n e s t i m a t e w i t h o u t t h e
numerical
c o m p l e x i t y of i n c l u d i n g t h e m a s a d d i t i o n a l s t a t e s .
M a t h e m a t i c a l l y t h e p r o b l e m is d e f i n e d a s f o l l o w s .
(1) χ
=
(2) Ρ =
a n e s t i m a t e of t h e s t a t e .
the
covariance
matrix
of
the
error
in
estimate
=
x) ).
E((x -x)(x(3) C =
Given
T
the correlation between the error in estimate and
unknown
T
p a r a m e t e r s ( C = E[(x — x)v ]).
( 4 ) A n o b s e r v a t i o n y,
where
y = H(t)x
+ G(t)v
+
q(t)
ν = u n k n o w n parameter (constant)
q(t) = r a n d o m e r r o r in m e a s u r e m e n t
( 5 ) T h e c o v a r i a n c e m a t r i x a n d m e a n v a l u e of t h e p a r a m e t e r s v>
E(v)
(6) T h e
covariance
T
= 0
E(w )
matrix and
mean
=
W
v a l u e of t h e
random
errors
w h i c h a r e n o t c o r r e l a t e d w i t h e i t h e r ν o r x,
E(q) = 0
E(qqT) = Q
Find:
( 1 ) A n e w e s t i m a t e of t h e s t a t e
xn
such that
L = E[(x — £n) (x — £n)]
T
is m i n i m i z e d .
(2) T h e
covariance
Pn = E[(x - *„)(* -
matrix
T
of
the
error
in
the
(3) T h e c o r r e l a t i o n b e t w e e n t h e n e w e s t i m a t e a n d
Cn = E[(x
-
χ)νη.
new
estimate,
xn) ].
the
parameters,
336
STANLEY F. SCHMIDT
F o r t h e d e r i v a t i o n of t h e o p t i m u m e s t i m a t e w e s h a l l p r o c e e d i n t h e
m a n n e r given previously in derivation two. Let
x
- xnn
E[(x - xn){x
= E{[(x
=
n
x
+
- x ) - A(y
= E[(x -
(119)
A ( y - y )
- y)][(x
7
x)(x -
x) ]
T
T
x)(y - y) A ]
-
E[{x -
+
q and y =
-
E[A(y
T
x) -
A(y
- y)(x
-
x) ]
- y)(y
-
+ E[A{y
-
y)] }
T
yfÄ^
(120)
Since y =
+
Hx
E[(y - y)(y
Gv
7
- y) ]
= E{[H(x
-
T
E[(x -
x)(y
- j ) ) M ] = E[(x
-
-
T
xf]
+
gv
?][//(* -
+
T
+ GC H
T
T
x) + Gv +
+ GWG
T
] q)
+ Q
Ϋ
r
x)((x
T T
-y)(x
)
+ HCG
= PH A
E[A(y
x
T
= HPH
=
Hic,
T
+ vG
T
+
T
T
q )A ]
T
CG A
T
= (PH A
T
x) H
T
+
T
T
-
T T
+ CG A )
= ΑΗΡ
T
+
AGC
Placing the above relations in (120), we obtain
Pn — Ρ — ΑΗΡ
-
AGC
T
T T
-
PH A
T
-
T
Τ
CG A
+ ΑΫΑ
A s s u m i n g all q u a n t i t i e s t o b e s c a l a r s , w e c a n d i f f e r e n t i a t e ( 1 2 1 )
(121)
with
r e s p e c t t o A a n d s e t t h e r e s u l t a n t t o z e r o . T h e s o l u t i o n of t h e r e s u l t a n t
f o r A w i l l g i v e t h a t v a l u e w h i c h m i n i m i z e s Pn
—HP
T
-
GC
T
-
PH
T
-
CG
,
+ 2ΑΫ
= 0
T h e first a n d t h i r d t e r m s a n d s e c o n d a n d f o u r t h t e r m s a r e i d e n t i c a l f o r
scalars, so
T
T
+ CG )
-2(PH
or
(
=
ρ Γ Η +A
€
= 0
+ 2ΑΫ
η0 γ - ι
(
1 2 2
)
T h e p r o o f t h a t t h i s is t h e s o l u t i o n i n g e n e r a l follows t h e s a m e p a t t e r n as
given before.
Let
T
A = Β + (PH
7
+
CG )?-
1
W e w i s h t o s h o w t h a t t h e t r a c e o f ( 1 2 1 ) is m i n i m i z e d f o r Β i n t h e a b o v e
equation, being equal to zero:
Tr[P„] = Tr{[P -
[PH
T
[B + {PH
T
T
+ CG )7-*\{HP
T
+ CG ][(7)-\HP
+ [(B + (PH
T
7
T
+ GC )
+ CG )?-^?)((B
T
T
+
+
GC )
T
B]
+ Ϋ-\ΗΡ
T
+ GC ))]}
(123)
337
NAVIGATION PROBLEMS
A n u m b e r of t e r m s o f ( 1 2 3 ) c a n c e l , l e a v i n g
T
T r [ P n] = T r [ P -
T
(PH
l
T
+ CG )Y~ {HP
Τ
+ GC )
(124)
+ ΒΫΒ ]
T h e q u a n t i t y Ϋ i n ( 1 2 4 ) is a p o s i t i v e - d e f i n i t e s y m m e t r i c m a t r i x . I t is
t h e c o v a r i a n c e m a t r i x of t h e d i f f e r e n c e b e t w e e n t h e o b s e r v a t i o n s y a n d y>
T
Ϋ. T h e v a l u e of Β w h i c h m i n i m i z e s t h e t r a c e of
E[(y
— y) (y — y) ]
[Pn]
is c l e a r l y e q u a l t o z e r o .
=
I t is a l s o c l e a r f r o m ( 1 2 4 ) a n d t h e a b o v e t h a t
P
n
= P -
T
T
(PH
T
+ CG )Y-\HP
(125)
+ GC )
T h e r e m a i n i n g t e r m w e m u s t find is
T
Cn = E[(x = E[((x
must
T
T
(PH
carry
T
+ CG )Y-\H(x
- x ) - (PH
T
= C One
xn)v ]
+ CG )Y-\HC
out
the
T
- x ) + Gv+
q))v ]
(126)
+ GW)
numerical
calculations
indicated
by
(119),
(122), (125), a n d (126) for e a c h o b s e r v a t i o n in o r d e r t o i n c l u d e p a r a m e t e r
errors in m e a s u r e m e n t in t h e m a n n e r they influence t h e estimate. T h e
equations are summarized in (127):
jtn
= £ + (PH
P
= P -
(PH
Cn = C -
(PH
n
Y = HPH
T
T
T
+ CG )Y-\Y
T
-
T
+ CG )Y-\HP
T
Y{X,
+
T
+ CG )Y-\HC
+ HCG
T
Vy t))
T
GC )
)
+ GW)
T
+ GC H
T
+ GWG
T
+ Q
For updating between measurements we use
Jt(t)
= £(t0)
+
f
£
dt
to
(128)
P(t)=<f>(t;t0)P(t0)<l>T(tyt0)
C(t)
= φ(ΐ;
t0)C(t0)
W e c a n , of c o u r s e , c o m b i n e o u r e s t i m a t e t o i n c l u d e b o t h t h e e q u a t i o n o f - m o t i o n t y p e of u n k n o w n s a n d t h e u n k n o w n p a r a m e t e r s i n m e a s u r e ment.
T h i s r e s u l t is s u m m a r i z e d
T
u n c o r r e l a t e d , i.e., E(uv )
=
0.
b e l o w for t h e case w h e r e
u and ν
are
(
1
2
7
338
STANLEY F.
For updating
between
SCHMIDT
measurements:
X(t)
= X(t0)
+ Ç
Pit)
= φΡ(ί0)4
τ
F(X,V0,t)dt
T
τ
+ υο Μφ
+ *CMU
τ
T
+
UDU
(129)
^ ux
where
= E((x(t)
C
- x(t))u*)
C
ux
At an
= E((x(t)
x{t))vT)
-
vx
observation:
Jtn = Jt + (PH
P
T
= P - (PH
n
T
+ CVXGT)(Y-*)(Y
T
+ CvxG )(Y-i)(HP
V0 , t))
+
T
CVXn = Cvx -
(PHT + CvxG )(?-i)(HCvx
CUXn = Cux -
(PHT +
T
GC VX)
+
GW)
(130)
CvxGT)(Y-i)(HCux)
where
Ϋ = HPH
T
+ HCVXG
T
W = E(vv )
=
T
T
+ GC VXH
T
+ GWG
+ Q
3Y
d Y
H -
9
T
D =
T
E(uu )
/ dx(t)
\
Γ dx(t) π
\ dx(t0)
1
I du(t0) J
An example problem:
I n v i e w of t h e c o m p l e x i t y of ( 1 2 7 ) a n d ( 1 2 8 ) ,
it is of i n t e r e s t t o c a r r y t h r o u g h a n e x a m p l e w h e r e i n t u i t i o n p r o v i d e s
t h e a n s w e r . T h i s will also serve as a c h e c k o n t h e e q u a t i o n s .
A s s u m e t h a t w e w i s h to e s t i m a t e a scalar χ w h i c h o b e y s t h e differential
equation
χ = 0
T h e s o l u t i o n of t h e a b o v e e q u a t i o n is
x(t)
= x(t0)
so
φ(ί; t0) = 1
Assume
E(x(t0))
E[(x(t0)
- x(t0))(x(t0)
= 10 = * ( i 0)
- * ( < 0) ) η = Ρ = 100
)
( 1 3 I
NAVIGATION
339
PROBLEMS
Let the measurement of χ be
(132)
where ν = a constant or bias and
(133)
For the first measurement let
Since no random error is involved in the experiment, it appears obvious
that after treatment of the first observation, all succeeding observations
would not affect the result. Also, the first observation is uncorrelated
with the estimate, so we may process the estimate and error in estimate
by the standard formula. N o t e in (127) that for C = 0, W = 0, the
equations reduce to that given previously in (99). T h i s is in agreement
with our intuition.
T h u s the estimate after the first observation is
(134)
T h e error in the estimate is
(135)
T h e numbers given in (134) and (135) agree with our intuition. After
one observation the estimate is nearly equal to the measurement [since
P(t0) ^> W] and the error in the estimate is practically the same as that
of the biased instrument. Continuing, we now compute Cn :
CΛ ° ~
-
(Ά
llOl/
Applying these numbers back in (127), note that (128) need not be
considered, since φ(ΐ; t0) = 1 ; we find that
( i> f fr
+
C r G)
=
J 0 0 _ 1 0 0
7 = HPH
100
T
= 0
+ HCG
100
T
T
T
+ GC H
100
+ GWG
/ 1 \
T
+ Q
340
STANLEY F. SCHMIDT
Therefore,
T
(PH
7
1
= 0
+ CG )?-
(136)
A s a c o n s e q u e n c e of ( 1 3 6 ) , r e g a r d l e s s of h o w m a n y a d d i t i o n a l m e a s u r e m e n t s a r e m a d e , t h e r e is n o i m p r o v e m e n t i n t h e e s t i m a t e o r e r r o r i n t h e
estimate. W e see, therefore, t h a t o u r e q u a t i o n s agree w i t h o u r intuition.
If t h e r e
is a r a n d o m
error
one
i m p r o v e t h e e s t i m a t e . I f φ = f(t),
can,
by
taking
more
measurements,
then we may expect some
improve-
m e n t as m o r e m e a s u r e m e n t s are m a d e . F o r t h e e x a m p l e cited, h o w e v e r ,
n o t h i n g f u r t h e r is g a i n e d b y m a k i n g m o r e o b s e r v a t i o n s .
ACKNOWLEDGMENTS
A good portion of this chapter w a s derived from notes the author u s e d for instruction
purposes at Santa Clara University. T h e author is indebted to W . S. Bjorkman and
P. J. R o h d e of Philco Corporation, W D L , for their technical h e l p in planning this work.
References
1. R. E . K A L M A N , A n e w approach to linear filtering and prediction problems. J. Banc
Eng. 8 2 , 35 (1960).
2. G . L . S M I T H , S. F . S C H M I D T , and L . A . M C G E E , A p p l i c a t i o n of statistical filter theory
to the optimal estimation of position and velocity o n board of a circumlunar
vehicle. N A S A T e c h . Rept. R - 1 3 5 , 1962.
3. J. D . M C L E A N , S. F . S C H M I D T , and L . A . M C G E E , Optimal filtering and linear prediction applied to a midcourse navigation s y s t e m for the circumlunar mission.
N A S A T e c h . N o t e D - 1 2 0 8 , 1962.
4. S. F . S C H M I D T , State space techniques applied to the design of a space navigation
system. J A C C Conf. Paper, 1962.