unscented kalman filter revisited - hermite-gauss ... - IEEE Xplore
UNSCENTED KALMAN FILTER REVISITED HERMITE-GAUSS QUADRATURE APPROACH ˇ Jan Stecha and Vladim´ır Havlena Department of Control Engineering Faculty of Electrical Engineering Czech Technical University in Prague Karlovo n´amˇest´ı 13 121 35 Prague, Czech Republic e-mail: [email protected] e-mail: [email protected] Abstract—Kalman filter is a frequently used tool for linear state estimation due to its simplicity and optimality. It can further be used for fusion of information obtained from multiple sensors. Kalman filtering is also often applied to nonlinear systems. As the direct application of bayesian functional recursion is computationally not feasible, approaches commonly taken use either a local approximation - Extended Kalman Filter based on linearization of the non-linear model - or the global one, as in the case of Particle Filters. An approach to the local approximation is the so called Unscented Kalman Filter. It is based on a set of symmetrically distributed sample points used to parameterise the mean and the covariance. Such filter is computationally simple and no linearization step is required. Another approach to selecting the set of sample points based on decorrelation of multivariable random variables and HermiteGauss Quadrature is introduced in this paper. This approach provides an additional justification of the Unscented Kalman Filter development and provides further options to improve the accuracy of the approximation, particularly for polynomial nonlinearities. A detailed comparison of the two approaches is presented in the paper.
the set of symmetrically distributed sample points (so called sigma points), which are used to parametrise the means and covariances. Such filter is simple and no linearization step is required. In this paper, another approach to selecting the set of sample points for obtaining the mean and covariance of the distribution is proposed, which is based on Hermite-Gauss Quadrature [6], [7], [8]. This approach is simpler compared to that using sigma points and exact for polynomial nonlinearities. These two approaches to Unscented Filter are compared in the paper. The paper is organized as follows: in Section II, basic properties of Hermite-Gauss Quadrature are shown; Section III shows how the mean and variance of nonlinear function can be computed using Hermite-Gauss Quadrature. In Section IV, sigma point transformation for Unscented filter is compared with the results obtained by Hermite-Gauss Quadrature. Finally, Section V describes how to use Hermite-Gauss Quadrature in Kalman filter.
I. I NTRODUCTION
II. H ERMITE -G AUSS Q UADRATURE
Whenever the state of the system needs to be estimated from noisy measurements, some kind of state estimator must be used. Minimum mean squared state error estimate for linear systems results in Kalman filter (KF) [1]. It is an excellent tool for information fusion - processing data obtained from different sensors. Kalman filter is the best tool for tracking and estimation due to its simplicity and optimality. However, its application to nonlinear systems is difficult. The most common approach to estimating states of nonlinear systems is to utilize the Extended Kalman filter [2], [3], which is based on the linearization of the non-linear model [4]. It is known that some difficulties with utilization of this approach may occur. First, the linearization requires computing Jacobian matrices, which is nontrivial. Moreover, the resulting filters may be unstable. Kalman filter operates on means and covariances of the probability distribution which may be non-Gaussian. The so called Unscented Kalman filter [5] was developed, based on
The objective of this section is to compute ∫ b v(x)f (x)dx
(1)
a
where v(x) is a priori chosen weighting function and f (x) is some nonlinear function. For our purposes (Hermite Quadra2 ture) the weighting function is chosen as v(x) = e−x , and the interval of integration equals to (a, b) = (−∞, ∞). We want to aproximate such integral by a quadrature formula of the form ∫ b v(x)f (x)dx = A1 f (a1 )+A2 f (a2 )+. . .+Ana f (ana ) (2) a
where Ai are the weighting coefficients, ai ∈< a, b > are the nodes and na is the order of the quadrature formula. Quadrature formula has the algebraic accuracy m, if polynomials up to order m are integrated exactly. Quadrature formula with na nodes has 2na parameters Ai , ai which leads to the
495
na 1 2
algebraic accuracy m = 2na − 1, because a polynomial of order m = 2na − 1 has 2na coefficients. The solution of Gauss Quadrature is given in the following theorem [9]. Theorem: Quadratic formula is Gaussian if the product of roots factors na ∏ (x − ai ) (3) ω(x) =
a2 = 3
4
is an orthogonal polynomial with weight v(x) and when coefficients of quadrature formula equal ∫
b
v(x)li (x)dx,
i = 1, 2, . . . na
5
(4)
a
where li are elementary Lagrange interpolating polynomials. Elementary Lagrange interpolating polynomials li (x) are equal to li (x) =
na ∏
j = 1, j ̸= i
x − aj , ai − aj
Hence,
{ li (aj ) = δi,j =
1, 0,
i = 1, 2, . . . na
(6)
where H0 (x) = 1, H1 (x) = 2x, H2 (x) = 4x2 − 2. These polynomials are called ”physical” Hermite orthogonal polynomials. Nodes ai are roots of Hermite polynomials. Thus, we are able to approximate the integral ∞
A2 =
−x
2
f (x)dx = A1 f (a1 ) + . . . + Ana f (ana ).
A1 = A2 = A3 A1 A2 A3 A4 A1 A2 A3 A4 A5 A1 A2 A3 A4 A5 A6
√ π √2 π 2 √ 2 π √3 π 6 √ π 6
= = 0.0813 = 0.0813 = 0.8049 = 0.8049 = 0.945 = 0.01995 = 0.01995 = 0.3936 = 0.3936 = 0.0045 = 0.0045 = 0.1571 = 0.1571 = 0.7246 = 0.7246
S S Hn+1 (x) = xHnS (x) − nHn−1 (x),
(7)
If the function f (x) is polynomial of order m ≤ 2na − 1, then the result is exact. The weights Ai can be computed by (4) or by the formula √ 2n−1 n! π Ai = 2 (8) n [Hn−1 (ai )]2 In the following table there are nodes ai and coefficients Ai for different orders of approximation na .
(9)
where H0S (x) = 1, H1S (x) = x, H2S (x) = x2 − 1. The nodes aSi are the roots of Hermite polynomials HnS (x) and the weights ASi are equal to √ n! 2π S Ai = 2 S (10) n [Hn−1 (aSi )]2 Using ”statistical” Hermite orthogonal polynomials the quadrature formula has the form ∫ ∞ n ∑ −x2 f (x)e 2 dx = ASi f (aSi ) (11) −∞
e −∞
√1 2 − √12
Note: Some references (e.g. [4]) use the weighting function −x2 v(x) = e 2 ; related Hermite orthogonal polynomials HnS (x) are called ”statistical”. Such polynomials are defined by the recurrent relation
Hermite orthogonal polynomials on the interval (−∞, ∞) 2 with respect to weighting function v(x) = e−x are defined by the recurrent relation
∫
6
(5)
i=j . i ̸= j
Hn+1 (x) = 2xHn (x) − 2nHn−1 (x),
Ai √ π A1 =
a1 = 0√ a2 = 32 √ a3 = − 32 a1 = 1.6507 a2 = −1.6507 a3 = 0.5246 a4 = −0.5246 a1 = 0 a2 = 2.0202 a3 = −2.0202 a4 = 0.9486 a5 = −0.9486 a1 = 2.3506 a2 = −2.3506 a3 = 1.335849 a4 = −1.335849 a5 = 0.436077 a6 = −0.436077
i=1
Ai =
ai 0 a1 =
i=1 x √ 2
Simple transformation s = can be used to relate quadrature of ”physical” and ”statistical” Hermite polynomials ∫ ∞√ √ ∫ ∞ −x2 2 f (x)e 2 dx = 2f ( 2s)e−s ds (12) −∞
−∞
Similar results are obtained using Hermite-Gauss ”physical” Quadrature: ∫ ∞ n ∑ −x2 f (x)e 2 dx = A˜i f (a˜i ) (13) where A˜i =
496
√
−∞
2Ai and a˜i =
√
i=1
2ai .
III. M EAN AND VARIANCE BY H ERMITE -G AUSS Q UADRATURE In this section we use Hermite-Gauss quadrature to compute the mean and the covariance of function f(x) of a random variable x. The mean µf = E {f (x)}, where x is a random variable with probability density function p(x), equals ∫ ∞ µf = f (x)p(x)dx. (14) −∞
{ } is because the variance σf2 = E (f (x) − µf )2 ; the order of the function for which the mean is computed equals 2m = 4 and 2na − 1 = 6 − 1 = 5 > 4. The variance σf2 equals σf2
σf2
A. One dimensional case
x−µ √ 2σ
= v is used, the formula for µf equals ∫ ∞ √ 2 1 µf = √ (16) f ( 2 σv + µ)e−v dv π −∞
If the substitution
Then Hermite-Gauss Quadrature can be used in the form 1 a1 ) + . . . + Ana f (¯ ana )] (17) µf = √ [A1 f (¯ π √ where a ¯i = 2 σai + µ, i = 1, . . . , na and Ai are coefficients of Hermite-Gauss Quadrature and ai are nodes of Hermite orthogonal polynomials of order na . For the order of Hermite polynomial na = 2, the formula for µf = E {f (x)} is simple: 1 [f (µ + σ) + f (µ − σ)] (18) 2 For the order of Hermite polynomial na = 3, the formula for µf = E {f (x)} becomes √ √ ] 1[ µf = 4f (µ) + f (µ + 3σ) + f (µ − 3σ) (19) 6 2 2 {For variance 2σ }f of function f (x), which equals σf = E (f (x) − µf ) , a similar formula for Hermite-Gauss Quadrature can be obtained } 1 { 2 2 σf2 = √ A1 [f (¯ a1 ) − µf ] + . . . Ana [f (¯ ana ) − µf ] π (20) where the nodes a ¯i and coefficients Ai are the same as in the previous case. µf =
Example 1: For f (x) = x2 , formula (18) yields the mean of f (x) as ] 1[ (µ + σ)2 + (µ − σ)2 = µ2 + σ 2 , (21) µf = 2 which is a well-known formula. The same result is obtained if the order of Hermite polynomial equals na = 3. For the variance of random function f (x) = x2 it is necessary to use the order of Hermite polynomial na = 3. It
(22)
After the substitution { of coefficients } Ai and nodes ai , the formula for σf2 = E (f (x) − µf )2 is
First, we will treat the one dimensional case; subsequently, the extension to multi-dimensional cases will be made. If a random variable x is of Gaussian distribution with mean µ and variance σ 2 , then µf equals ∫ ∞ −(x−µx )2 1 µf = f (x) √ e 2σ2 dx (15) 2π σ −∞
3 ]2 1 ∑ [ √ Ai f ( 2 ai + µ) − µf . =√ π i=1
=
1[ 2 4(µ − µf )2 + 6 ] √ √ ((µ + 3σ)2 − µf )2 + ((µ − 3σ)2 − µf )2
= 2σ 4 . √ Example 2: If f (x) = x for x ≥ 0 and f (x) = 0 for x < 0 then, for the order of Hermite polynomial na = 2, formula (18) for the mean yields µf =
] √ 1 [√ µ+σ+ µ−σ 2
If the mean of random variable x equals µ = 10 and variance σ = 3, the mean of the function f (x) equals √ ] 1 [√ 10 + 3 + 10 − 3 = 3.1257 2 √ If na = 3, the mean of f (x) = x under the same conditions equals ] [ √ √ √ √ 1 √ µf = 4 µ + µ + 3σ + µ − 3σ = 3.1232 6 √ . Sometimes approximate formula µf = f (µ) = µ is used, which in our case gives the result µf = 3.1623. The same result is obtained by Hermite-Gauss Quadrature if the order of quadrature equals na = 1. The mean value obtained by Monte Carlo simulation (with 107 samples) gives the result µf = 3.1499. The polynomial approximation is not exact in this case, but the results are satisfactory. µf =
B. Extension to multidimensional cases Let us assume n dimensional random vector x with mean µ and covariance matrix P . We are looking to compute first two moments of multidimensional function f (x) of random vector x. Vector mean µf = E {f (x)} of function f (x) equals ∫ ∞ T −1 1 1 µf = f (x) √ e 2 (x−µ) P (x−µ) dx (23) n (2π) |P | −∞ where |P | is the determinant of covariance matrix P . To simplify the exponent in √ the previous formula, let us √ = P v, where the square root make the substitution x−µ 2 √ √ of covariance matrix P = P ( P )T . The square root of a covariance matrix can √ be obtained by Cholesky factorization. Realize that |P√| =√ |( P )2 | and according the substitution theorem dx = 2| P |dv.
497
After the substitution, the formula for the mean of f (x) becomes √ ∫ ∞ √ √ T 2 f ( 2 P v + µ)e−v v dv µf = √ n (2π) −∞ Using the so called stochastic decorelation technique, vector √ P v can be expressed as √ √ √ √ P v = ( P )1 v1 + ( P )2 v2 + . . . + ( P )n vn (24) √ √ where ( P )i is the ith column of matrix P . According to the Fubinia theorem, a multidimensional integral can be changed to the product of one dimensional integrals of the form ∫ ∞ √ T 1 √ f ( 2P i vi + µ)e−vi vi dvi π −∞ These integrals can be solved by Gauss-Hermite Quadrature and so the function mean µf equals µf
n √ √ 1 ∑[ √ A1 f ( 2( P )i a1 + µ)+ π i=1 ] √ √ . . . + Ana f ( 2( P )i ana + µ) .
=
(25)
For the order of quadrature na = 2, the formula for function mean takes the simple form n ] √ 1 ∑[ √ µf = f (( P )i + µ) + f (−( P )i + µ) ; 2 i=1
(26)
the corresponding formula for na = 3 is given by 1∑ [4f (µ)+ 6 i=1 ] √ √ √ √ f (µ + 3( P )i ) + f (µ − 3( P )i + µ) . n
µf
=
Covariance matrix Pf of vector} function f (x) equals { Pf = E (f (x) − µf )(f (x) − µf )T and can be expressed by integral formula ∫ ∞ (f (x) − µf )(f (x) − µf )T × (27) Pf = −∞
T −1 1 1 √ e 2 −(x−µ) P (x−µ) dx n (2π) |P |
√ √ This relation is simplified using the substitution x−µ = P v, 2 √ where P is again obtained via Cholesky factorization. The resulting formula for covariance matrix Pf is obtained as √
Pf
=
∫ ∞ √ √ 2 √ (f ( 2 P v + µ) − µf ) × (2π)n −∞ √ √ T (f ( 2 P v − µ) − µf )T e−v v dv
This integral can be solved by Gauss-Hermite Quadrature n ) 1 ∑[ ( √ √ A1 f ( 2( P )i a1 + µ) − µf × Pf = √ π i=1 ( √ √ )T f ( 2( P )i a1 + µ) − µf + n ( ) ∑ √ √ . . . + Ana f ( 2( P )i ana + µ) − µf ×
( √ √ )T ] f ( 2( P )i ana + µ) − µf i=1
(28)
For the order of quadrature na = 2 the formula for covariance matrix is obtained as n 1∑ T Pf = si si + wi wiT (29) 2 i=1 √ (f (( P )i + µ) − µf ) and where si = √ wi = (f (−( P )i + µ) − µf ). For order of Hermite polynomial na = 3, the formula for covariance matrix takes the form n 1∑ (30) Pf = 4si sTi + wi wiT + zi ziT 6 i=1 where √ √ si = (f (µ) 3( P )i + µ) − µf ) and − µ ), w = (f ( f i √ √ zi = (f (− 3( P )i + µ) − µf ). IV. S IGMA P OINT T RANSFORMATION FOR U NSCENTED F ILTER Unscented Kalman filter based on the so called reduced sigma points is described in [5]. These points are mapped by a nonlinear transformation to obtain the mean and the covariance of nonlinear function. The algorithm will be described, using the notation introduced in [5]. A symmetrically distributed set of points which match the mean and covariance is obtained as X0 Xi
= =
Xi+n
=
x ˆ √ x ˆ + (n + k)Pi √ x ˆ − (n + k)Pi
and a set of weights is chosen as k 1 , . W0 = Wi = Wn+i = n+k 2(n + k) There, k ∈ R is a tuning parameter and i = 1, . . . , n. Further, x ˆ and P denote the mean and the covariance of x, respectively; Pi is ith column of covariance matrix P . Let the random vectors x and y be related by a known nonlinear function y = f (x). The problem is to calculate the mean and the covariance matrix of vector y and the crosscovariance matrix Pxy , i.e., yˆ = E {y} = E {f (x)} { } Pyy = E (y − yˆ)(y − yˆ)T { } Pxy = E (x − x ˆ)(y − yˆ)T
498
The solution of this problem by Unscented Transformation is based on the selection of symmetrically distributed set of points X0 , Xi and Xi+n and transformed points Yi related to Xi as Yi = f (Xi ), i = 0, 1, . . . , 2n. The solution is given by yˆU T
=
UT Pyy
=
UT Pxy
=
p ∑ i=0 p ∑ i=0 p ∑
Wi Yi Wi (Yi − yˆ)(Yi − yˆ)
T
Wi (Xi − x ˆ)(Yi − yˆ)T
i=0
where p = 2n. The constant k is chosen so that (n + k) = 3. If the notation introduced in the previous sections is used, the set of sigma point vectors is given by a1 a2,i
= µ
√ √ (n + k)( P )i √ √ = µ − (n + k)( P )i
= µ+
a3,i √ where ( P )i is the ith column of square root of covariance matrix and the weights A1 =
k , n+k
A2 = A3 =
1 2(n + k)
The mean of function f (x) using Unscented transformation equals T µU = A1 f (µ) + f n ∑ √ √ √ A2 f (µ + (n + k)( P )i ) + A3 f (µ − (n + k)( P )i ) i=1
and after the substitution the formula simplifies to T µU f
k f (µ) + 3 n √ √ √ √ 1∑ f (µ + 3( P )i ) + f (µ − 3( P )i ) 6 i=1
HGQ is solved in Example 1 obtaining µf = µ2 + σ 2 . Using sigma point transformation the function mean is obtained as ] √ √ k 2 1[ µ + (µ + 3σ)2 + (µ − 3σ)2 µf = 3 6) ( k 1 = + µ2 + σ 2 . 3 3 For k = 2 the same result is obtained as for HGQ, equal to the exact value. For variance σf2 of function f (x) = x2 , the result using HGQ was obtained in Example 1 as σf2 = 2σ 4 . Using sigma point transformation, the variance approximated as [ ]2 σf2 = A1 µ2 − µf + ]2 [ ]2 [ √ √ A2 (µ + 3 σ)2 − µf + A3 (µ − 3 σ)2 and, after the substitution of the weight values, the same result σf2 = 2σ 4 are recovered (again for k = 2). For multidimensional cases, due to the stochastic decorelation technique, the situation is similar. Unscented transformation based on sigma points gives similar results as Hermite-Gauss Quadrature for order of quadrature na = 3. For more complicated functions f (x), it is possible to use a higher order of quadrature resulting in more accurate results. So it can be stated that computation the mean and variance of function f (x) using Hermite-Gauss-Quadrature is more general. V. U TILIZATION OF H ERMITE -G AUSS Q UADRATURE IN K ALMAN FILTER State estimation problem for discrete time nonlinear stochastic system is solved. State equations of the system have the form x(t + 1) = f (x(t), u(t)) + v(t), y(t) = g(x(t), u(t)) + e(t),
=
where n + k = 3 is used. The covariance matrix and crosscovariance matrix computed by sigma points result in PfU T
= A1 ssT +
n ∑
A2 wi wiT + A3 zi ziT
i=1 UT Pxy
=
n ∑
√ √ A2 ( (n + k)( P )i )wiT +
(31)
where v(t) is zero mean white system noise sequence independent of the past and current states. Usually, normality of the noise is considered v(t) ∼ N (0, Rv (t)). Similarly e(t) is also zero mean white measurement noise sequence of known probability density function (p.d.f.), independent of past and present state and system noise. Normality of the measurement noise is also usually assumed, e(t) ∼ N (0, Re (t)). A. Bayesian approach to state estimation
i=1
√ √ A3 (− (n + k)( P )i )ziT √ √ where s = (f (µ) − √µf ), wi = √(f (µ + (n + k)( P )i ) − µf ) and zi = (f (µ − (n + k)( P )i ) − µf ). Good performance of this algorithm is shown in [5]. Example 3: Let us compare the sigma point filter with Hermite-Gauss Quadrature (HGQ) algorithm for one dimensional case with function f (x) = x2 . The function mean using
Assume we observe the inputs u(τ ) and outputs y(τ ) for τ = 1, . . . , t − 1 and our knowledge of the parameters and state of the process based on the data set Dt−1 = {u(1), y(1), . . . , u(t−1), y(t−1)} is described by( a conditional probability density func) tion (c.p.d.f.) p x(t)| Dt−1 . The problem (is how to )update the knowledge described by c.p.d.f. p x(t)| Dt−1 to p (x(t+1)| Dt ) after new input-output data {u(t), y(t)} have been measured. The output equation (31b), defining the c.p.d.f.
499
p (y(t)| x(t), u(t)) and state transition equations (31a) defining the c.p.d.f. p ( x(t+1)| x(t), u(t), y(t)) are given. The solution can be given in the following steps: ( ) 1) C.p.d.f. p x(t)| Dt−1 is given. 2) Using the output model p (y(t)| x(t), u(t)) determine the joint c.p.d.f. ( ) p y(t), x(t)| Dt−1 , u(t) = (32) ( ) p (y(t)| x(t), u(t)) p x(t)| Dt−1 , u(t) Natural condition of control ( ) ( ) p x(t)| Dt−1 , u(t) = p x(t)| Dt−1 is used to complete this step. Natural condition of control expresses the fact that all information about state is in input-output data only and controller which generates the control u(t) has no extra information about the state. 3) Using the output measurement y(t), determine the c.p.d.f. ( ) ( ) p y(t), x(t)| Dt−1 , u(t) t (33) p x(t)| D = p (y(t)| Dt−1 , u(t)) where
( ) p y(t)| Dt−1 , u(t) = ∫ ( ) p y(t), x(t)| Dt−1 , u(t) dx(t)
(34)
Covariances and cross-covariances are given by Pyy (t, t − 1) Pxy (t, t − 1)
x ˆ(t + 1, t) = E(f (x(t), u(t)) + v(t))
Data update step: Let us have state mean estimate x ˆ(t, t−1) and state covariance matrix P (t, t − 1) and new data y(t), u(t) are obtained. Data update step or prediction step of Kalman filter is the following: Predictive state mean equals x ˆ(t, t) = x ˆ(t, t − 1) + Pxy (t, t − 1)Pyy (t, t − 1)−1 × (y(t) − yˆ(t, t − 1)) (37) where yˆ(t, t − 1) = E {g(x(t, t − 1), u(t − 1)} and state covariance matrix P (t, t) = P (t, t − 1)−Pxy (t, t − 1)Pyy (t, t − 1)−1 Pyx (t, t − 1) (38)
(40)
(41)
and state covariance matrix P (t + 1, t)
= E {(f (.) − x ˆ(t, t))((f (.) − x ˆ(t, t))} + Rv .
All means and covariances can be obtained by Hermite-Gauss Quadrature or Unscented transformation as it is shown in the next section. C. Kalman filter by Hermite-Gauss Quadrature Data update step: State x ˆ(t, t) is computed according to (37) where yˆ(t, t − 1) = E {g(x(t, t − 1), u(t − 1)} = n √ √ 1 ∑ √ ˆ(t, t − 1), u(t)) + . . . + A1 g( 2( P )i a1 + x π i=1 √ √ Ana g( 2( P )i ana + x ˆ(t, t − 1), u(t)),
p ( x(t+1)| x(t), u(t), y(t)))
B. Kalman filter Kalman filter operates on the first two moments of random variable x(t). Our aim is to estimate the state mean of the system which can be denoted as x ˆ(t, i) and state covariance matrix P (t, i). It is the state mean and covariance estimate in time t based on the data u(τ ), y(τ ) till time i. Kalman filter consists on two steps:
(39)
Time update step: Let us have the state mean estimate x ˆ(t, t) and we can proceed further to obtain x ˆ(t + 1, t), which is the state mean estimate in time (t+1), based on the same set of data. Such estimate is called time update or model update step, because such update is based only on the model of the system and no new data are given. So state time update mean equals
4) Using the state transition model (31a), for which ( ) p x(t+1)| x(t), Dt = (35) determine the predictive c.p.d.f. ( ) p x(t+1)| Dt = (36) ∫ ( ) ( ) p x(t+1)| x(t), Dt p x(t)| Dt dx(t)
= E {(y(t) − yˆ(t, t − 1))× } (y(t) − yˆ(t, t − 1))T + Re = E {(x(t) − x ˆ(t, t − 1))× } (y(t) − yˆ(t, t − 1))T
and the covariances n 1 ∑ Pyy (t, t − 1) = √ A1 si,1 sTi,1 + . . . Ana si,na sTi,na +Re , π i=1 (42) √ √ where si,j = g( 2( P )i aj + x ˆ(t, t − 1), u(t)) − yˆ(t, t − 1),
1 ∑ Pxy (t, t−1) = √ A1 wi,1 sTi,1 + . . . Ana wi,na sTi,na , π i=1 √ √ where wi,j = f ( 2( P )i aj + x ˆ(t, t − 1), u(t)) − x ˆ(t, t − 1). n
The solution for time update step by Hermite-Gauss Quadrature equals: n √ √ 1 ∑ A1 f ( 2( P )i a1 + x x ˆ(t + 1, t) = √ ˆ(t, t), u(t)) + π i=1 √ √ ˆ(t, t), u(t)), . . . + Ana f ( 2( P )i ana + x and the state covariance n 1 ∑ T T P (t+1, t) = √ A1 zi,1 zi,1 + . . . + Ana zi,na zi,n +Rv , a π i=1 √ √ where zi,j = f ( 2( P )i aj +√x ˆ(t, t),√ u(t)) − x ˆ(t + 1, t). In data update step √ we set √P i = P (t, t − 1)i , while in time update step it is P i = P (t, t)i .
500
D. Kalman filter by Cholesky factors of covariance matrices From√ the previous formulas it is obvious that only Cholesky factors P of covariance matrices are needed. For the data update step according the formula (39) and (42) the covariance matrix Pyy (t, t − 1) can be expressed as
Updating state mean in the data update step is simple, using the formula x ˆ(t, t) = x ˆ(t, t − 1) + Gs (44) where the vector s is obtained as the solution of linear algebraic equation
Pyy (t, t − 1) = N N T
Hs = y(t) − yˆ(t, t − 1).
where matrix N is of the form N
=
[s1,1 , . . . , sn,1 , . . . , sn,na , Im×m ] × √ √ A1 / π .. . √ √ An / π √
where H is a lower triangular matrix. Previous formulas follow from equation for Kalman gain K = GH −1 . In the time update step, state x ˆ(t + 1, t) is obtained in the standard way using the Hermite-Gauss Quadrature. The state covariance matrix is given by P (t + 1, t) = SS T , where the auxiliary matrix S equals
Re
S
−1 , (the It is clear from (37) that for Kalman gain K = Pxy Pyy arguments being omitted) there [ is] Pxy = KPyy . The mutual y covariance matrix Pc = cov equals x [ ] [ ] Pyy Pyx NNT N N T KT Pc = = Pxy Pxx KN N T MMT
where matrix M is the Cholesky factor of the covariance matrix Pxx . The mutual covariance matrix Pc can be expressed in the form Pc = QQT where Q is given by [ ] s1,1 . . . sn,1 . . . sn,na , Im×m 0 Q = × v1,1 . . . vn,1 . . . vn,na , 0 In×n √ √ A1 / π .. . √ √ Ana / π √ Re √0 0 Rv Applying some orthogonal transformation, matrix Q can be transformed to a lower triangular matrix such that Pc = QQT = [ ][ T H 0 H G F 0
GT FT
]
[ =
HH T GH T
HGT T GG + F F T
]
where matrices H and F are lower triangular matrices. It can be proved that according to formula (38) the state covariance matrix P (t, t) equals P (t, t) = F F T
(43)
Comparing two previous matrices reveals that GGT +F F T = M M T , and hence F F T = M M T − GGT . The state covariance matrix P (t, t) which is the result of data update step according to (38) equals P (t, t) = M M T − KN N T K T = M M T − KHH T K T
= [z1,1 , . . . , zn,1 , . . . , zn,na , In×n ] × √ √ A1 / π .. . √ √ An / π √
Cholesky factorization P (t + 1, t) = SS T =
[
Q 0
]
[
QT 0
Rv ]
results in lower triangular matrix Q – the desired Cholesky factor of state covariance matrix QQT = P (t + 1, t). This concludes the algorithm of Kalman filter operating entirely on Cholesky factors of covariance matrices. VI. C ONCLUSION We described in this paper how to use Hermite-Gauss Quadrature for computing the mean and variance of function f (x), where x is a random variable of normal distribution whose mean and variance are known. Kalman filtering involving nonlinear systems results in a non-normal distribution of the random state x. Applying the proposed procedure thus yields an approximation of this random variable distribution by the first two moments. Unscented filter based on sigma points selection was introduced in [5]. In this paper it was shown that this filter gives the same results as that using Hermite-Gauss Quadrature introduced here, if the order of quadrature equals na = 3. Hermite-Gauss-Quadrature can be used also for higher or lower order of quadrature and if the function f (x) is a polynomial function of normal random variable x, the results obtained by Hermite-Gauss Quadrature of sufficient order are exact. While attempts have been made to improve the accuracy of the Unscented filter by sigma point randomization [10], Hermite-Gauss Quadrature provides an alternative approach with a sound mathematical background.
and therefore, G = KH.
501
ACKNOWLEDGMENT This work was supported by the grant P103/11/1353 of the Czech Science Foundation. R EFERENCES [1] R. E. Kalman, “A new approach to linear filtering and prediction problems,” in Trans. ASME J. Basic. Eng., vol. 82, 1960, pp. 34–45. [2] G. C. Goodwin, Adaptive Filtering, Prediction and Control. PrenticeHall, Englewood Cliffs, 1985. [3] F. L. Lewis, Optimal Estimation. John Wiley & Sons Inc., New York, 1986. [4] V. Havlena, Estimation and Filtering (in Czech). Publishing Co. CTU Prague, 2002. [5] S. Julier and J. K. Uhlmann, “Reduced Sigma Point Filters for the Propagation of Means and Covariances Through Nonlinear Transformation,” in Proceedings of the American Control Conference, 2002, pp. 887–892. [6] E. W. Weisstein, “Hermite polynomial,” 2011. [Online]. Available: http://mathworld.wolfram.com/HermitePolynomial.html, [7] S. R. MCReynolds, “Multidimensional Hermite Gauss Quadrature Formulae and their Application to Nonlinear Estimation,” in Proceedings of the Symposium on Nonlinear Estimation Theory an its Application, 1975, pp. 188–191. [8] I. Arasarathan, S. Haykin, and R. Elliott, “Dicrete-time nonlinear filtering algorithms using gauss-hermite quadrature,” in Proceedings if IEEE Trans. ASME J. Basic. Eng., vol. 95, no. 5, 2007, pp. 953–977. [9] W. Gautchi, Orthogonal Polynomials: computation and approximation. Oxford University Press, New York, 2004. [10] J. Dunik, O. Straka, and M. Simandl, “The Development of a Randomised Unscented Kalman Filter,” in Preprints of the 18th IFAC World Congress Milano (Italy), 2011, pp. 8–13.
502
the set of symmetrically distributed sample points (so called sigma points), which are used to parametrise the means and covariances. Such filter is simple and no linearization step is required. In this paper, another approach to selecting the set of sample points for obtaining the mean and covariance of the distribution is proposed, which is based on Hermite-Gauss Quadrature [6], [7], [8]. This approach is simpler compared to that using sigma points and exact for polynomial nonlinearities. These two approaches to Unscented Filter are compared in the paper. The paper is organized as follows: in Section II, basic properties of Hermite-Gauss Quadrature are shown; Section III shows how the mean and variance of nonlinear function can be computed using Hermite-Gauss Quadrature. In Section IV, sigma point transformation for Unscented filter is compared with the results obtained by Hermite-Gauss Quadrature. Finally, Section V describes how to use Hermite-Gauss Quadrature in Kalman filter.
I. I NTRODUCTION
II. H ERMITE -G AUSS Q UADRATURE
Whenever the state of the system needs to be estimated from noisy measurements, some kind of state estimator must be used. Minimum mean squared state error estimate for linear systems results in Kalman filter (KF) [1]. It is an excellent tool for information fusion - processing data obtained from different sensors. Kalman filter is the best tool for tracking and estimation due to its simplicity and optimality. However, its application to nonlinear systems is difficult. The most common approach to estimating states of nonlinear systems is to utilize the Extended Kalman filter [2], [3], which is based on the linearization of the non-linear model [4]. It is known that some difficulties with utilization of this approach may occur. First, the linearization requires computing Jacobian matrices, which is nontrivial. Moreover, the resulting filters may be unstable. Kalman filter operates on means and covariances of the probability distribution which may be non-Gaussian. The so called Unscented Kalman filter [5] was developed, based on
The objective of this section is to compute ∫ b v(x)f (x)dx
(1)
a
where v(x) is a priori chosen weighting function and f (x) is some nonlinear function. For our purposes (Hermite Quadra2 ture) the weighting function is chosen as v(x) = e−x , and the interval of integration equals to (a, b) = (−∞, ∞). We want to aproximate such integral by a quadrature formula of the form ∫ b v(x)f (x)dx = A1 f (a1 )+A2 f (a2 )+. . .+Ana f (ana ) (2) a
where Ai are the weighting coefficients, ai ∈< a, b > are the nodes and na is the order of the quadrature formula. Quadrature formula has the algebraic accuracy m, if polynomials up to order m are integrated exactly. Quadrature formula with na nodes has 2na parameters Ai , ai which leads to the
495
na 1 2
algebraic accuracy m = 2na − 1, because a polynomial of order m = 2na − 1 has 2na coefficients. The solution of Gauss Quadrature is given in the following theorem [9]. Theorem: Quadratic formula is Gaussian if the product of roots factors na ∏ (x − ai ) (3) ω(x) =
a2 = 3
4
is an orthogonal polynomial with weight v(x) and when coefficients of quadrature formula equal ∫
b
v(x)li (x)dx,
i = 1, 2, . . . na
5
(4)
a
where li are elementary Lagrange interpolating polynomials. Elementary Lagrange interpolating polynomials li (x) are equal to li (x) =
na ∏
j = 1, j ̸= i
x − aj , ai − aj
Hence,
{ li (aj ) = δi,j =
1, 0,
i = 1, 2, . . . na
(6)
where H0 (x) = 1, H1 (x) = 2x, H2 (x) = 4x2 − 2. These polynomials are called ”physical” Hermite orthogonal polynomials. Nodes ai are roots of Hermite polynomials. Thus, we are able to approximate the integral ∞
A2 =
−x
2
f (x)dx = A1 f (a1 ) + . . . + Ana f (ana ).
A1 = A2 = A3 A1 A2 A3 A4 A1 A2 A3 A4 A5 A1 A2 A3 A4 A5 A6
√ π √2 π 2 √ 2 π √3 π 6 √ π 6
= = 0.0813 = 0.0813 = 0.8049 = 0.8049 = 0.945 = 0.01995 = 0.01995 = 0.3936 = 0.3936 = 0.0045 = 0.0045 = 0.1571 = 0.1571 = 0.7246 = 0.7246
S S Hn+1 (x) = xHnS (x) − nHn−1 (x),
(7)
If the function f (x) is polynomial of order m ≤ 2na − 1, then the result is exact. The weights Ai can be computed by (4) or by the formula √ 2n−1 n! π Ai = 2 (8) n [Hn−1 (ai )]2 In the following table there are nodes ai and coefficients Ai for different orders of approximation na .
(9)
where H0S (x) = 1, H1S (x) = x, H2S (x) = x2 − 1. The nodes aSi are the roots of Hermite polynomials HnS (x) and the weights ASi are equal to √ n! 2π S Ai = 2 S (10) n [Hn−1 (aSi )]2 Using ”statistical” Hermite orthogonal polynomials the quadrature formula has the form ∫ ∞ n ∑ −x2 f (x)e 2 dx = ASi f (aSi ) (11) −∞
e −∞
√1 2 − √12
Note: Some references (e.g. [4]) use the weighting function −x2 v(x) = e 2 ; related Hermite orthogonal polynomials HnS (x) are called ”statistical”. Such polynomials are defined by the recurrent relation
Hermite orthogonal polynomials on the interval (−∞, ∞) 2 with respect to weighting function v(x) = e−x are defined by the recurrent relation
∫
6
(5)
i=j . i ̸= j
Hn+1 (x) = 2xHn (x) − 2nHn−1 (x),
Ai √ π A1 =
a1 = 0√ a2 = 32 √ a3 = − 32 a1 = 1.6507 a2 = −1.6507 a3 = 0.5246 a4 = −0.5246 a1 = 0 a2 = 2.0202 a3 = −2.0202 a4 = 0.9486 a5 = −0.9486 a1 = 2.3506 a2 = −2.3506 a3 = 1.335849 a4 = −1.335849 a5 = 0.436077 a6 = −0.436077
i=1
Ai =
ai 0 a1 =
i=1 x √ 2
Simple transformation s = can be used to relate quadrature of ”physical” and ”statistical” Hermite polynomials ∫ ∞√ √ ∫ ∞ −x2 2 f (x)e 2 dx = 2f ( 2s)e−s ds (12) −∞
−∞
Similar results are obtained using Hermite-Gauss ”physical” Quadrature: ∫ ∞ n ∑ −x2 f (x)e 2 dx = A˜i f (a˜i ) (13) where A˜i =
496
√
−∞
2Ai and a˜i =
√
i=1
2ai .
III. M EAN AND VARIANCE BY H ERMITE -G AUSS Q UADRATURE In this section we use Hermite-Gauss quadrature to compute the mean and the covariance of function f(x) of a random variable x. The mean µf = E {f (x)}, where x is a random variable with probability density function p(x), equals ∫ ∞ µf = f (x)p(x)dx. (14) −∞
{ } is because the variance σf2 = E (f (x) − µf )2 ; the order of the function for which the mean is computed equals 2m = 4 and 2na − 1 = 6 − 1 = 5 > 4. The variance σf2 equals σf2
σf2
A. One dimensional case
x−µ √ 2σ
= v is used, the formula for µf equals ∫ ∞ √ 2 1 µf = √ (16) f ( 2 σv + µ)e−v dv π −∞
If the substitution
Then Hermite-Gauss Quadrature can be used in the form 1 a1 ) + . . . + Ana f (¯ ana )] (17) µf = √ [A1 f (¯ π √ where a ¯i = 2 σai + µ, i = 1, . . . , na and Ai are coefficients of Hermite-Gauss Quadrature and ai are nodes of Hermite orthogonal polynomials of order na . For the order of Hermite polynomial na = 2, the formula for µf = E {f (x)} is simple: 1 [f (µ + σ) + f (µ − σ)] (18) 2 For the order of Hermite polynomial na = 3, the formula for µf = E {f (x)} becomes √ √ ] 1[ µf = 4f (µ) + f (µ + 3σ) + f (µ − 3σ) (19) 6 2 2 {For variance 2σ }f of function f (x), which equals σf = E (f (x) − µf ) , a similar formula for Hermite-Gauss Quadrature can be obtained } 1 { 2 2 σf2 = √ A1 [f (¯ a1 ) − µf ] + . . . Ana [f (¯ ana ) − µf ] π (20) where the nodes a ¯i and coefficients Ai are the same as in the previous case. µf =
Example 1: For f (x) = x2 , formula (18) yields the mean of f (x) as ] 1[ (µ + σ)2 + (µ − σ)2 = µ2 + σ 2 , (21) µf = 2 which is a well-known formula. The same result is obtained if the order of Hermite polynomial equals na = 3. For the variance of random function f (x) = x2 it is necessary to use the order of Hermite polynomial na = 3. It
(22)
After the substitution { of coefficients } Ai and nodes ai , the formula for σf2 = E (f (x) − µf )2 is
First, we will treat the one dimensional case; subsequently, the extension to multi-dimensional cases will be made. If a random variable x is of Gaussian distribution with mean µ and variance σ 2 , then µf equals ∫ ∞ −(x−µx )2 1 µf = f (x) √ e 2σ2 dx (15) 2π σ −∞
3 ]2 1 ∑ [ √ Ai f ( 2 ai + µ) − µf . =√ π i=1
=
1[ 2 4(µ − µf )2 + 6 ] √ √ ((µ + 3σ)2 − µf )2 + ((µ − 3σ)2 − µf )2
= 2σ 4 . √ Example 2: If f (x) = x for x ≥ 0 and f (x) = 0 for x < 0 then, for the order of Hermite polynomial na = 2, formula (18) for the mean yields µf =
] √ 1 [√ µ+σ+ µ−σ 2
If the mean of random variable x equals µ = 10 and variance σ = 3, the mean of the function f (x) equals √ ] 1 [√ 10 + 3 + 10 − 3 = 3.1257 2 √ If na = 3, the mean of f (x) = x under the same conditions equals ] [ √ √ √ √ 1 √ µf = 4 µ + µ + 3σ + µ − 3σ = 3.1232 6 √ . Sometimes approximate formula µf = f (µ) = µ is used, which in our case gives the result µf = 3.1623. The same result is obtained by Hermite-Gauss Quadrature if the order of quadrature equals na = 1. The mean value obtained by Monte Carlo simulation (with 107 samples) gives the result µf = 3.1499. The polynomial approximation is not exact in this case, but the results are satisfactory. µf =
B. Extension to multidimensional cases Let us assume n dimensional random vector x with mean µ and covariance matrix P . We are looking to compute first two moments of multidimensional function f (x) of random vector x. Vector mean µf = E {f (x)} of function f (x) equals ∫ ∞ T −1 1 1 µf = f (x) √ e 2 (x−µ) P (x−µ) dx (23) n (2π) |P | −∞ where |P | is the determinant of covariance matrix P . To simplify the exponent in √ the previous formula, let us √ = P v, where the square root make the substitution x−µ 2 √ √ of covariance matrix P = P ( P )T . The square root of a covariance matrix can √ be obtained by Cholesky factorization. Realize that |P√| =√ |( P )2 | and according the substitution theorem dx = 2| P |dv.
497
After the substitution, the formula for the mean of f (x) becomes √ ∫ ∞ √ √ T 2 f ( 2 P v + µ)e−v v dv µf = √ n (2π) −∞ Using the so called stochastic decorelation technique, vector √ P v can be expressed as √ √ √ √ P v = ( P )1 v1 + ( P )2 v2 + . . . + ( P )n vn (24) √ √ where ( P )i is the ith column of matrix P . According to the Fubinia theorem, a multidimensional integral can be changed to the product of one dimensional integrals of the form ∫ ∞ √ T 1 √ f ( 2P i vi + µ)e−vi vi dvi π −∞ These integrals can be solved by Gauss-Hermite Quadrature and so the function mean µf equals µf
n √ √ 1 ∑[ √ A1 f ( 2( P )i a1 + µ)+ π i=1 ] √ √ . . . + Ana f ( 2( P )i ana + µ) .
=
(25)
For the order of quadrature na = 2, the formula for function mean takes the simple form n ] √ 1 ∑[ √ µf = f (( P )i + µ) + f (−( P )i + µ) ; 2 i=1
(26)
the corresponding formula for na = 3 is given by 1∑ [4f (µ)+ 6 i=1 ] √ √ √ √ f (µ + 3( P )i ) + f (µ − 3( P )i + µ) . n
µf
=
Covariance matrix Pf of vector} function f (x) equals { Pf = E (f (x) − µf )(f (x) − µf )T and can be expressed by integral formula ∫ ∞ (f (x) − µf )(f (x) − µf )T × (27) Pf = −∞
T −1 1 1 √ e 2 −(x−µ) P (x−µ) dx n (2π) |P |
√ √ This relation is simplified using the substitution x−µ = P v, 2 √ where P is again obtained via Cholesky factorization. The resulting formula for covariance matrix Pf is obtained as √
Pf
=
∫ ∞ √ √ 2 √ (f ( 2 P v + µ) − µf ) × (2π)n −∞ √ √ T (f ( 2 P v − µ) − µf )T e−v v dv
This integral can be solved by Gauss-Hermite Quadrature n ) 1 ∑[ ( √ √ A1 f ( 2( P )i a1 + µ) − µf × Pf = √ π i=1 ( √ √ )T f ( 2( P )i a1 + µ) − µf + n ( ) ∑ √ √ . . . + Ana f ( 2( P )i ana + µ) − µf ×
( √ √ )T ] f ( 2( P )i ana + µ) − µf i=1
(28)
For the order of quadrature na = 2 the formula for covariance matrix is obtained as n 1∑ T Pf = si si + wi wiT (29) 2 i=1 √ (f (( P )i + µ) − µf ) and where si = √ wi = (f (−( P )i + µ) − µf ). For order of Hermite polynomial na = 3, the formula for covariance matrix takes the form n 1∑ (30) Pf = 4si sTi + wi wiT + zi ziT 6 i=1 where √ √ si = (f (µ) 3( P )i + µ) − µf ) and − µ ), w = (f ( f i √ √ zi = (f (− 3( P )i + µ) − µf ). IV. S IGMA P OINT T RANSFORMATION FOR U NSCENTED F ILTER Unscented Kalman filter based on the so called reduced sigma points is described in [5]. These points are mapped by a nonlinear transformation to obtain the mean and the covariance of nonlinear function. The algorithm will be described, using the notation introduced in [5]. A symmetrically distributed set of points which match the mean and covariance is obtained as X0 Xi
= =
Xi+n
=
x ˆ √ x ˆ + (n + k)Pi √ x ˆ − (n + k)Pi
and a set of weights is chosen as k 1 , . W0 = Wi = Wn+i = n+k 2(n + k) There, k ∈ R is a tuning parameter and i = 1, . . . , n. Further, x ˆ and P denote the mean and the covariance of x, respectively; Pi is ith column of covariance matrix P . Let the random vectors x and y be related by a known nonlinear function y = f (x). The problem is to calculate the mean and the covariance matrix of vector y and the crosscovariance matrix Pxy , i.e., yˆ = E {y} = E {f (x)} { } Pyy = E (y − yˆ)(y − yˆ)T { } Pxy = E (x − x ˆ)(y − yˆ)T
498
The solution of this problem by Unscented Transformation is based on the selection of symmetrically distributed set of points X0 , Xi and Xi+n and transformed points Yi related to Xi as Yi = f (Xi ), i = 0, 1, . . . , 2n. The solution is given by yˆU T
=
UT Pyy
=
UT Pxy
=
p ∑ i=0 p ∑ i=0 p ∑
Wi Yi Wi (Yi − yˆ)(Yi − yˆ)
T
Wi (Xi − x ˆ)(Yi − yˆ)T
i=0
where p = 2n. The constant k is chosen so that (n + k) = 3. If the notation introduced in the previous sections is used, the set of sigma point vectors is given by a1 a2,i
= µ
√ √ (n + k)( P )i √ √ = µ − (n + k)( P )i
= µ+
a3,i √ where ( P )i is the ith column of square root of covariance matrix and the weights A1 =
k , n+k
A2 = A3 =
1 2(n + k)
The mean of function f (x) using Unscented transformation equals T µU = A1 f (µ) + f n ∑ √ √ √ A2 f (µ + (n + k)( P )i ) + A3 f (µ − (n + k)( P )i ) i=1
and after the substitution the formula simplifies to T µU f
k f (µ) + 3 n √ √ √ √ 1∑ f (µ + 3( P )i ) + f (µ − 3( P )i ) 6 i=1
HGQ is solved in Example 1 obtaining µf = µ2 + σ 2 . Using sigma point transformation the function mean is obtained as ] √ √ k 2 1[ µ + (µ + 3σ)2 + (µ − 3σ)2 µf = 3 6) ( k 1 = + µ2 + σ 2 . 3 3 For k = 2 the same result is obtained as for HGQ, equal to the exact value. For variance σf2 of function f (x) = x2 , the result using HGQ was obtained in Example 1 as σf2 = 2σ 4 . Using sigma point transformation, the variance approximated as [ ]2 σf2 = A1 µ2 − µf + ]2 [ ]2 [ √ √ A2 (µ + 3 σ)2 − µf + A3 (µ − 3 σ)2 and, after the substitution of the weight values, the same result σf2 = 2σ 4 are recovered (again for k = 2). For multidimensional cases, due to the stochastic decorelation technique, the situation is similar. Unscented transformation based on sigma points gives similar results as Hermite-Gauss Quadrature for order of quadrature na = 3. For more complicated functions f (x), it is possible to use a higher order of quadrature resulting in more accurate results. So it can be stated that computation the mean and variance of function f (x) using Hermite-Gauss-Quadrature is more general. V. U TILIZATION OF H ERMITE -G AUSS Q UADRATURE IN K ALMAN FILTER State estimation problem for discrete time nonlinear stochastic system is solved. State equations of the system have the form x(t + 1) = f (x(t), u(t)) + v(t), y(t) = g(x(t), u(t)) + e(t),
=
where n + k = 3 is used. The covariance matrix and crosscovariance matrix computed by sigma points result in PfU T
= A1 ssT +
n ∑
A2 wi wiT + A3 zi ziT
i=1 UT Pxy
=
n ∑
√ √ A2 ( (n + k)( P )i )wiT +
(31)
where v(t) is zero mean white system noise sequence independent of the past and current states. Usually, normality of the noise is considered v(t) ∼ N (0, Rv (t)). Similarly e(t) is also zero mean white measurement noise sequence of known probability density function (p.d.f.), independent of past and present state and system noise. Normality of the measurement noise is also usually assumed, e(t) ∼ N (0, Re (t)). A. Bayesian approach to state estimation
i=1
√ √ A3 (− (n + k)( P )i )ziT √ √ where s = (f (µ) − √µf ), wi = √(f (µ + (n + k)( P )i ) − µf ) and zi = (f (µ − (n + k)( P )i ) − µf ). Good performance of this algorithm is shown in [5]. Example 3: Let us compare the sigma point filter with Hermite-Gauss Quadrature (HGQ) algorithm for one dimensional case with function f (x) = x2 . The function mean using
Assume we observe the inputs u(τ ) and outputs y(τ ) for τ = 1, . . . , t − 1 and our knowledge of the parameters and state of the process based on the data set Dt−1 = {u(1), y(1), . . . , u(t−1), y(t−1)} is described by( a conditional probability density func) tion (c.p.d.f.) p x(t)| Dt−1 . The problem (is how to )update the knowledge described by c.p.d.f. p x(t)| Dt−1 to p (x(t+1)| Dt ) after new input-output data {u(t), y(t)} have been measured. The output equation (31b), defining the c.p.d.f.
499
p (y(t)| x(t), u(t)) and state transition equations (31a) defining the c.p.d.f. p ( x(t+1)| x(t), u(t), y(t)) are given. The solution can be given in the following steps: ( ) 1) C.p.d.f. p x(t)| Dt−1 is given. 2) Using the output model p (y(t)| x(t), u(t)) determine the joint c.p.d.f. ( ) p y(t), x(t)| Dt−1 , u(t) = (32) ( ) p (y(t)| x(t), u(t)) p x(t)| Dt−1 , u(t) Natural condition of control ( ) ( ) p x(t)| Dt−1 , u(t) = p x(t)| Dt−1 is used to complete this step. Natural condition of control expresses the fact that all information about state is in input-output data only and controller which generates the control u(t) has no extra information about the state. 3) Using the output measurement y(t), determine the c.p.d.f. ( ) ( ) p y(t), x(t)| Dt−1 , u(t) t (33) p x(t)| D = p (y(t)| Dt−1 , u(t)) where
( ) p y(t)| Dt−1 , u(t) = ∫ ( ) p y(t), x(t)| Dt−1 , u(t) dx(t)
(34)
Covariances and cross-covariances are given by Pyy (t, t − 1) Pxy (t, t − 1)
x ˆ(t + 1, t) = E(f (x(t), u(t)) + v(t))
Data update step: Let us have state mean estimate x ˆ(t, t−1) and state covariance matrix P (t, t − 1) and new data y(t), u(t) are obtained. Data update step or prediction step of Kalman filter is the following: Predictive state mean equals x ˆ(t, t) = x ˆ(t, t − 1) + Pxy (t, t − 1)Pyy (t, t − 1)−1 × (y(t) − yˆ(t, t − 1)) (37) where yˆ(t, t − 1) = E {g(x(t, t − 1), u(t − 1)} and state covariance matrix P (t, t) = P (t, t − 1)−Pxy (t, t − 1)Pyy (t, t − 1)−1 Pyx (t, t − 1) (38)
(40)
(41)
and state covariance matrix P (t + 1, t)
= E {(f (.) − x ˆ(t, t))((f (.) − x ˆ(t, t))} + Rv .
All means and covariances can be obtained by Hermite-Gauss Quadrature or Unscented transformation as it is shown in the next section. C. Kalman filter by Hermite-Gauss Quadrature Data update step: State x ˆ(t, t) is computed according to (37) where yˆ(t, t − 1) = E {g(x(t, t − 1), u(t − 1)} = n √ √ 1 ∑ √ ˆ(t, t − 1), u(t)) + . . . + A1 g( 2( P )i a1 + x π i=1 √ √ Ana g( 2( P )i ana + x ˆ(t, t − 1), u(t)),
p ( x(t+1)| x(t), u(t), y(t)))
B. Kalman filter Kalman filter operates on the first two moments of random variable x(t). Our aim is to estimate the state mean of the system which can be denoted as x ˆ(t, i) and state covariance matrix P (t, i). It is the state mean and covariance estimate in time t based on the data u(τ ), y(τ ) till time i. Kalman filter consists on two steps:
(39)
Time update step: Let us have the state mean estimate x ˆ(t, t) and we can proceed further to obtain x ˆ(t + 1, t), which is the state mean estimate in time (t+1), based on the same set of data. Such estimate is called time update or model update step, because such update is based only on the model of the system and no new data are given. So state time update mean equals
4) Using the state transition model (31a), for which ( ) p x(t+1)| x(t), Dt = (35) determine the predictive c.p.d.f. ( ) p x(t+1)| Dt = (36) ∫ ( ) ( ) p x(t+1)| x(t), Dt p x(t)| Dt dx(t)
= E {(y(t) − yˆ(t, t − 1))× } (y(t) − yˆ(t, t − 1))T + Re = E {(x(t) − x ˆ(t, t − 1))× } (y(t) − yˆ(t, t − 1))T
and the covariances n 1 ∑ Pyy (t, t − 1) = √ A1 si,1 sTi,1 + . . . Ana si,na sTi,na +Re , π i=1 (42) √ √ where si,j = g( 2( P )i aj + x ˆ(t, t − 1), u(t)) − yˆ(t, t − 1),
1 ∑ Pxy (t, t−1) = √ A1 wi,1 sTi,1 + . . . Ana wi,na sTi,na , π i=1 √ √ where wi,j = f ( 2( P )i aj + x ˆ(t, t − 1), u(t)) − x ˆ(t, t − 1). n
The solution for time update step by Hermite-Gauss Quadrature equals: n √ √ 1 ∑ A1 f ( 2( P )i a1 + x x ˆ(t + 1, t) = √ ˆ(t, t), u(t)) + π i=1 √ √ ˆ(t, t), u(t)), . . . + Ana f ( 2( P )i ana + x and the state covariance n 1 ∑ T T P (t+1, t) = √ A1 zi,1 zi,1 + . . . + Ana zi,na zi,n +Rv , a π i=1 √ √ where zi,j = f ( 2( P )i aj +√x ˆ(t, t),√ u(t)) − x ˆ(t + 1, t). In data update step √ we set √P i = P (t, t − 1)i , while in time update step it is P i = P (t, t)i .
500
D. Kalman filter by Cholesky factors of covariance matrices From√ the previous formulas it is obvious that only Cholesky factors P of covariance matrices are needed. For the data update step according the formula (39) and (42) the covariance matrix Pyy (t, t − 1) can be expressed as
Updating state mean in the data update step is simple, using the formula x ˆ(t, t) = x ˆ(t, t − 1) + Gs (44) where the vector s is obtained as the solution of linear algebraic equation
Pyy (t, t − 1) = N N T
Hs = y(t) − yˆ(t, t − 1).
where matrix N is of the form N
=
[s1,1 , . . . , sn,1 , . . . , sn,na , Im×m ] × √ √ A1 / π .. . √ √ An / π √
where H is a lower triangular matrix. Previous formulas follow from equation for Kalman gain K = GH −1 . In the time update step, state x ˆ(t + 1, t) is obtained in the standard way using the Hermite-Gauss Quadrature. The state covariance matrix is given by P (t + 1, t) = SS T , where the auxiliary matrix S equals
Re
S
−1 , (the It is clear from (37) that for Kalman gain K = Pxy Pyy arguments being omitted) there [ is] Pxy = KPyy . The mutual y covariance matrix Pc = cov equals x [ ] [ ] Pyy Pyx NNT N N T KT Pc = = Pxy Pxx KN N T MMT
where matrix M is the Cholesky factor of the covariance matrix Pxx . The mutual covariance matrix Pc can be expressed in the form Pc = QQT where Q is given by [ ] s1,1 . . . sn,1 . . . sn,na , Im×m 0 Q = × v1,1 . . . vn,1 . . . vn,na , 0 In×n √ √ A1 / π .. . √ √ Ana / π √ Re √0 0 Rv Applying some orthogonal transformation, matrix Q can be transformed to a lower triangular matrix such that Pc = QQT = [ ][ T H 0 H G F 0
GT FT
]
[ =
HH T GH T
HGT T GG + F F T
]
where matrices H and F are lower triangular matrices. It can be proved that according to formula (38) the state covariance matrix P (t, t) equals P (t, t) = F F T
(43)
Comparing two previous matrices reveals that GGT +F F T = M M T , and hence F F T = M M T − GGT . The state covariance matrix P (t, t) which is the result of data update step according to (38) equals P (t, t) = M M T − KN N T K T = M M T − KHH T K T
= [z1,1 , . . . , zn,1 , . . . , zn,na , In×n ] × √ √ A1 / π .. . √ √ An / π √
Cholesky factorization P (t + 1, t) = SS T =
[
Q 0
]
[
QT 0
Rv ]
results in lower triangular matrix Q – the desired Cholesky factor of state covariance matrix QQT = P (t + 1, t). This concludes the algorithm of Kalman filter operating entirely on Cholesky factors of covariance matrices. VI. C ONCLUSION We described in this paper how to use Hermite-Gauss Quadrature for computing the mean and variance of function f (x), where x is a random variable of normal distribution whose mean and variance are known. Kalman filtering involving nonlinear systems results in a non-normal distribution of the random state x. Applying the proposed procedure thus yields an approximation of this random variable distribution by the first two moments. Unscented filter based on sigma points selection was introduced in [5]. In this paper it was shown that this filter gives the same results as that using Hermite-Gauss Quadrature introduced here, if the order of quadrature equals na = 3. Hermite-Gauss-Quadrature can be used also for higher or lower order of quadrature and if the function f (x) is a polynomial function of normal random variable x, the results obtained by Hermite-Gauss Quadrature of sufficient order are exact. While attempts have been made to improve the accuracy of the Unscented filter by sigma point randomization [10], Hermite-Gauss Quadrature provides an alternative approach with a sound mathematical background.
and therefore, G = KH.
501
ACKNOWLEDGMENT This work was supported by the grant P103/11/1353 of the Czech Science Foundation. R EFERENCES [1] R. E. Kalman, “A new approach to linear filtering and prediction problems,” in Trans. ASME J. Basic. Eng., vol. 82, 1960, pp. 34–45. [2] G. C. Goodwin, Adaptive Filtering, Prediction and Control. PrenticeHall, Englewood Cliffs, 1985. [3] F. L. Lewis, Optimal Estimation. John Wiley & Sons Inc., New York, 1986. [4] V. Havlena, Estimation and Filtering (in Czech). Publishing Co. CTU Prague, 2002. [5] S. Julier and J. K. Uhlmann, “Reduced Sigma Point Filters for the Propagation of Means and Covariances Through Nonlinear Transformation,” in Proceedings of the American Control Conference, 2002, pp. 887–892. [6] E. W. Weisstein, “Hermite polynomial,” 2011. [Online]. Available: http://mathworld.wolfram.com/HermitePolynomial.html, [7] S. R. MCReynolds, “Multidimensional Hermite Gauss Quadrature Formulae and their Application to Nonlinear Estimation,” in Proceedings of the Symposium on Nonlinear Estimation Theory an its Application, 1975, pp. 188–191. [8] I. Arasarathan, S. Haykin, and R. Elliott, “Dicrete-time nonlinear filtering algorithms using gauss-hermite quadrature,” in Proceedings if IEEE Trans. ASME J. Basic. Eng., vol. 95, no. 5, 2007, pp. 953–977. [9] W. Gautchi, Orthogonal Polynomials: computation and approximation. Oxford University Press, New York, 2004. [10] J. Dunik, O. Straka, and M. Simandl, “The Development of a Randomised Unscented Kalman Filter,” in Preprints of the 18th IFAC World Congress Milano (Italy), 2011, pp. 8–13.
502
