Joseph Formulation of Unscented and Quadrature Filters. with Application to Consider States

Transcription

1 Joseph Formulaton of Unscented and Quadrature Flters wth Applcaton to Consder States Renato Zanett 1 The Charles Stark Draper Laboratory, Houston, Texas Kyle J. DeMars 2 Ar Force Research Laboratory, Space Vehcles Drectorate, Krtland AFB, NM I. Introducton The Joseph formula [1] s a general covarance update equaton vald not only for the Kalman gan, but for any lnear unbased estmator under standard Kalman flterng assumptons. The Joseph formula s gven by P + = (I KH)P (I KH) T + KRK T, where I s the dentty matrx, K s the gan, H s the measurement mappng matrx, R s the measurement nose covarance matrx, and P, P + are the pre and post measurement update estmaton error covarance matrces, respectvely. The optmal lnear unbased estmator (equvalently the optmal lnear mnmum mean square error estmator) or Kalman flter often utlzes smplfed covarance update equatons such as P + = (I KH)P and P + = P K(HP H T + R)K T. Whle these alternatve formulatons requre fewer computatons than the Joseph formula, they are only vald when K s chosen as the optmal Kalman gan. In engneerng applcatons, stuatons arse where the optmal Kalman gan s not utlzed and the Joseph formula must be employed to update the estmaton error covarance. Two examples of such a scenaro are underweghtng measurements [2] and consderng states [3]. Even when the optmal gan s used, the Joseph formulaton s stll preferable because t possesses greater numercal accuracy than the smplfed equaton [4]. In ths note, an equvalent to the Joseph formula s derved for lnear estmators but wthout the assumpton of lnear measurements. The formula s appled to the quadrature flter [5] and the unscented flter [6] n 1 Senor Member of the Techncal Staff, Vehcle Dynamcs and Control, El Camno Real, Sute 470, rzanett@draper.com, AIAA Senor Member. 2 Natonal Research Councl Postdoctoral Research Fellow, AIAA Member 1

2 the presence of consder parameters. Schmdt s approach for consder states (Schmdt-Kalman flter) s based on mnmum varance estmaton [7]. Jazwnsk [8] detals the dervaton of the consder Kalman flter n the presence of lnear measurements. For nonlnear measurements, the standard extended Kalman flter approach s used,.e. lnearzaton around the condtonal expectaton s performed. Woodbury and Junkns [3] performed a careful analyss of both the Schmdt-Kalman flter and the consder analyss approach as derved by Tapley et al. [9]. The analyss by Woodbury and Junkns shows the dfferences and the benefts of each of the two approaches. The consder flter has receved consderable attenton n recent years. Woodbury et al. provde new nsght nto consderng parameters n the measurement model [10]. Equvalent formulatons to the consder flter were also studed [11, 12] and appled to Mars entry navgaton [13] and orbt determnaton [14]. Lsano [15] ntroduced an unscented formulaton of the covarance analyss approach by Tapley et al. As descrbed by Woodbury and Junkns that approach s dfferent from that of the Schmdt-Kalman flter. Instead of dervng the consder flter for lnear measurements and then extend the results to nonlnear measurements, ths work derves the general lnear consder optmal flter n the presence of nonlnear measurements. The optmal estmator reduces to the consder flter n the case of lnear measurements and t can be approxmated by lnearzaton around the condtonal mean to obtan the well known consder flter results. However, ths work does not approxmate the general consder flter equatons va lnearzaton around the mean, but through the use of a set of determnstc ponts. Dependng on the scheme chosen for the ponts selecton, the consder quadrature flter and the consder unscented flter are obtaned. II. Generalzed Joseph Formula and Lnear Mnmum Mean Square Consder Flter Gven an n x -dmensonal random vector x, the mean s denoted by m x E{x}, and the covarance s denoted by P xx E{(x m x )(x m x ) T }. Addtonally, gven an n y -dmensonal random vector y, the covarance between x and y s P xy E{(x m x ) (y m y ) T }. Let x be the random vector to be estmated and y be a random vector whose samples are avalable; y s potentally a nonlnear functon of x, as well as other non-estmated random states c, and zero-mean whte 2

