Guidance Filter Fundamentals

Transcription

1 Guidance Filer Fundamenals Neil F. Palumbo, Gregg A. Harrison, oss A. Blauwamp, and Jeffrey K. Marquar hen designing missile guidance laws, all of he saes necessary o mechanize he implemenaion are assumed o be direcly available for feedbac o he guidance law and uncorruped by noise. In pracice, however, his is no he case. he separaion heorem saes ha he soluion o his problem separaes ino he opimal deerminisic conroller driven by he oupu of an opimal sae esimaor. hus, his aricle serves as a companion o our oher aricle in his issue, Modern Homing Missile Guidance heory and echniques, wherein opimal guidance laws are discussed and he aforemenioned assumpions hold. Here, we briefly discuss he general nonlinear filering problem and hen urn our focus o he linear and exended Kalman filering approaches; boh are popular filering mehodologies for homing guidance applicaions. INODUCION Our companion aricle in his issue, Modern Homing Missile Guidance heory and echniques, discusses linear-quadraic opimal conrol heory as i is applied o he derivaion of a number of differen homing guidance laws. egardless of he specific srucure of he guidance law e.g., proporional navigaion (PN) versus he opimal guidance law all of he saes necessary o mechanize he implemenaion are assumed o be (direcly) available for feedbac and uncorruped by noise. e refer o his case as he perfec sae informaion problem, and he resuling linear-quadraic opimal conroller is deerminisic. For example, consider he Caresian version of PN, derived in he abovemenioned companion aricle and repeaed below for convenience: u PN ( ) = [ x ( ) x ( ) go. () go Examining Eq., and referring o Fig., he saes of he PN guidance law are x ( ) _ r r, y My 6 JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE ()

2 M = Missile fligh pah angle = arge fligh pah angle = LO angle r M = Missile inerial posiion vecor r = arge inerial posiion vecor v M = Missile velociy vecor v = arge velociy vecor y I /z I Origin (O) a M Missile (M) r M M v M L a M = Missile acceleraion, normal o LO a = arge acceleraion, normal o V L = Lead angle r x = elaive posiion x (r x r Mx ) r y = elaive posiion y (r y r My ) = ange o arge r x v r r y a arge () xi deerminisic conroller driven by he oupu of an opimal sae esimaor. 6 In his aricle, we will inroduce he opimal filering conceps necessary o mee hese needs. In general erms, he purpose of filering is o develop esimaes of cerain saes of he sysem, given a se of noisy measuremens ha conain informaion abou he saes o be esimaed. In many insances, one wans o perform his esimaion process in some ind of opimal fashion. Depending on he assumpions made abou he dynamic behavior of he saes o be esimaed, he saisics of he (noisy) measuremens ha are aen, and how opimaliy is defined, differen ypes of filer srucures (someimes referred o as observers ) and equaions can be developed. In his aricle, Kalman filering is emphasized, bu we firs provide some brief general commens abou opimal filering and he more general (and implemenaionally complex) Bayesian filer. Figure. Planar engagemen geomery. he planar inercep problem is illusraed along wih mos of he angular and Caresian quaniies necessary o derive modern guidance laws. he x axis represens downrange while he y/z axis can represen eiher crossrange or aliude. A fla-earh model is assumed wih an inerial coordinae sysem ha is fixed o he surface of he Earh. he posiions of he missile (M) and arge () are shown wih respec o he origin (O) of he coordinae sysem. Differeniaion of he argemissile relaive posiion vecor yields relaive velociy; double differeniaion yields relaive acceleraion. x( ) _ v v y M (ha is, componens of relaive y posiion and relaive velociy perpendicular o he reference x axis shown in Fig. ). ecall from he companion aricle ha he relaive posiion measuremen is really a pseudo-measuremen composed of noisy line-ofsigh (LO) angle and relaive range measuremens. In addiion o relaive posiion, however, his (Caresian) form of he deerminisic PN conroller also requires relaive velociy and ime-o-go, boh of which are no (usually) direcly available quaniies. Hence, in words, he relaive posiion pseudo-measuremen mus be filered o miigae noise effecs, and a relaive velociy sae mus be derived (esimaed) from he noisy relaive posiion pseudo-measuremen (we deal wih how o obain ime-o-go in he companion aricle menioned above). Consequenly, a criical quesion o as is: ill deerminisic linear opimal conrol laws (such as hose derived by using he echniques discussed in our companion aricle in his issue) sill produce opimal resuls given esimaed quaniies derived from noisy measuremens? Forunaely, he separaion heorem saes ha he soluion o his problem separaes ino he opimal BAYEIAN FILEING Bayesian filering is a formulaion of he esimaion problem ha maes no assumpions abou he naure (e.g., linear versus nonlinear) of he dynamic evoluion of he saes o be esimaed, he srucure of he uncerainies involved in he sae evoluion, or he saisics of he noisy measuremens used o derive he sae esimaes. I does assume, however, ha models of he sae evoluion (including uncerainy) and of he measuremen-noise disribuion are available. In he subsequen discussions on filering, discreeime models of he process and measuremens will be he preferred represenaion. Discree-ime processes may arise in one of wo ways: (i) he sequence of evens aes place in discree seps or (ii) he coninuous-ime process of ineres is sampled a discree imes. For our purposes, boh opions come ino play. For example, a radar sysem may provide measuremens a discree (perhaps unequally spaced) inervals. In addiion, he filering algorihm is implemened in a digial compuer, hus imposing he need o sample a coninuous-ime process. hus, we will begin by assuming very general discree-ime models of he following form: x = f ( x, w ) y = c ( x, ). In Eq., x is he sae vecor a (discree) ime, he process noise w is a funcional represenaion of he (assumed) uncerainy in he nowledge of he sae evoluion from ime o ime, y is he vecor of measuremens made a ime, and he () JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE () 6

3 N. F. PALUMBO e al. vecor is a saisical represenaion of he noise ha corrups he measuremen aen a ime. e will revisi coninuous-o-discree model conversion laer. Eq. models how he saes of he sysem are assumed o evolve wih ime. he funcion f is no assumed o have a specific srucure oher han being of closed form. In general, i will be nonlinear. Moreover, no assumpion is made regarding he saisical srucure of he uncerainy involved in he sae evoluion; we assume only ha a reasonably accurae model of i is available. he second saemen in Eq. models how he measuremens are relaed o he saes. Again, no assumpions are made regarding he srucure of c or he saisics of he measuremen noise. uppose ha a ime one has a probabiliy densiy ha describes he nowledge of he sysem sae a ha ime, based on all measuremens up o and including ha a ime. his densiy is referred o as he prior densiy of he sae expressed as p( x Y ) where Y represens all measuremens aen up o and including ha a ime. hen, suppose a new measuremen becomes available a ime. he problem is o updae he probabiliy densiy of he sae, given all measuremens up o and including ha a ime. he updae is accomplished in a propagaion sep and a measuremen-updae sep. he propagaion sep predics forward he probabiliy densiy from ime o via he Chapman Kolmogorov equaion (Eq. ). 7 p( x Y ) Predicion = # p( x x ) p( x Y ) dx () ransiional densiy Prior Eq. propagaes he sae probabiliy densiy funcion from he prior ime o he curren ime. he inegral is aen of he produc of he probabilisic model of he sae evoluion (someimes called he ransiional densiy) and he prior sae densiy. his inegraion is over he mulidimensional sae vecor, which can render i quie challenging. Moreover, in general, no closed-form soluion will exis. he measuremen-updae sep is accomplished by applying Bayes heorem o he predicion shown above; he sep is expressed in Eq. 4: 4 4 Poserior Densiy Lielihood Predicion p( y x ) p( x Y ) p( x Y ) =. # p( y x ) p( x Y ) dx Normalizing Consan (4) Given a measuremen y, he lielihood funcion (see Eq. 4) characerizes he probabiliy of obaining ha value of he measuremen, given a sae x. he lielihood funcion is derived from he sensor-measuremen model. Equaions and 4, when applied recursively, consiue he Bayesian nonlinear filer. he poserior densiy encapsulaes all curren nowledge of he sysem sae and is associaed uncerainy. Given he poserior densiy, opimal esimaors of he sae can be defined. Generally, use of a Bayesian recursive filer paradigm requires a mehodology for esimaing he probabiliy densiies involved ha ofen is nonrivial. ecen research has focused on he use of paricle filering echniques as a way o accomplish his. Paricle filering has been applied o a range of racing problems and, in some insances, has been shown o yield superior performance as compared wih oher filering echniques. For a more deailed discussion of paricle filering echniques, he ineresed reader is referred o ef. 7. KALMAN FILEING For he purpose of missile guidance filering, he more familiar Kalman filer is widely used., 6, 8, 9 he Kalman filer is, in fac, a special case of he Bayesian filer. Lie he Bayesian filer, he Kalman filer (i) requires models of he sae evoluion and he relaionship beween saes and measuremens and (ii) is a wo-sep recursive process (i.e., firs predic he sae evoluion forward in ime, hen updae he esimae wih he measuremens). However, Kalman revealed ha a closed-form recursion for soluion of he filering problem could be obained if he following wo assumpions were made: (i) he dynamics and measuremen equaions are linear and (ii) he process and measuremen-noise sequences are addiive, whie, and Gaussian-disribued. Gaussian disribuions are described raher simply wih only wo parameers: heir mean value and heir covariance marix. he Kalman filer produces a mean value of he sae esimae and he covariance marix of he sae esimaion error. he mean value provides he opimal esimae of he saes. As menioned above, discree-ime models of he process and measuremens will be he preferred represenaion when one considers Kalman filering applicaions. In many insances, his preference will necessiae he represenaion of an available coninuous-ime model of he dynamic sysem by a discree-ime equivalen. For ha imporan reason, in Box we review how his process is applied. In Box, we provide a specific example of how one can discreize a consan-velociy coninuousime model based on he resuls of Box. 6 JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE ()

4 GUIDANCE FILE FUNDAMENAL BOX. DEVELOPMEN OF A DICEE-IME EQUIVALEN MODEL e sar wih a linear coninuous-ime represenaion of a sochasic dynamic sysem, as shown in Eq. 5: xo ( ) = A( ) x( ) B( ) u( ) w( ) y( ) = Cx( ) ( ). In his model, xd n is he sae vecor, ud m is he (deerminisic) conrol vecor (e.g., guidance command applied o he missile conrol sysem a ime ), yd p is he measuremen vecor, wd n and d p are vecor whie-noise processes (wih assumed zero cross-correlaion), and represens ime. Marices A, B, and C are compaibly dimensioned so as o suppor he vecor-marix operaions in Eq. 5. he whie-noise processes are assumed o have covariance marices as given in Eq. 6: E[ w( ) w( ) = Q ( ) (6) E[ ( ) ( ) = ( ). In Eq. 6, E( ) represens he expecaion operaor defined as E( x ) = # xp( x) dx, where p(x) is he probabiliy densiy of x. Above, noe ha he coninuous Dirac dela funcion has he propery ha # f( ) ( ) d = f( ), for any f( ), coninuous a. Nex, we consider samples of he coninuous-ime dynamic process described by Eq. 5 a he discree imes,,...,, and we use sae-space mehods o wrie he soluion a ime 4, 6, : # # (7) x( ) = (, ) x( ) (, ) B( ) u( ) d (, ) w( ) d u w A In Eq. 7, (, ) e ( = ) represens he sysem sae ransiion marix from ime o, where he marix A( ) exponenial can be expressed as e = / A ( ) /!. =, 5, Noe ha if he dynamic sysem is linear and ime-invarian, hen he sae ransiion marix may be calculaed as (, ) L [ si A = ",, where L { $ } represens he inverse Laplace ransform and I is a compaibly pariioned ideniy marix. hus, using Eq. 7, we wrie he (shorhand) discree-ime represenaion of Eq. 5 as given below: x = x u w y = C x. In Eq. 8, he represenaion of he measuremen equaion is wrien direcly as a sampled version of he coninuous-ime counerpar in Eq. 5. In addiion, he discree-ime process and measuremen-noise covariance marices, Q and, respecively, are defined as shown below: E[ ww i = Qd i E[ i = d i (9) E[ w i = 6i,. Here we have used he discree Dirac dela funcion, defined as d o =, d n = for n. hus, as par of he discreizaion process, we also see he relaionships beween he coninuous and discree-ime pairs {Q, Q } and {, }. I can be shown ha given he coninuous-ime process disurbance covariance marix Q and sae ransiion marix, and referring o Eqs. 7 and 9, he discree-ime process disurbance covariance marix Q can be approximaed as given in Eq. 4 : Q. # (, ) Q( ) (, ) d. () o obain an approximaion of he measuremen-noise covariance, we ae he average of he coninuous-ime measuremen over he ime inerval = as shown below 4 : y = [ Cx( ) ( ) d Cx #. ( ) d. # () From Eqs. 6, 9, and, we obain he desired relaionship beween he coninuous-ime measuremen covariance and is discree-ime equivalen : E[ E[ ( ) ( ) d d i = =. # # = () (5) (8) JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE () 6

5 N. F. PALUMBO e al. BOX. DICEIZAION EXAMPLE: CONAN-VELOCIY MODEL Here, we illusrae (wih an example) how one can derive a discree-ime model from he coninuous-ime represenaion. For his illusraive example, we consider he sae-space equaions associaed wih a consan-velociy model driven by Gaussian whie noise () (i.e., he velociy sae is modeled as a einer process 4 ). d r( ) r( ) ; E = ; E ; E ; E ( ) d v( ) v( ) [ [ U A x( ) D r( ) y( ) = [ ; E ( ) Z v( ) C () (Compare he specific srucure above o he general expression in Eq. 5.) In Eq., he process and measuremen-noise saisics are given by he following: E[() =, E[()() = Qd( ), E[() =, E[()() = d( ), and E[()() =. e compue he sae ransiion marix as = L "[ si A,, where we have defined he sample ime as =!. hen, from Eq. 7, we obain he following discree-ime represenaion for his sysem: r r ; v E = ; E ; v E [ x r y = ;. v E [ C (Compare he discree-ime represenaion in his example o he more general one shown in Eq. 8.) Based on he resuls of he previous subsecion, he discree-ime process and measuremen-noise componens in Eq. 4 are given by = # (, ) D( ) d = # [ ( ) d and ( ) d = #, respecively. Consequenly, by using Eqs. and, he discree-ime process and measuremen-covariance marices are compued as (4) # # Q E[ = = E; ; E ( ) d ( ) d E = # ; E[ Qd = > Q H E[ E[ ( u) ( ) dud d = = = =. # # # (5) he Discree-ime Kalman Filer he heory says ha he Kalman filer provides sae esimaes ha have minimum mean square error. 4,, An exhausive reamen and derivaion of he discree-ime Kalman filer is beyond he scope of his aricle. Insead, we shall inroduce he design problem and direcly presen he derivaion resuls. o his end, we noe ha if x is he sae vecor a ime, and x is an esimae of he sae vecor a ime, hen he design problem may be saed as given below: min: subjec o: where: J = race{ E([ x x [ x x Y)} x = x u w ) y = Cx E[ wwi = Qd i * E[ i = d i. E[ w = 6i, i (6) he expressions in Eq. 6 embody he opimal design problem, which is o minimize he mean square esimaion error race { E([ x x [ x x Y)} subjec o he assumed plan dynamics and given a sequence of measuremens up o ime represened by Y = " y, y, f, y,. As previously discussed, he discree-ime Kalman filer (algorihm) is mechanized by employing wo disinc seps: (i) a predicion sep (aen prior o receiving a new measuremen) and (ii) a measuremen-updae sep. As such, we will disinguish a sae esimae ha exiss prior o a measuremen a ime, x () (he a priori esimae) from one consruced afer a measuremen a ime, x ( ) (he poseriori esimae). Moreover, we use he erm P o denoe he covariance of he esimaion error, where P () E[ x x () [ x x () = ) ) ) and P ( E[ x x ( [ x x ( =. In wha follows, we denoe x ( ) as our iniial esimae, where x ( ) = E[ x( ). 64 JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE ()

6 GUIDANCE FILE FUNDAMENAL able. Discree-ime Kalman filer algorihm. ep Descripion Expression Iniializaion ) Z Predicion [ \ Z Correcion [ \ (a) Iniial condiions x ) E x(), P ) E x x ) x x V ) = = 8 V B8 V B (b) ae exrapolaion () ( ) Vx = Vx u (c) Error-covariance exrapolaion () ( ) P = P Q (d) Kalman gain updae () () K P C CP C = 8 B (e) Measuremen updae x V ) x K y C x = V ` V j (f) Error-covariance updae P ) I K C P = Based on his descripion, he discree-ime Kalman filer algorihm is encapsulaed as shown in able. In able, he filer operaional sequence is shown in he order of occurrence. he filer is iniialized as given in sep a. eps b and c are he wo predicion (or exrapolaion) seps; hey are execued a each sample insan. eps d, e, and f are he correcion (or measuremenupdae) seps; hey are brough ino he execuion pah when a new measuremen y becomes available o he filer. Figure illusraes he basic srucure of he linear Kalman filer, based on he equaions and sequence laid ou in able. Example: Missile Guidance ae Esimaion via Linear Discree-ime Kalman Filer y Measuremens v u Deerminisic inpus Kalman (guidance) filer () ˆx K xˆ () C xˆ () z Delay () () P = P Q () K = P C () [C P C () () P = [ I K C P Figure. he bloc diagram of he discree-ime, linear Kalman filer. In our companion aricle in his issue, Modern Homing Missile Guidance heory and echniques, he planar version of augmened proporional navigaion (APN) guidance law, repeaed below for convenience (Eq. 7), requires esimaes of relaive posiion x () r(), relaive velociy x () v(), and arge acceleraion x () a() perpendicular o he argemissile LO in order o develop missile acceleraion commands (see Fig. ). u APN ( ) x ( ) x ( ) = go x ( ) 8 gob (7) go eferring bac o he planar engagemen geomery shown in Fig., consider he following sochasic coninuous-ime model represening he assumed engagemen inemaics in he y (or z) axis: ro ( ) r( ) > vo ( ) H = > H> v( ) H > Hu( ) > H ( ) ao ( ) a( ) (8) r( ) y( ) = > v( ) H ( ). a ( ) In his example, he arge acceleraion sae is driven by whie noise; i is modeled as a einer process. 4 I can be shown ha his model is saisically equivalen o a arge o guidance law maneuver of consan ampliude and random maneuver sar ime. As in he previous discreizaion example, we assume ha he process and measuremen-noise saisics are given by he following relaions: E[v() =, E[v()v() = Qd( ), E[() =, E[()() = d( ), and E[v()() =. If we discreize he coninuousime sysem considered in Eq. 8, we obain he following discree-ime dynamics and associaed discree-ime process and measuremen-noise covariance marices: JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE () 65

7 N. F. PALUMBO e al. r v a r 5 4 V 8 6 = v 4 u, Q = H H H 8 Q a Y x X V r y v, = =,. = > \ H C a X > > > o illusrae he srucure of he linear Kalman filer for he APN esimaion problem, we will develop a bloc diagram of he filer based on he discree-ime model presened above. o help faciliae his, we assume ha is he rae a which measuremens are available and ha he filer runs a his rae (in he general case, his assumpion is no necessary). For his case, he equaions presened in able can be used o express he esimaion equaion in he alernae (and inuiively appealing) form given below: ( ) ( ) (9) x = [ I KC [ x u Ky. () ecall ha, for APN, componens of he sae vecor x are defined o be relaive posiion, x _ r, relaive velociy, x _ v, and arge acceleraion, x _ a, leading o x x x x herefore, using he sysem described by Eq. 9 in he alernae Kalman filer form shown in Eq., we obain he APN esimaion equaions below: r v a ( ) ( ) ( ) V ( K )( ( ) ( ) ( ) r v a u ) Ky ( ) ( ) ( ) ( ) ( ) = K ( r v a u y ) v a u ( ) ( ) ( ) ( ) K( r v a u y ) a X V. () X Figure depics he srucure dicaed by he filering equaions above. eferring o Fig., he maeup of he Kalman gain marix, K, is shown below (see Eq. d in able ): p V K p sr p K = K = > H. p s () r K p p s r X In his expression, is he filer and measuremen sample ime (in seconds), s is he (coninuous-ime) relaive posiion measuremen variance (in pracice, an esimae of he acual variance), and p ij represens he {i, j}h enry of he (symmeric) a priori error-covariance marix P () as given below: P () P () r / p p p = > p p ph. () p p p is recursively compued using Eq. c in able and Eq. 9 o compue he Kalman gain. he a poseriori error-covariance marix, P ( ), is recursively compued by using Eq. f in able and leads o he following srucure: ( K) p ( K) p ( K) p ( ) P = > ( p pk) ( p pk) ( p pk) H. (4) ( p p K ) ( p p K ) ( p p K ) o mechanize his filer, an esimae of he measuremen-noise variance, s r, is required. his parameer is imporan because i direcly affecs he filer bandwidh. here are a number of ways in which one may se or selec his parameer. he concepually simple hing o do is o se he parameer based on nowledge of he sensor characerisics, which may or may no be easy in pracice because he noise variance may change as he engagemen unfolds (depending on he ype of argeing sensor being employed). Anoher, more effecive, approach is o adapively adjus or esimae he measuremen variance in real ime. ee ef. 4 for a more in-deph discussion on his opic. Example: Discree-ime APN Kalman Filer Performance As a simple example, he Kalman filer shown in Fig. was implemened o esimae he laeral moion of a weaving arge. he oal arge simulaion ime was 5 s, and he filer ime sep () was. s. Figure 4 illusraes he resuls for his example problem. he arge laeral acceleraion (shown as a in Fig. ) was modeled as a sinusoidal funcion in he x (laeral) direcion wih a -g ampliude and 5-s period. Moion in he x direcion is a consan velociy of Mach (6.4 f/s). he arge iniial condiions are x = [,, (f) and v = [, 6 (f/s) for posiion and velociy, respecively. he pseudo-measuremen is he laeral posiion (x), which was modeled as rue x plus Gaussian whie noise 66 JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE ()

8 GUIDANCE FILE FUNDAMENAL Kinemaics a s Final miss [y( f ) ensor(s) M M r M Angle noise Measured/ esimaed range hree-sae guidance filer K K K z z u z z rˆ vˆ aˆ Guidance law (APN) go go go a* c Hold a c Commanded missile acceleraion Figure. APN guidance filer. A discree-ime hree-sae Kalman filer is illusraed here, as is is place wihin he guidance loop. he filer sae esimaes are relaive posiion, relaive velociy, and arge acceleraion. hese esimaes are passed o he APN guidance law, which generaes he acceleraion commands necessary o achieve inercep. Noice ha a perfec inercepor response o he acceleraion command is assumed in his simplified feedbac loop. wih saisics N(, s = f) for he low-noise case and N(, s = f) for he high-noise case. he esimaed saes of he filer comprise arge laeral posiion, velociy, and acceleraion. he filer was iniialized by firs collecing four laeral posiion measuremen samples {x M (), x M (), x M (), x M (4)} and assigning he iniial sae values as shown below: x 4 x ( i) ( x ( 4) x ( )) M, M M = / v i=, 4 = v a ( xm ( ) xm ( )) =. (5) As menioned, wo cases are shown in Fig. 4: (lef) a low-noise case wih a measuremen sandard deviaion s = f and (righ) a high-noise case wih measuremen sandard deviaion of s = f. he error-covariance marix was iniialized as P s = f = diag", 5,, for he low-noise case and P s = f = diag",, 5,, for he high-noise case. For each case, he op plo shows he arge posiion as x versus x (ime is implici). rue, measured, and esimaed posiions are shown along wih he s bounds. For he low-noise case, i is difficul o disinguish ruh from measuremen or esimae (given he resoluion of he plo). For he high-noise case, he posiion esimaion error is more obvious. he secondrow plos show he esimaed and measured laeral posi- ion error for each case. he hird-row plos illusrae laeral velociy, and he boom plos show laeral acceleraion. I is clear ha wih he high-noise measuremen, he esimaes deviae from ruh much more as compared o he low-noise case. NONLINEA FILEING VIA HE EXENDED KALMAN FILE he convenional linear Kalman filer produces an opimal sae esimae when he sysem and measuremen equaions are linear (see Eq. 5). In many filering problems, however, one or boh of hese equaions are nonlinear, as previously illusraed in Eq.. In paricular, his nonlineariy can be he case for he missile guidance filering problem. he sandard way in which his issue of nonlineariy is reaed is via he exended Kalman filer (EKF). In he EKF framewor, he sysem and measuremen equaions are linearized abou he curren sae esimaes of he filer. he linearized sysem of equaions hen is used o compue he (insananeous) Kalman gain sequence (including he a priori and a poseriori error covariances). However, sae propagaion is carried ou by using he nonlinear equaions. his on-he-fly linearizaion approach implies ha he EKF gain sequence will depend on he paricular series of (noisy) measuremens as he engagemen unfolds raher JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE () 67

9 N. F. PALUMBO e al. x (f) x posiion error (f) x velociy (f/s) x acceleraion (f/s ), x (f) = f Esimaed, measured posiion error vs. ime x x posiion Velociy 4 Acceleraion ime (s) Esimae ruh Measured bounds x x posiion x (f) Velociy Acceleraion = f Esimaed, measured posiion error vs. ime 4 5 ime (s) Figure 4. APN Kalman filer resuls. A planar linear Kalman filer is applied o esimae he posiion, velociy, and acceleraion of a arge ha is maneuvering (acceleraing) perpendicular o he x coordinae. he filer aes a posiion measuremen in he x direcion. he (sensor) noise on he laeral posiion measuremen was modeled as rue x plus zero-mean Gaussian whie noise wih sandard deviaion σ. he arge maneuver is modeled as a sinusoid wih a -g magniude and a period of 5 s. arge moion in he x direcion is consan, wih a sea-level velociy of Mach (~6.4 f/s). wo cases are shown: (lef) a low-noise measuremen case (σ = f) and (righ) a high-noise case (σ = f). he plos illusrae he rue and esimaed posiion, velociy, and acceleraion of he arge, along wih he σ bounds for he respecive esimae. For each case, he second-row plo shows he errors in he measured and esimaed posiion compared o ruh vs. ime. han be predeermined by he process and measuremen model assumpions (linear Kalman filer). Hence, he EKF may be more prone o filer divergence given a paricularly poor sequence of measuremens. Neverheless, in many insances, he EKF can operae very well and, herefore, is worh consideraion. A complee derivaion of he EKF is beyond he scope of his aricle. (ee efs., 4,, and for more on his opic.) Insead, we inroduce he concep and presen he resuls as a modificaion o he linear Kalman filer compuaions illusraed in able. o sar, consider he nonlinear dynamics and measuremen equaions given below, where he (deerminisic) conrol and he process and measuremen disurbances are all assumed o be inpu-affine: x = f ( x ) b ( x ) u w y = c ( x ). (6) As before, we assume ha he sysem disurbances are zero-mean Gaussian whie-noise sequences wih he following properies: E[ w w i = Q d i, E[ i = d i, and E[ w i = 6 i,. In Eq. 6, f, b, and c are nonlinear vecor-valued funcions of he sae. e noe ha, given he n-dimensional sae vecor x * = [ x *,..., xn * and any vecor-valued funcion of he sae m ( x *) = [ m ( x *),..., m ( x *) n, we will denoe he Jacobian marix M as shown: m ( x* ) m ( x* ) V m ( x * x g x n ) M _ = h j h. (7) x m ( x* ) m ( x* n ) n x g x n X Consequenly, we can modify he able linear Kalman filer calculaions o implemen he sequence of EKF equaions (able ). Noice ha he sep sequence is idenical o he linear Kalman filer. However, unlie he linear Kalman filer, he EKF is no an opimal esimaor. Moreover, because he filer uses is (insananeous) sae esimaes o linearize he sae equaions on he fly, he filer may quicly diverge if he esimaion error becomes oo grea or if he process is modeled incorrecly. Neverheless, he EKF is he sandard in many navigaion and GP applicaions. he ineresed reader is referred o efs. 4 and 8 for some addiional discussion on his opic. CLOING EMAK In our companion aricle in his issue, Modern Homing Missile Guidance heory and echniques, a number of opimal guidance laws were derived and discussed. In each case, i was assumed ha all of he saes necessary o mechanize he implemenaion (e.g., relaive posiion, relaive velociy, arge acceleraion) were direcly available for feedbac and uncorruped by noise (referred o as he perfec sae informaion problem). In pracice, his generally is no he case. In his aricle, we poined o he separaion heorem ha saes ha an opimal soluion o his problem separaes ino he opi- 68 JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE ()

10 GUIDANCE FILE FUNDAMENAL able. Discree-ime EKF algorihm. ep Descripion Expression (a) Iniial condiions x ) E x( ), P ) E x x ) x x V ) = = 8 V B8 V B () (b) ae exrapolaion Vx f x( ) b x( ) = ` u j ` j (c) (d) Error-covariance exrapolaion Kalman gain updae ( ) ( ) () f ( Vx ) ( ) f ( Vx ) P = = GP = G Q x x () () () () c ( V x ) c ( V x ) () c ( V x ) K = P = G f= G P = G x x p x (e) Measuremen updae x V ) x = V K 8y c ( x ) B (f) Error-covariance updae () ( ) c ( V x ) () P = ei K = Go P x mal deerminisic conroller driven by he oupu of an opimal sae esimaor. hus, we focused here on a discussion of opimal filering echniques relevan for applicaion o missile guidance; his is he process of aing raw (argeing, inerial, and possibly oher) sensor daa as inpus and esimaing he necessary signals (esimaes of relaive posiion, relaive velociy, arge acceleraion, ec.) upon which he guidance law operaes. Moreover, we focused primarily on (by far) he mos popular of hese, he discree-ime Kalman filer. e emphasized he fac ha he Kalman filer shares wo salien characerisics wih he more general Bayesian filer, namely, (i) models of he sae dynamics and he relaionship beween saes and measuremens are needed o develop he filer and (ii) a wo-sep recursive process is followed (predicion and measuremen updae) o esimae he saes of he sysem. However, one big advanage of he Kalman filer (as compared o general nonlinear filering conceps) is ha a closedform recursion for soluion of he filering problem is obained if wo condiions are me: (i) he dynamics and measuremen equaions are linear and (ii) he process and measuremen-noise sequences are addiive, whie, and Gaussian-disribued. Moreover, because discreeime models of he process and measuremens are he preferred represenaion when one considers Kalman filering applicaions, we also discussed (and illusraed) how one can discreize a coninuous-ime sysem for digial implemenaion. As par of he discreizaion process, we poined ou he necessiy o deermine he relaionships beween he coninuous and discree-ime versions of he process covariance marix {Q, Q } and he measuremen-covariance marix {, }. easonable approximaions of hese relaionships were given ha are appropriae for many applicaions. Finally, we recognize ha mos real-world dynamic sysems are nonlinear. As such, he applicaion of linear Kalman filering mehods firs requires he designer o linearize (i.e., approximae) he nonlinear sysem such ha he Kalman filer is applicable. he EKF is an inuiively appealing heurisic approach o acling he nonlinear filering problem, one ha ofen wors well in pracice when uned properly. However, unlie is linear counerpar, he EKF is no an opimal esimaor. Moreover, care mus be aen when using an EKF because he approach is based on linearizing he sae dynamics and oupu funcions abou he curren sae esimae and hen propagaing an approximaion of he condiional expecaion and covariance forward. hus, if he iniial esimae of he sae is wrong, or if he process is modeled incorrecly, he EKF filer may quicly diverge. EFEENCE Ahans, M., and Falb, P. L., Opimal Conrol: An Inroducion o he heory and Is Applicaions, McGraw-Hill, New Yor (966). Basar,., and Bernhard, P., H-Infiniy Opimal Conrol and elaed Minimax Design Problems, Birhäuser, Boson (995). Bar-halom, Y., Li, X.., and Kirubarajan,., Esimaion wih Applicaions o racing and Navigaion, John iley and ons, New Yor (). 4 Brown,. G., and Hwang, P. Y. C., Inroducion o andom ignals and Applied Kalman Filering, nd Ed., John iley and ons, New Yor (99). 5 Bryson, A. E., and Ho, Y.-C., Applied Opimal Conrol, Hemisphere Publishing Corp., ashingon, DC (975) 6 Lewis, F. L., and yrmos, V. L., Opimal Conrol, nd Ed., John iley and ons, New Yor (995). 7 isic, B., Arulampalam,., and Gordon, N., Beyond he Kalman Filer: Paricle Filers for racing Applicaions, Arech House, Norwood, MA (4). JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE () 69

11 N. F. PALUMBO e al. 8 iouris, G. M., An Engineering Approach o Opimal Conrol and Esimaion heory, John iley and ons, New Yor (996). 9 Zarchan, P., and Musoff, H., Fundamenals of Kalman Filering: A Pracical Approach, American Insiue of Aeronauics and Asronauics, eson, VA (). Franlin, G. F., Powel, J. D., and orman, M. L., Digial Conrol of Dynamic ysems, Chap., Addison-esley, eading, MA (June 99). Chui, C. K., and Chen, G., Kalman Filering wih eal-ime Applicaions, rd Ed., pringer, New Yor (999). Grewal, M.., and Andrews, A. P., Kalman Filering heory and Pracice, Prenice Hall, Englewood Cliffs, NJ (99). Zames, G., Feedbac and Opimal ensiiviy: Model eference ransformaions, Muliplicaive eminorms, and Approximae Inverses, IEEE rans. Auom. Conrol 6, (98). 4 Osborne,.., and Bar-halom, Y., adar Measuremen Noise Variance Esimaion wih arges of Opporuniy, in Proc. 6 IEEE/AIAA Aerospace Conf., Big y, M (Mar 6). he Auhors Neil F. Palumbo is a member of APL s Principal Professional aff and is he Group upervisor of he Guidance, Navigaion, and Conrol Group wihin he Air and Missile Defense Deparmen (AMDD). He joined APL in 99 afer having received a Ph.D. in elecrical engineering from emple Universiy ha same year. His ineress include conrol and esimaion heory, faul-oleran resrucurable conrol sysems, and neuro-fuzzy inference sysems. Dr. Palumbo also is a lecurer for he JHU hiing chool s Engineering for Professionals program. He is a member of he Insiue of Elecrical and Elecronics Engineers and he American Insiue of Aeronauics and Asronauics. Gregg A. Harrison is a enior Professional aff engineer in he Guidance, Navigaion, and Conrol Group of AMDD a APL. He holds B.. and M.. degrees in mahemaics from he Universiy of California, iverside, an M..E.E. (aerospace conrols emphasis) from he Universiy of ouhern California, and an M..E.E. (conrols and signal processing emphases) from he Johns Hopins Universiy. He has more han 5 years of experience woring in he aerospace indusry, primarily on missile sysem and spacecraf programs. Mr. Harrison has exensive experise in missile guidance, navigaion, and conrol; saellie aiude conrol; advanced filering echniques; and resource opimizaion algorihms. He is a senior member of he American Insiue of Aeronauics and Asronauics. oss A. Blauwamp received a B..E. degree from Calvin College in 99 and an M..E. degree from he Universiy of Illinois in 996; boh degrees are in elecrical engineering. He is pursuing a Ph.D. from he Universiy of Illinois. Mr. Blauwamp joined APL in May and currenly is he supervisor of he Advanced Conceps and imulaion echniques ecion in he Guidance, Navigaion, and Conrol Group of AMDD. His ineress include dynamic games, nonlinear conrol, and numerical mehods for conrol. He is a member of he Insiue of Elecrical and Elecronics Engineers and he American Insiue of Aeronauics and Asronauics. Jeffrey K. Marquar is a member of APL s Associae Professional aff in AMDD. He joined he Guidance, Navigaion, and Conrol Group in January 8 afer receiving boh his B.. and M.. degrees in aerospace engineering from he Universiy of Maryland a College Par. He currenly is woring on auopilo analysis, simulaion validaion, and guidance law design for he andard Missile. Mr. Marquar is a member of he American Insiue of Aeronauics and Asronauics. For furher informaion on he wor repored here, conac Neil Palumbo. His address is neil.palumbo@ Neil F. Palumbo Gregg A. Harrison oss A. Blauwamp Jeffrey K. Marquar jhuapl.edu. he Johns Hopins APL echnical Diges can be accessed elecronically a 7 JOHN HOPKIN APL ECHNICAL DIGE, VOLUME 9, NUMBE ()