3 nose v. Thus, n general, y may be of the form y = h(x, c, v). The lnear estmators of x from y s the famly of functons gven by ˆx = l(y) = Ay+b. The goal s to fnd optmal values for A and b n a mnmum mean square error (MMSE) sense. The optmal coeffcents are denoted wth an astersk. The orthogonalty prncple [16] s vald when the famly of estmaton functons s closed under addton and multplcaton by a scalar. Under ths hypothess the orthogonalty prncple establshes that the optmal estmaton error, e = x (A y + b ), s perpendcular to every possble estmator,.e. { } E [x A y b ] T [Ay + b] = 0 A, b (1) ( { b T E {x A y b } + trace AE y [x A y b ] T}) = 0 A, b. (2) Notng that the orthogonalty condton must be satsfed for all A and b t follows that the coeffcents of b and A n Eq. (2) must be zero E {x A y b } = 0 (3) { E y [x A y b ] T} = O (4) The frst condton mples b = E{x} A E{y} = m x A m y. The lnear MMSE (LMMSE) estmator therefore has the form ˆx = m x + A (y m y ), from whch t s establshed that the estmate s unbased (.e. the estmaton error e = x ˆx s zero mean). Combnng Eq. (3) and Eq. (4) we obtan that for any vector m of approprate dmensons { E (y m) [x A y b ] T} = O m (5) The optmal gan A can be derved by substtutng the optmal b = m x A m y nto Eq. (5) to obtan { E (y m y ) [(x m x ) A (y m y )] T} = O, the optmal matrx s therefore gven by A = P xy P 1 yy, (6) 3

4 where P 1 yy s the matrx nverse of P yy. The LMMSE estmator s therefore gven by ˆx = m x + P xy P 1 yy (y m y ). (7) When ntroducng consder states c, t s necessary to know ther covarance and the correlaton between them and x n order to calculate P xy and P yy. When measurements are lnear and n the absence of consder states y = Hx + v P xy = P xx H T P yy = HP xx H T + R, where R s the covarance of the zero-mean measurement nose v. When substtutng the above equatons n Eq. (7) the famlar Kalman flter emerges. The famly of all lnear unbased estmators s gven by ˆx = m x + A(y m y ) and ther estmaton error has covarance matrx P ee gven by P ee = P xx P xy A T AP T xy + AP yy A T. (8) Eq. (8) s the equvalent to the Joseph formula n the case of nonlnear measurements; the equaton s vald for any value of A, not just the optmal value. When measurements are lnear and n the absence of consder states Eq. (8) reduces to the famlar Joseph formula P ee = (I AH)P xx (I AH) T + ARA T. In the presence of nonlnear measurements and consder states, we defne an augmented state vector z T = [x T c T ], and the lnear consder estmator s gven by ẑ = b + K con y, where the rows of K con correspondng to c are zero. The famly of all lnear consder estmators s closed under addton and multplcaton by a scalar, therefore the orthogonalty prncple holds, and the same steps prevously used n determnng optmal values for b and K con can be repeated to obtan the optmal consder state update m x ẑ = m z + K con (y m y ) = + m c O A (y m y) = m x + A (y m y ) m c, (9) 4

5 where A s defned n Eq. (6). The update of the estmaton error covarance s gven by the generalzed Joseph formula P aug = P zz P zy K T con K con P T zy + P zy P yy P T zy. (10) For lnear measurements and consder states Eqs. (9) and (10) reduce to the consder flter. III. New Consder Flter Algorthms In order to mplement the consder flter that s descrbed by Eqs. (9) and (10), the values of m y, P yy, P xy, and P zy need to be determned. Frst, defne a composte nput, u, to the measurement functon such that u T = [x T c T v T ]. Gven x R nx, c R nc, and v R nv, t follows that u R n where n = n x + n c + n v, and that the measurement functon may be expressed as y = h(u). Recallng that y R ny and gven a value of P uy, t follows that P zy s the upper (n x + n c ) n y block of P uy. Furthermore, P xy s the upper n x n y block of P uy. Therefore, gven the values of m y, P yy, and P uy, the necessary components requred n Eqs. (9) and (10) are avalable. The a pror mean and covarance of the composte nput, m u and P uu, are known, and are gven by m u = m x m c m v P xx P xc P xv and P uu = P cx P cc P cv, P vx P vc P vv where P cx = P T xc, P vx = P T xv, and P vc = P T cv. For zero-mean nose wth covarance R, m v = 0 and P vv = R. Addtonally, when the nose s not correlated wth the state or consder states, P xv = P T vx = O and P cv = P T vc = O. The a pror probablty densty functon of u s denoted as p(u); from t, the mean, covarance, and cross-covarance are obtaned as m y = h(u)p(u)du (11) R n P yy = (h(u) m y )(h(u) m y ) T p(u)du R n P uy = (u m u )(h(u) m y ) T p(u)du. R n 5

6 The covarance terms admt a smplfcaton as P yy = P yy m y m T y and P uy = P uy m u m T y, where P yy = h(u)h T (u)p(u)du (12) R n P uy = uh T (u)p(u)du. (13) R n Therefore, the three ntegral terms of Eqs. (11) (13) need to be evaluated n order to evaluate the consder flter that s descrbed by Eqs. (9) and (10), where each of the three terms has the form I = f(u)p(u)du ; (14) R n the quadrature and unscented flters approxmate these ntegrals by the summaton of a fnte number of determnstc ponts. A. The Consder Quadrature Kalman Flter The quadrature Kalman flter assumes that the a pror densty s Gaussan wth mean m u and covarance P uu,.e. { p(u) = 2πP uu 1/2 exp 1 } 2 (u m u) T P 1 uu(u m u ). The method s based on the Gauss-Hermte quadrature rule, whch s gven by 1 π f(u)e u2 du = m w f(q ), where q and w are the quadrature ponts and weghts, respectvely, and the equalty holds for all polynomals of degree up to 2m 1, where m s the chosen order of the quadrature rule. The quadrature ponts and weghts can be determned va an egenvalue problem as follows. Let J be a symmetrc, trdagonal matrx wth zeros on the man dagonal. The elements of the frst upper and lower dagonals are gven by J,+1 = J +1, = /2 for 1 m 1. Then, the quadrature ponts are the egenvalues of J and the quadrature weghts =1 are gven by w = (v ) 1 2, where (v ) 1 s the frst element of the th normalzed egenvector of J [5, 17]. Consder a scalar random varable, u, whch s dstrbuted accordng to a standard normal dstrbuton (.e. a Gaussan dstrbuton wth zero mean and unt varance). It readly follows by a change of varables that the Gauss-Hermte quadrature rule may be employed as f(u)n (u; 0, 1)du = 1 2π f(u)e u2 /2 du = m w f(κ ), =1 6

7 where κ = 2 q. In the case of an n-dmensonal vector-valued random varable, u, wth zero mean and dentty varance, the unvarate Gauss-Hermte quadrature rule s extended to a multvarate quadrature rule by successve applcaton to the mutually uncorrelated elements of u, yeldng [5] R n f(u )N (u ; 0, I)du = m w n n=1 m m n w 1 f(κ 1,..., κ n ) = λ f(κ ), where κ = [κ 1 κ n ] T and λ = n j=1 w j. Thus, an m-pont unvarate quadrature rule generates an m n -pont quadrature rule for n-dmensonal ntegral evaluatons. Whle the prevous equaton represents an n-dmensonal quadrature, t s not of the form expressed n Eq. (14). Snce an arbtrary multvarate Gaussan dstrbuton s a lnear transformaton from a zero-mean, unt-varance Gaussan dstrbuton, the fnal step s to perform a lnear change of varables, whch yelds 1=1 m n f(u)n (u; m u, P uu )du = λ f(u ), (15) R n where U = m u + S uu κ and S uu s a square-root factor of P uu, such that P uu = S uu S T uu. In order to utlze the quadrature approach for the consder flter, frst select the quadrature rule va the parameter m. Usng the prevously descrbed approach, generate the n-dmensonal quadrature rule, yeldng the m n quadrature ponts κ and assocated weghts λ. Compute the square-root factor S uu from P uu (e.g. =1 =1 usng a Cholesky factorzaton) n order to determne U = m u + S uu κ. Then, the ntegral terms of Eqs. (11) (13) are computed va Eq. (15) as m n m y = λ h(u ) =1 P yy = λ h(u )h T (U ) m n =1 P uy = λ U h T (U ). m n =1 P yy and P uy are then gven by P yy = P yy m y m T y and P uy = P uy m u m T y, from whch P zy and P xy may be extracted. Fnally, use Eqs. (9) and (10) to complete the quadrature consder flter. B. The Consder Unscented Kalman Flter Gven an n-dmensonal random varable u wth mean and covarance, m u and P uu, respectvely, and a nonlnear transformaton y = h(u), 7

8 the unscented Kalman flter, lke the quadrature Kalman flter, employs a set of determnstcally selected ponts n order to compute the mean and covarance of y, as well as the cross-covarance between u and y. Unlke the quadrature Kalman flter, the unscented Kalman flter selects ts ponts based on moment matchng. That s, a set of sgma-ponts, U and assocated weghts, w, are selected so that the moments of y are well approxmated. In general, gven a set of K sgma-ponts, U, and the transformed values, Y = h(u ), the mean, covarance, and cross-covarance are computed as m y = I w (m) Y (16a) P yy = I w (c) Y Y T (16b) P uy = I w (c) U Y T, (16c) wth P yy = P yy m y m T y and P uy = P uy m u m T y, and where the cardnalty of I s K,.e. the number of sgma-ponts. It should be noted that the unscented Kalman flter can employ dfferent weghts for the mean and covarance calculatons. Three methods for constructng the nput sgma-ponts and ther assocated weghts are revewed: the symmetrc, extended symmetrc, and scaled extended symmetrc sgmapont selecton schemes. The symmetrc sgma-pont selecton scheme chooses a set of K = 2n sgma-ponts that are on the th n covarance contour as [18] U = m u + n s = 1,..., n wth assocated weghts of w (m) U = m u n s n = n + 1,..., 2n, = w (c) = 1/2n for = 1,..., 2n, and I = {1,..., 2n}. Here, s represents the th column of the square-root factor of the covarance matrx,.e. s s the th column of S uu, where S uu S T uu = P uu. The symmetrc sgma-pont selecton scheme guarantees matchng of the mean and covarance of the nput dstrbuton. Addtonally, snce the scheme s symmetrc by constructon, the thrd moment for symmetrc dstrbutons s also matched; however, ntroducton of a tunng parameter (and another sgma-pont) enables the sgma-ponts to capture up to 4 th moments. Ths s done by extendng the symmetrc sgmapont set to nclude an addtonal sgma-pont that s the mean, yeldng the extended symmetrc sgma-pont 8

9 selecton scheme as [19] U = m u = 0 U = m u + n + κ s = 1,..., n wth weghts gven by w (m) U = m u n + κ s n = n + 1,..., 2n, = w (c) = κ/(n+κ) for = 0, and w (m) = w (c) = 1/2(n+κ) for = 1,..., 2n, and wth I = {1,..., 2n + 1}. Choosng κ such that n + κ = 3 ensures that the 4 th moment matches [19]. When κ = 3 n < 0, the weght for U 0 becomes negatve, and the calculated covarance can become non-postve semdefnte [20]. Ths effect motvated the development of the scaled unscented transform whch replaces the extended symmetrc sgma-ponts wth the scaled extended symmetrc set of sgma-ponts as U = U 0 + α(u U 0 ) for = 1,..., 2n, where α s a postve scalng parameter such that 0 α 1. Addtonally, snce the weghtng of the mean sgma-pont drectly affects the magntude of the errors n the fourth and hgher order terms for symmetrc pror dstrbutons, a thrd parameter, β s ntroduced to allow for the mnmzaton of hgher order errors n the presence of knowledge of the pror dstrbuton. Thus, the scaled extended symmetrc sgma-pont selecton scheme s gven by [20] U = m u = 0 U = m u + n + λ s = 1,..., n U = m u n + λ s n = n + 1,..., 2n, where λ = α 2 (n + κ) n, and the weghts are gven by w (m) (1 α 2 +β) for = 0, and w (m) = w (c) for the scaled symmetrc sgma-pont selecton scheme. = λ/(n + λ) for = 0, w (c) = λ/(n + λ) + = 1/2(n+λ) for = 1,..., 2n. Addtonally, I = {1,..., 2n+1} In contrast to the extended symmetrc sgma-pont selecton scheme, the scaled extended symmetrc sgma-pont selecton scheme has three tunng parameters: κ, α, and β. Choosng κ 0 guarantees postve semdefnteness of the covarance matrx, so a good default value s κ = 0 [20]. Snce α controls the spread of the sgma-ponts, choosng smaller values of α ensures the avodance of non-local samplng; choosng α = 1, however, produces the same set of sgma-ponts as the extended symmetrc method. Fnally, β s a 9

10 non-negatve parameter that can be used to ncoporate pror dstrbuton knowledge; n the case that the pror s Gaussan, the optmal choce s β = 2 [21]. In order to utlze the unscented approach for the consder flter, frst select the sgma-pont scheme and any assocated tunng parameters. Usng the square-root factor S uu of P uu, determne the sgma-ponts, U, and the assocated weghts, w (m) and w (c), accordng to the chosen scheme. After computng the transformed sgma-ponts va Y = h(u ) for I, the ntegral terms of Eqs. (11) (13) are computed usng Eqs. (16). P yy and P uy are then gven by P yy = P yy m y m T y and P uy = P uy m u m T y, from whch P zy and P xy may be extracted. Fnally, use Eqs. (9) and (10) to complete the unscented consder flter. IV. Conclusons Ths note ntroduces a general covarance update equaton whch s the extenson of the well-known Joseph formula for the nonlnear measurements case. Ths formula can be used n lnear estmators for nonlnear measurements that do not rely on lnearzaton around the current estmate; whch s the assumpton made by the extended Kalman flter. Two estmaton schemes that do not rely on lnearzaton centered the current estmate are the unscented Kalman flter and quadrature flters. The proposed generalzed Joseph formula s necessary to update the estmaton error covarance whenever a non-optmal gan s chosen n the lnear unbased estmator. Varous reasons could dctate the need of a non-optmal gan selecton. One reason for the utlzaton of the generalzed Joseph formula and a non-optmal gan s detaled n ths note: the ncluson of consder states nto the lnear estmator. The resultng algorthms are the extenson of the well-known consder flter to ether the unscented transformaton or the Gauss-Hermte quadrature rule. The classc Joseph formula s known to be more numercally stable than the smplfed optmal covarance update equaton. The proposed generalzed Joseph formula s potentally preferable over the standard covarance update of the unscented and quadrature flters even n the presence of optmal gans for the same reason. References [1] R. S. Bucy and P. D. Joseph, Flterng for Stochastc Processng wth Applcatons to Gudance. Provdence, RI: AMS Chelsea Publshng, 2nd ed., 2005, pp

11 [2] Renato Zanett, Kyle J. DeMars, and Robert H. Bshop. Underweghtng Nonlnear Measurements. Journal of Gudance, Control, and Dynamcs, 33(5): , September October [3] Woodbury, D. and Junkns, J., On the Consder Kalman Flter, Proceedngs of the AIAA Gudance, Navgaton, and Control Conference, August [4] G. J. Berman, Factorzaton Methods for Dscrete Sequental Estmaton, Vol. 128 of Mathematcs n Scences and Engneerng. Academc Press, 1978, pp [5] Arasaratnam, I., Haykn, S., and Ellot, R. J., Dscrete-Tme Nonlnear Flterng Algorthms usng Gauss-Hermte Quadrature, Proceedngs of the IEEE, Vol. 95, No. 5, May 2007, pp [6] S. J. Juler, J. K. Uhlmann, and H. F. Durrant-Whyte, A new method for the nonlnear transformaton of means and covarances n flters and estmators, IEEE Transactons on Automatc Control, vol. 45, no. 3, pp , March [7] Schmdt, S. F., Applcaton of State-Space Methods to Navgaton Problems, Advances n Control Systems, Vol. 3, 1966, pp [8] Jazwnsk, A. H., Stochastc Processes and Flterng Theory, Vol. 64 of Mathematcs n Scences and Engneerng, Academc Press, New York, New York 10003, 1970, p [9] Tapley, B. D., Schutz, B. E., and Born, G. H., Statstcal Orbt Determnaton, Elsever Academc Press, 2004, Chapter 6. [10] Woodbury, D., Majj, M., and Junkns, J., Consderng Measurement Model Parameter Errors n Statc and Dynamc Systems, Proceedngs of the George H. Born Symposum, May [11] Zanett, R., Advanced Navgaton Algorthms for Precson Landng, Ph.D. thess, The Unversty of Texas at Austn, Austn,Texas, December [12] Zanett, R. and Bshop, R. H., Kalman Flters wth Uncompensated Bases, Journal of Gudance, Control, and Dynamcs, Vol. 35, No. 1, January February 2012, pp [13] Zanett, R. and Bshop, R. H., Entry Navgaton Dead-Reckonng Error Analyss: Theoretcal Foundatons of the Dscrete-Tme Case, Proceedngs of the AAS/AIAA Astrodynamcs Specalst Conference held August 19 23, 2007, Macknac Island, Mchgan, Vol. 129 of Advances n the Astronautcal Scences, 2007, pp , AAS [14] Hough, M. E., Orbt Determnaton wth Improved Covarance Fdelty, Includng Sensor Measurement Bases, Journal of Gudance, Control, and Dynamcs, Vol. 34, No. 3, May June 2011, pp [15] Lsano, M. E., Nonlnear Consder Covarance Analyss Usng a Sgma-Pont Flter Formulaton, 29th Annual AAS Gudance and Control Conference, Breckenrdge, Colorado, 4 8 February 2006, AAS [16] Papouls, A., Probablty, Random Varables, and Stochastc Processes, McGraw-Hll, 1st ed., 1965, pp [17] Ito, K. and Xong, K., Gaussan Flters for Nonlnear Flterng Problems, IEEE Transactons on Automatc Con- 11

12 trol, Vol. 45, No. 5, May 2000, pp [18] Uhlmann, J. K., Smultaneous Map Buldng and Localzaton for Real Tme Applcatons, Ph.D. thess, Unversty of Oxford, [19] Juler, S. J. and Uhlmann, J. K., Unscented Flterng and Nonlnear Estmaton, Proceedngs of the IEEE, Vol. 92, March [20] van der Merwe, R., Sgma-Pont Kalman Flters for Probablstc Inference n Dynamc State-Space Models, Ph.D. thess, Oregon Health and Scence Unversty, [21] Juler, S. J., The Scaled Unscented Transformaton, Proceedngs of the Amercan Control Conference, May