2 estimators

1

Estimators

SOLO HERMELIN

Updated: 22.02.09 17.06.14

http://www.solohermelin.com

http://www.solohermelin.com/

2

EstimatorsSOLO

Table of Content

Summary of Discrete Case Kalman FilterExtended Kalman FilterUscented Kalman Filter

Kalman Filter Discrete Case & Colored Measurement Noise

Parameter EstimationHistory

Optimal Parameter EstimateOptimal Weighted Last-Square EstimateRecursive Weighted Least Square Estimate (RWLS)Markov Estimate

Maximum Likelihood Estimate (MLE)

Bayesian Maximum Likelihood Estimate (Maximum Aposterior – MAP Estimate)

The Cramér-Rao Lower Bound on the Variance of the Estimator

Kalman Filter Discrete Case

Properties of the Discrete Kalman Filter ( ) ( ) 01|1~1|1ˆ =++++ kkxkkxE T

(1)(2) Innovation =White Noise for Kalman Filter Gain

EstimatorsSOLO

Table of Content (continue – 1)

Optimal State Estimation in Linear Stationary Systems

Kalman Filter Continuous Time Case

Applications

Multi-sensor Estimate

Target Acceleration Models

Kalman Filter for Filtering Position and Velocity Measurements

α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model

Optimal Filtering

Continuous Filter-Smoother Algorithms

References

End of Estimation Presentation

Review of Probability

Random Variables

Matrices

Inner Product

Signals

4

Estimators

v

( )vxh , z

x

SOLO

Estimate parameters x of a given system, by using measurements z corrupted by noise v.

Parameter is a quantity (scalar or vector-valued) that isusually assumed to be time-invariant. If the parameter does change with time, it is designed as a time-varyingparameter, but its time variation is assumed slow relativeto system states. The estimation is performed on different measurements j = 1,…,k that provide different results z (j) because of the random variables (noises) v (j)

( ) ( )( ) kjjvxjhjz ,,1,, ==

We define the observation (information) vector as: ( ) ( ) ( ) k

j

Tk jzkzzZ 11: ===

We want to find the estimation of x, given the measurements Zk:

( ) ( )kZkxkx ,ˆˆ =

Assuming that the parameters x are observable (defined later) from the measurement, and knowledge of the system h (x,ν) theestimation of x will be done in some sense.

Parameter Estimation

5

Estimators

v

( )vxh , z

x

SOLO

Desirable Properties of Estimators.

( ) ( ) ( )kxZkxEkxE k == ,ˆˆ

Unbiased Estimator1

Consistent or Convergent Estimator2

( ) ( )[ ] ( ) ( )[ ] 00ˆˆProblim =>>−−∞→

εkxkxkxkx T

k

( ) ( )[ ] ( ) ( )[ ] ( ) ( )[ ] ( ) ( )[ ] KkforkxkxkxkxEkxkxkxkxE TT >−−≤−− γγ ˆˆˆˆ

Efficient or Assymptotic Efficient Estimator if for All Unbiased Estimators 3 ( )( )kxγγ ˆ

Sufficient Estimator if it contains all the information in the set of observed values regarding the parameter to be observed.

4 kZ( )kx

Table of Content

6

EstimatorsSOLO

History

The Linear Estimation Theory is credited o Gauss, who, in 1798, atage of 18, invented the method of Least Square.

On January 1st, 1801, the Italian astronomer Giuseppe Piazzi had discovered the asteroid Ceres and had been able to track its path for 40 days before it was lost in the glare of the sun. Based on this data, it was desired to determine the location of Ceres after it emerged from behind the sun without solving the complicated Kepler’s nonlinear equations of planetary motion. The only predictions that successfully allowed the German astronomer Franz Xaver von Zach to relocate Ceres on 7 December 1801, were those performed by the 24-year-old Gauss using least-squares analysis.However, Gauss did not publish the method until 1809, when it appeared in volume two of his work on celestial mechanics, “Theoria Motus Corporum Coelestium in sectionibus conicis solem ambientium”.

Giuseppe Piazzi1746 - 1826

Franz Xaver von Zach1754 - 1832

Gauss' potrait published in Astronomische Nachrichten 1828

Johann Carl Friedrich Gauss

1777 - 1855

http://en.wikipedia.org/wiki/Image:Franz_Xaver%2C_Baron_Von_Zach.jpg

7

"In this work Gauss systematically developed the method of orbit calculation from three observations he had devised in 1801 to locate the planetoid Ceres, the earliest discovered of the 'asteroids,' which had been spotted and lost by G. Piazzi in January 1801. Gauss predicted where the planetoid would be found next, using improved numerical methods based on least squares, and a more accurate orbit theory based on the ellipse rather than the usual circular approximation. Gauss's calculations, completed in 1801, enabled the astronomer W. M. Olbers to find Ceres in the predicted position, a remarkable feat that cemented Gauss's reputation as a mathematical and scientific genius" (Norman 879).

http://www.19thcenturyshop.com/apps/catalogitem?id=84#

Theoria motus corporum coelestium (1809)

8

Sketch of the orbits of Ceres and Pallas, by Gauss

http://www.math.rutgers.edu/~cherlin/History/Papers1999/weiss.html

9

EstimatorsSOLO

History

Legendre published a book on determining the orbits of comets in 1806. His method involved three observations taken at equal intervals and he assumed that the comet followed a parabolic path so that he ended up with more equations than there were unknowns. He applied his methods to the data known for two comets. In an appendix Legendre gave the least squares method of fitting a curve to the data available. However, Gauss published his version of the least squares method in 1809 and, while acknowledging that it appeared in Legendre's book, Gauss still claimed priority for himself. This greatly hurt Legendre who fought for many years to have his priority recognized.

Adrien-Marie Legendre1752 - 1833

The idea of least-squares analysis was independently formulated by the Frenchman Adrien-Marie Legendre in 1805 and the american Robert Adrain in 1808.

Robert Adrain1775 - 1843

Legendre, A.M. “Nouvelles Méthodes pour La Déterminationdes Orbites des Comètes”, Paris, 1806

10

EstimatorsSOLO

History

Mark Grigorievich Krein1907 - 1989

Andrey Nikolaevich Kolmogorov1903 - 1987

Norbert Wiener1894 - 1964

The first studies of minimum-mean-square estimation in stochasticprocesses were made by Kolmogorov (1939), Krein (1945) and Wiener (1949)

Kolmogorov, A.N., “Sur l’interpolation et extrapolation dessuites stationaires”, C.R. Acad. Sci. Paris, vol.208, 1939, pp.2043-2045

Krein, M.G., “On a problem of extrapolation of A. N. Kolmogorov”, C.R. (Dokl) Akad. Nauk SSSR, vol.46, 1945, pp.306-309

Wiener, N., “Extrapolation, Interpolation and Smoothing of Stationary Time Series, with Engineering Applications”, MIT Press, Cambridge, MA, 1949 (secret version 1942)

Kolmogorov developed a comprehensive treatment of the linearprediction problem for discrete-time stochastic processes.

Krein extended the results to continuous time by the lever use of bilinear transformation.

Wiener, independently, formulated the continuous time linearprediction problem and derived an explicit formula for the optimal predictor. Wiener also considered the filtering problem of estimatinga process corrupted by additive noise.

11

Kalman, Rudolf E. 1920 -

Peter Swerling1929 - 2000

The filter is named after Rudolf E. Kalman, though Thorvald Nicolai Thiele and Peter Swerling actually developed a similar algorithm earlier. Stanley F. Schmidt is generally credited with developing the first implementation of a Kalman filter. It was during a visit of Kalman to the NASA Ames Research Center that he saw the applicability of his ideas to the problem of trajectory estimation for the Apollo program, leading to its incorporation in the Apollo navigation computer. The filter was developed in papers by Swerling (1958), Kalman (1960), and Kalman and Bucy (1961).

Kalman Filter History

Thorvald Nicolai Thiele1830 - 1910

Stanley F. Schmidt1926 -

The filter is sometimes called filter due to the fact that it is a special case of a more general, non-linear filter developed earlier by Ruslan L. Stratonovich. In fact, equations of the special case, linear filter appeared in these papers by Stratonovich that were published before summer 1960, when Rudolf E. Kalman met with Ruslan L. Stratonovich during a conference in Moscow. In control theory, the Kalman filter is most commonly referred to as linear quadratic estimator (LQE).

Kalman, R.E., “A New Approach to Filtering and Prediction Problems”,J. Basic Eng., March 1960, p. 35-46

Kalman, R.E., Bucy, R.S.,“New Results in Filtering and Prediction Theory”,J. Basic Eng., March 1961, p. 95-108 Table of Content

12

EstimatorsSOLO

Optimal Parameter Estimate v

H zx

The optimal procedure to estimate depends on the amount of knowledge of theprocess that is initially available.

x

The following estimators are known and are used as function of the assumed initial knowledge available:

Estimators Known initiallyWeighted Least Square (WLS)& Recursive WLS

1

( ) ( ) Tkkkkkkk vvvvERvEv −−== &Markov Estimator2

Maximum Likelihood Estimator3 ( ) ( )xZLxZp xZ ,:|| =

Bayes Estimator4 ( ) ( )Zxporvxp Zxvx |, |,

The amount of assumed initial knowledge available on the process increases in this order.

Table of Content

13

Estimators for Static Systems

z

SOLO

Optimal Weighted Last-Square Estimate

Assume that the set of p measurements, can be expressed as a linear combination,of the elements of a constant vector plus a random, additive measurement error, :

v

H zx

x v

vxHz +=

( ) ( ) 1

1−−=−−= −

W

T xHzxHzWxHzJ

( )T

pzzzz ,,, 21 =

( ) T

nxxxx ,,, 21 =( )T

pvvvv ,,, 21 =We want to find , the estimation of the constant vector , that minimizes the cost function:

x

x

that minimizes J, is obtained by solving:0x

( ) 02/ 1 =−=∂∂=∇ − xHzWHxJJ T

x

( ) zWHHWHx TT 111

0

−−−=

This solution minimizes J iff :

( ) [ ] ( ) ( ) ( ) 02/ 0

1

00

22

0 <−−−=−∂∂− − xxHWHxxxxxJxx TTT

or the matrix HTW-1H is positive definite.

W is a hermitian (WH = W, H stands for complex conjugate and matrix transpose), positive definite weighting matrix.

14

v

H zx

SOLO

Optimal Weighted Least-Square Estimate (continue – 1)

( ) zWHHWHx TT 111

0

−−−=

Since the mean of the estimate is equal to the estimated parameter, the estimator is unbiased.

vxHz +=Since is random with mean

xHvExHvxHEzE =+=+=0

( ) ( ) xxHWHHWHzEWHHWHxE TTTT === −−−−−− 111111

0

is also random with mean:0x

( ) ( ) ( ) ( )0

1

00

12

00

1

0

* : xHzWHxxHzWzxHzxHzWxHzJ TTT

W

T −+−=−=−−= −−−

Using we want to find the minimum value of J:0

11 xHWHzWH TT −− =

( ) ( ) ( )0

1

0

0

11

00

1 xHzWzxHWHzWHxxHzWz TTTTT −=−+−= −−−−

2

0

2

0

1

0

1

0

11

10

WW

TTT

HWHx

TT xHzxHWHxzWzxHWzzWzTT

−=−=−= −−−−

−


15

v

H zx

2

0

22

0*

111 −−− −=−=WWW

xHzxHzJ

SOLO


where is a norm.aWaa T

W

12: −=

Using we obtain: 0

11 xHWHzWH TT −− =

( ) ( )0

,

0

1

0

1

0

0

1

000

01

=−=

−=−−−

−

−

xHWHxzWHx

xHzWxHxHzxHTT

xHWH

TT

T

W

T

bWaba T

W

1:, −=

This suggest the definition of an inner product of two vectors and (relative to the

weighting matrix W) as

ba

Projection Theorem

The Optimal Estimate is such that is the projection (relative to the weightingmatrix W) of on the plane.

0x

z0xH

xHTable of Content


16

v

H zx

2

0

22

0*

111 −−− −=−=WWW

xHzxHzJ

SOLO


Projection Theorem

The Optimal Estimate is such that is the projection (relative to the weightingmatrix W) of on the plane.

0x

z0xH

xHTable of Content

( )vxHz

zWHHWHx TT

+== −−− 111

0

( ) ( ) ( ) vWHHWHxvxHWHHWHxx TTTT 1111110

−−−−−− =−+=−


18

0z

SOLO

Recursive Weighted Least Square Estimate (RWLS)

Assume that the set of N measurements, can be expressed as a linear combination,of the elements of a constant vector plus a random, additive measurement error, :

0v

0zx 0H

x vvxHz += 00

( ) ( ) 10

00001

0000 −−=−−= −W

T xHzxHzWxHzJ

We found that the optimal estimator , that minimizes the cost function:

( )−x

( ) ( ) 0

1

00

1

0

1

00 zWHHWHx TT −−−=−is

Let define the following matrices for the complete measurement set

=

=

=

W

WW

z

zz

H

HH

0

0:,:,: 0

1

0

1

0

1( ) ( ) 1

0

1

00:−−=− HWHP T

Therefore:

( ) ( )1

1 10 0 0 01 1

1 1 1 1 1 1 0 01 1

0 0

0 0T T T T T TW H W z

x H W H H W z H H H HH zW W

−− −− −

− −

+ = = ÷ ÷ ÷

v

H zx

( ) ( ) 0

1

00 zWHPx T −−=−

An additional measurement set, is obtainedand we want to find the optimal estimator .

z ( )+x


19

SOLO

Recursive Weighted Least Square Estimate (RWLS) (continue -1)

( ) ( ) 1

0

1

00:−−=− HWHP T( ) ( ) 0

1


( ) ( ) [ ] [ ]

( ) ( )zWHzWHHWHHWH

z

z

W

WHH

H

H

W

WHHzWHHWHx

TTTT

TTTTTT

1

0

1

00

11

0

1

00

0

1

1

0

0

1

0

1

1

0

01

1

111

1

110

0

0

0

−−−−−

−

−−

−

−−−

++=

==+

Define ( ) ( ) HWHPHWHHWHP TTT 111

0

1

00

1 : −−−−− +−=+=+

( ) ( )[ ] ( ) ( ) ( )[ ] ( )−+−−−−=+−=+ −−−− PHWHPHHPPHWHPP TTLemmaMatrixInverse

T 1111

( ) ( )[ ] ( )[ ] ( ) 111111 −−−−−− +=+−≡+−− WHPWHHWHPWHPHHP TTTTT

( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( )−+−−=−+−−−−=+ −−PHWHPPPHWHPHHPPP TTT 11

( ) ( ) ( )( ) ( ) ( )[ ] ( ) ( ) zWHPzWHPHWHPHHPP

zWHzWHPxTTTT

TT

1

0

1

00

1

1

0

1

00

−−−

−−

++−+−−−−=

++=+


20

v

H zx

SOLO


( ) ( ) ( )( ) ( ) ( )[ ] ( ) ( )

( )( )

( ) ( )[ ]( )

( )( )

( )

( ) ( ) ( ) ( ) zWHPxHWHPx

zWHPzWHPHWHPHHPzWHP

zWHPzWHPHWHPHHPP

zWHzWHPx

TT

T

x

T

WHP

TT

x

T

TTTT

TT

T

11

1

0

1

00

1

0

1

00

1

0

1

00

1

1

0

1

00

1

−−

−

−

−

+

−

−

−

−−−

−−

++−+−−=

++−+−−−−=

++−+−−−−=

++=+

−

( ) ( ) 0

1


( ) ( ) HWHPP T 111 −−− +−=+

( ) ( ) ( ) ( )( )−−++−=+ − xHzWHPxx T 1

Recursive Weighted Least Square Estimate (RWLS)

z

( )−x

( )+x

Delay

( ) HWHP T 11 −− =+

H

( ) 1−+ WHP T

Estimator


21

( ) ( )

( ) ( )[ ]( ) ( ) ( ) ( )xHzWxHzxHzWxHz

xHz

xHz

W

WxHzxHz

xHz

xHz

W

W

xHz

xHzxHzWxHzJ

TT

TT

T

T

−−+−−=

−−

−−=

−−

−−

=−−=

−−

−

−

−−

100

1000

00

1

10

00

00

1

00011

11111

0

0

0

0

( ) 0

1

00

1 : HWHP T −− =−

SOLO


Second Way

We want to prove that

where ( ) ( ) 0

1

00: zWHPx T −−=−

( ) ( ) ( )[ ] ( ) ( )[ ]−−−−−=−− −− xxPxxxHzWxHz TT 1

00

1

000

Therefore

( )[ ] ( ) ( )[ ] ( ) ( ) ( ) ( ) 11

111 −− −+−−=−−+−−−−−=

−−−

WP

TT xHzxxxHzWxHzxxPxxJ


22

( ) 0

1

00

1 : HWHP T −− =−

SOLO


Second Way (continue – 1)


Define

( ) ( ) 0

1

00: zWHPx T −−=− ( ) ( )−=− − PHWzx TT

0

1

00

( ) ( )−−= −− xPzWH T 1

0

1

00( ) ( )−−= −− 1

0

1

00 PxHWz TT

( ) ( ) ( )[ ] ( ) ( )[ ]−−−−−=−− −− xxPxxxHzWxHz TT 1

00

1

000

( ) ( )xHWHxzWHxxHWzzWz

xHzWxHzTTTTTT

T

0

1

000

1

000

1

000

1

00

00

1

000

−−−−

−

+−−=

−−

( )[ ] ( ) ( )[ ]( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )−−−+−−−−−−−=

−−−−−−−−−

−

xPxxPxxPxxPx

xxPxxTTTT

T

1111

1

( ) ( ) xPxxHWz TT −−= −− 1

0

1

00( ) ( )−−= −− xPxzWx TTT 1

0

1

00 R

( ) xHWHxxPx TTT 0

1

00

1 −− =−


23

( ) 0

1

00

1 : HWHP T −− =− ( ) ( ) 0

1

00: zWHPx T −−=−

SOLO




Define

( ) ( ) ( )[ ] ( ) ( )[ ]−−−−−=−− −− xxPxxxHzWxHz TT 1

00

1

00

( ) ( )xHWHxzWHxxHWzzWz

xHzWxHzTTTTTT

T

0

1

000

1

000

1

000

1

00

00

1

000

−−−−

−

+−−=

−−

( )[ ] ( ) ( )[ ]( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )−−−+−−−−−−−=

−−−−−−−−−

−

xPxxPxxPxxPx

xxPxxTTTT

T

1111

1

( ) ( ) ( ) ( ) 0

1

00

1

0

1

00

1 zWHPHWzxPx TTT −−−− −=−−−

Use the identity: ( )1

00

1

0

1

00

1

0

1

000

1

0

1

0

1−

−−−−−−

+≡+− TTT HIHWWHIHWHHWW

εε

( ) 0lim1

lim1

lim1

000

1

000

1

00

1

00

1

0 ==

=

+−

−

→

−

→

−−

→

− TTT HHHHHIHWW εεε εεε

( ) ( ) 1

00

1

0

1

0

1

00

1

0

1

000

1

0

1

0

−−−−−−−− −== WHPHWWHHWHHWW TTT

( ) ( ) ( ) ( ) 0

1

000

1

00

1

0

1

00

1 zWzzWHPHWzxPx TTTT −−−−− =−=−−− q.e.d.


24

( )[ ] ( ) ( )[ ] ( ) ( )xHzWxHzxxPxxJ TT −−+−−−−−= −− 11

1

SOLO



x

Choose that minimizes the scalar cost function

Solution

( ) ( )[ ] ( ) 022 *1*11 =−−−−−=

∂∂ −− xHzWHxxP

x

J T

T

Define: ( ) ( ) HWHPP T 111 : −−− +−=+Then:

( ) ( )[ ] ( ) ( ) ( ) ( )[ ]−−+−+=+−−+=+ −−−−−− xHzWHxPzWHxHWHPxP TTT 11111*1

( )[ ] ( ) ( ) zWHxPxHWHP TT 11*11 −−−− +−−=+−

( ) ( ) ( ) ( )[ ]−−++−=+= − xHzWHPxxx T 1*

( )[ ] ( )+=+−=

∂∂ −−− 111

2

1

2

22 PHWHPx

J T

T

If P-1(+) is a positive definite matrix then is a minimum solution.*x


25

SOLO


( ) ( ) 1

1−−=−−= −

W

T xHzxHzWxHzJ

10

1000

000

000

0002

1

<<

=

−

−

λλ

λλ

k

k

W

For W = I (Identity Matrix) we have the Least-Square Estimator (LSE).

How to choose W?

1

If x (i) ≠ constant we can use either one step of measurement or if we assume thatx (i) changes continuously we can choose

2

λ is the fading factor.

Table of Content


26

vxHz += 00

v

0H0zx

( ) zRHHRHx TT 10

1

01

00−−−=

SOLO

Markov Estimate

For the particular vector measurement equation

where for the measurement noise, we know the mean: vEv =

and the variance: ( ) ( ) TvvvvER −−=

v

We choose W = R in WLS, and we obtain:

( ) ( ) 1

01

0:−−=− HRHP T

( ) ( ) HRHPP T 111 −−− +−=+

( ) ( ) ( ) ( )( )−−++−=+ − xHzRHPxx T 1

RWLS = Markov EstimateW = R

In Recursive WLS, we obtain for a newobservation: vxHz +=

v

H zx

Table of Content


27

vxHz +=

SOLO

Maximum Likelihood Estimate (MLE)

For the particular vector measurement equation

where the measurement noise, is gaussian (normal), with zero mean:

v

H zx

( )RNv ,0~

( ) ( )( )xp

zxpxzp

x

zxxz

,| ,

| =

and independent of , the conditional probability can be written, using Bayes rule as:

x ( )xzp xz ||

( )

−

−

==−=

1

111

1111

1

1

,

nxpp

nx

pxnxpxnpxpx

xHz

xHz

zxfxHzv

xn

xn

( ) ( ) 2/1

,, /,, T

vxzx JJvxpzxp =

The measurement noise can be related to and by the function:v zx

pxp

p

pp

p

I

z

f

z

f

z

f

z

f

z

fJ =

∂∂

∂∂

∂∂

∂∂

=

∂∂=

1

1

1

1

( ) ( ) ( ) ( )vpxpvxpzxp vxvxzx ⋅== ,, ,,

v

Since the measurement noise is independent of :xv

zThe joint probability of and is given by:x


28

SOLO

Maximum Likelihood Estimate (continue – 1)

v

H zx

( ) ( ) ( ) ( )vpxpvxpzxp vxvxzx ⋅== ,, ,,

x

v

( )vxp vx ,,

( ) ( )

( )( ) ( )

−−−=

−=

− xHzRxHzR

xHzpxzp

T

p

vxz

12/12/

|

2

1exp

2

1

|

π

( ) ( ) ( )[ ] ( )RWWLSxHzRxHzxzp T

xxz

x⇒−−⇔ −1

| min|max

( ) ( )[ ] ( ) 02 11 =−−=−−∂∂ −− xHzRHxHzRxHzx

TT

0*11 =− −− xHRHzRH TT ( ) zRHHRHxx TT 111*: −−−==

( ) ( )[ ] HRHxHzRxHzx

TT 11

2

2

2 −− =−−∂∂ this is a positive definite matrix, therefore

the solution minimizesand maximizes

( ) ( )[ ]xHzRxHz T −− −1

( )xzp xz ||

( ) ( )( ) ( )

( )

−=== − vRv

Rvp

xp

zxpxzp T

pvx

zxxz

12/12/

/| 2

1exp

2

1,|

π

Gaussian (normal), with zero mean

( ) ( )xzpxzL xz |:, |= is called the Likelihood Function and is a measureof how likely is the parameter given the observation .x z


29

SOLO

Maximum Likelihood Estimate (continue – 2)

( ) ( )xzpxzL xz |:, |= is called the Likelihood Function and is a measureof how likely is the parameter given the observation .x z


Fisher, Sir Ronald Aylmer 1890 - 1962

R.A. Fisher first used the term Likelihood. His reason for theterm likelihood function was that if the observation is and , then it is more likely that the true value of is than .

zZ =( ) ( )21 ,, xzLxzL >

1x 2xX

30

SOLO

Bayesian Maximum Likelihood Estimate (Maximum Aposterior – MAP Estimate)

v

H zxvxHz +=Consider a gaussian vector , where ,measurement, , where the Gaussian noiseis independent of and .( )Rv ,0~ N

vx ( ) ( )[ ]−− Pxx ,~

N

x

( )( ) ( )

( )( ) ( ) ( )( )

−−−−−−

−= − xxPxx

Pxp T

nx

1

2/12/ 2

1exp

2

1

π

( ) ( )( )

( ) ( )

−−−=−= − xHzRxHz

RxHzpxzp T

pvxz1

2/12/| 2

1exp

2

1|

π

( ) ( ) ( ) ( )∫∫+∞

∞−

+∞

∞−

== xdxpxzpxdzxpzp xxzzxz |, |,

is Gaussian with( )zp z ( ) ( ) ( ) ( ) ( )−=+=+= xHvExEHvxHEzE

0

( ) ( )[ ] ( )[ ] ( )[ ] ( )[ ] ( )( )[ ] ( )( )[ ] ( )[ ] ( )[ ] ( )[ ] ( )[ ] ( ) RHPHvvEHxxvEvxxEH

HxxxxEHvxxHvxxHE

xHvxHxHvxHEzEzzEzEz

TTTTT

TTT

TT

+−=+−−−−−−

−−−−=+−−+−−=

−−+−−+=−−=

00

cov

( )( ) ( )

( )[ ] ( )[ ] ( )[ ]

−−+−−−−

+−= −

xHzRHPHxHzRHPH

zp TT

Tpz ˆˆ2

1exp

2

1 1

2/12/π


31

SOLO

Bayesian Maximum Likelihood Estimate (Maximum Aposterior Estimate) (continue – 1)

v

H zxvxHz +=Consider a Gaussian vector , where ,measurement, , where the Gaussian noiseis independent of and .( )Rvv ,0;~ N

vx ( ) ( )[ ]−− Pxxx ,;~

N

x

( )( ) ( )

( )( ) ( ) ( )( )

−−−−−−

−= − xxPxx

Pxp T

nx

1

2/12/ 2

1exp

2

1

π( ) ( )

( )( ) ( )

−−−=−= − xHzRxHz

RxHzpxzp T

pvxz1

2/12/| 2

1exp

2

1|

π

( )( ) ( )

( )[ ] ( )[ ] ( )[ ]

−−+−−−−

+−= −

xHzRHPHxHzRHPH

zp TT

Tpz ˆˆ2

1exp

2

1 1

2/12/π

( ) ( ) ( )( ) ( ) ( )

( )

( ) ( ) ( )( ) ( ) ( )( ) ( )[ ] ( )[ ] ( )[ ]

−−+−−−+−−−−−−−−−⋅

+−

−==

−−− xHzRHPHxHzxxPxxxHzRxHz

RHPH

RPzp

xpxzpzxp

TTTT

T

nz

xxzzx

ˆˆ2

1

2

1

2

1exp

2

1||

111

2/1

2/12/12/

||

π

from which


32

SOLO


( ) ( ) ( )( ) ( ) ( )( ) ( )( ) ( )[ ] ( )( )−−+−−−−−−−−−+−−−−− xHzRHPHxHzxxPxxxHzRxHz TTTT 111

( ) ( )( )[ ] ( ) ( )( )[ ] ( )( ) ( ) ( )( )( )( ) ( )[ ] ( )( ) ( )( ) ( )[ ] ( )( )

( )( ) ( )( ) ( )( ) ( )( ) ( )( ) ( )[ ] ( )( )−−+−−−+−−−−−−−−−−

−−+−−−−=−−+−−−−

−−−−−+−−−−−−−−−−=

−−−−

−−−

−−

xxHRHPxxxxHRxHzxHzRHxx

xHzRHPHRxHzxHzRHPHxHz

xxPxxxxHxHzRxxHxHz

TTTTT

TTTT

TT

1111

111

11

( )( ) ( )( ) ( )( ) ( ) ( )( ) ( )( ) ( )[ ] ( )( )−−+−−−−−−−−−+−−−−−−− xHzRHPHxHzxxPxxxHzRxHz TTTT 111

( )[ ] ( )[ ] 11111111 −−−−−−−− −++/−/=+−− RHPHRHHRRRRHPHR TTTwe have

then

Define: ( ) ( )[ ] 111:−−− +−=+ HRHPP T

( )( ) ( ) ( )[ ] ( ) ( )( )( )( ) ( ) ( )[ ] ( )( ) ( )( ) ( ) ( )[ ] ( )( )( )( ) ( )[ ] ( )( )−−+−−−+

−−++−−−−−++−−−

−−+++−−=

−−

−−−−

−−−

xxHRHPxx

xxPPHRxHzxHzRHPPxx

xHzRHPPPHRxHz

TT

TTT

TT

11

1111

111

( ) ( ) ( )( )[ ] ( ) ( ) ( ) ( )( )[ ]−−++−−+−−++−−= −−− xHzRHPxxPxHzRHPxx TTT 111

( )( ) ( )

( ) ( ) ( )( )[ ] ( ) ( ) ( ) ( )( )[ ]

−−+−−−+−−+−−−−⋅

+= −−− xHzRHPxxPxHzRHPxx

Pzxp TTT

nzx

1112/12/| 2

1exp

2

1|

π


33

SOLO


then

where: ( ) ( )[ ] 111:−−− +−=+ HRHPP T

( )( ) ( )

( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ]

−+−−−+−+−−−−⋅

+= −−− xHzRHPxxPxHzRHPxx

Pzxp TTT

nzx111

2/12/| 2

1exp

2

1|

π

( )zxp zxx

|max | ( ) ( ) ( ) ( )( )−−++−==+ − xHzRHPxxx T 1*:

Table of Content


34

Estimators

v

( )vxh ,z

x

Estimatorx

SOLO

The Cramér-Rao Lower Bound (CRLB) on the Variance of the Estimator

xE

- estimated mean vector

[ ]( ) [ ]( ) TTT

x xExExxExExxExE

−=−−=2σ - estimated variance matrix

For a good estimator we want

xxE =- unbiased estimator vector

TT

x xExExxE

−=2σ - minimum estimation variance

( ) ( ) Tk kzzZ 1:= - the observation matrix after k observations

( ) ( ) ( ) xkzzLxZL k ,,,1, = - the Likelihood or the joint density function of Zk

We have:

( )T

pzzzz ,,, 21 = ( ) T

nxxxx ,,, 21 = ( )T

pvvvv ,,, 21 =

The estimation of , using the measurements of a system corrupted by noise is a random variable with

x x zv

( ) ( ) ( ) ( )∫== dvvpxvZpxZpxZL vk

vzk

xzk ;||, ||

( ) ( )[ ] ( ) ( )[ ] ( ) ( )[ ] ( ) ( )

[ ] [ ] ( )xbxZdxZLZx

kzdzdxkzzLkzzxkzzxE

kkk +==

=

∫∫

,

1,,,1,,1,,1

- estimator bias( )xb

therefore:

35

Estimators

v

( )vxh ,z

x

Estimatorx

SOLO

The Cramér-Rao Lower Bound on the Variance of the Estimator (continue – 1)

[ ] [ ] [ ] ( )xbxZdxZLZxZxE kkkk +== ∫ ,

We have:

[ ] [ ] [ ] ( )x

xbZd

x

xZLZx

x

ZxE kk

kk

∂∂+=

∂∂=

∂∂ ∫ 1

,

Since L [Zk,x] is a joint density function, we have:

[ ] 1, =∫ kk ZdxZL

[ ] [ ] [ ] [ ]0,,

0, =

∂∂=

∂∂→=

∂∂ ∫∫∫ k

kk

kk

k

Zdx

xZLxZd

x

xZLxZd

x

xZL

[ ]( ) [ ] ( )x

xbZd

x

xZLxZx k

kk

∂∂+=

∂∂−∫ 1

,

Using the fact that: [ ] [ ] [ ]x

xZLxZL

x

xZL kk

k

∂∂=

∂∂ ,ln

,,

[ ]( ) [ ] [ ] ( )x

xbZd

x

xZLxZLxZx k

kkk

∂∂+=

∂∂−∫ 1

,ln,

36

EstimatorsSOLO


[ ]( ) [ ] [ ] ( )x

xbZd

x

xZLxZLxZx k

kkk

∂∂+=

∂∂−∫ 1

,ln,

Hermann Amandus Schwarz

1843 - 1921

Let use Schwarz Inequality:

( ) ( ) ( ) ( )∫∫∫ ≤ dttgdttfdttgtf22

2

The equality occurs if and only if f (t) = k g (t)

[ ]( ) [ ] [ ] [ ]xZLx

xZLgxZLxZxf k

kkk ,

,ln:&,:

∂∂=−= choose:

[ ]( ) [ ] [ ]

( ) [ ]( ) [ ]( ) [ ] [ ]

∂

∂−≤

∂

∂+=

∂

∂−

∫∫

∫k

kkkkk

kk

kk

Zdx

xZLxZLZdxZLxZx

x

xb

Zdx

xZLxZLxZx

2

2

2

2

,ln,,1

,ln,

[ ]( ) [ ]( )

[ ] [ ]∫∫

∂

∂

∂

∂+≥−

kk

k

kkk

Zdx

xZLxZL

x

xb

ZdxZLxZx2

2

2

,ln,

1

,

37

EstimatorsSOLO


[ ]( ) [ ]( )

[ ] [ ]∫∫

∂

∂

∂

∂+≥−

kk

k

kkk

Zdx

xZLxZL

x

xb

ZdxZLxZx2

2

2

,ln,

1

,

This is the Cramér-Rao bound for a biased estimator

Harald Cramér1893 – 1985

Cayampudi RadhakrishnaRao

1920 -

[ ] ( ) [ ] 1,& =+= ∫ kkk ZdxZLxbxZxE

[ ]( ) [ ] [ ] [ ] ( )( ) [ ][ ] [ ] ( ) [ ] ( ) [ ] [ ] ( ) [ ]

( ) [ ]

1

2

0

2

22

,

,2,

,,

∫

∫∫∫∫

+

−+−=

+−=−

kk

kkkkkkkk

kkkkkkk

ZdxZLxb

ZdxZLZxEZxxbZdxZLZxEZx

ZdxZLxbZxEZxZdxZLxZx

[ ] [ ] ( ) [ ]( )

[ ] [ ]( )xb

Zdx

xZLxZL

x

xb

ZdxZLZxEZxk

kk

kkkk

x

2

2

2

22

,ln,

1

, −

∂

∂

∂

∂+≥−=

∫∫

σ

38

EstimatorsSOLO


[ ] [ ] ( ) [ ]( )

[ ] [ ]( )xb

Zdx

xZLxZL

x

xb

ZdxZLZxEZxk

kk

kkkk

x

2

2

2

22

,ln,

1

, −

∂

∂

∂

∂+≥−=

∫∫

σ

[ ] [ ] [ ][ ]

[ ] [ ] [ ] 0,,ln

0,

1,,

,,ln

=∂

∂→=∂

∂→= ∫∫∫∂

∂

=∂

∂

kkkxZL

x

xZL

x

xZL

kk

kk ZdxZLx

xZLZd

x

xZLZdxZL

k

k

k

[ ] [ ] [ ] [ ] [ ][ ]

0,,ln,ln

,,ln

,

2

2

=∂

∂∂

∂+∂

∂→ ∫∫∂

∂

∂∂

k

x

xZL

kkk

kkkx

ZdxZLx

xZL

x

xZLZdxZL

x

xZL

k

[ ] [ ]0

,ln,ln2

2

2

=

∂

∂+

∂∂→

∂∂

x

xZLE

x

xZLE

kkx

( )

[ ]( )

( )

[ ] ( )xb

x

xZLE

x

xb

xb

x

xZLE

x

xb

kkx

2

2

2

2

2

2

2

2

,ln

1

,ln

1

−

∂∂

∂

∂+−=−

∂

∂

∂

∂+≥σ

39

Estimators

[ ]( ) [ ]( )

[ ]

( )

[ ]

∂∂

∂

∂+−=

∂

∂

∂

∂+≥−∫

2

2

2

2

2

2

,ln

1

,ln

1

,

x

xZLE

x

xb

x

xZLE

x

xb

ZdxZLxZxkk

kkk

SOLO


( )

[ ]( )

( )

[ ] ( )xb

x

xZLE

x

xb

xb

x

xZLE

x

xb

kkx

2

2

2

2

2

2

2

2

,ln

1

,ln

1

−

∂∂

∂

∂+−=−

∂

∂

∂

∂+≥σ

For an unbiased estimator (b (x) = 0), we have:

[ ] [ ]

∂∂

−=

∂

∂≥

2

22

2

,ln

1

,ln

1

x

xZLE

x

xZLE

kkxσ

http://www.york.ac.uk/depts/maths/histstat/people/cramer.gif

40

Estimators

[ ]( ) [ ]( ) [ ] [ ]( ) [ ]( ) ( ) [ ] [ ] ( )

( ) [ ] ( )

∂

∂+

∂∂

∂

∂+−=

∂

∂+

∂

∂

∂

∂

∂

∂+≥

−−=−−

−

−

∫

x

xbI

x

xZLE

x

xbI

x

xbI

x

xZL

x

xZLE

x

xbI

xZxxZxEZdxZLxZxxZx

x

kT

x

Tkk

T

x

TkkkkTkk

1

2

2

1

,ln

,ln,ln

,

SOLO


The multivariable form of the Cramér-Rao Lower Bound is:

[ ]( )[ ]

[ ]

−

−=−

n

k

n

k

k

xZx

xZx

xZx

11

[ ]( ) [ ][ ]

[ ]

∂∂

∂∂

=

∂

∂=∇

n

k

k

kk

x

x

xZL

x

xZL

x

xZLxZL

,ln

,ln

,ln,ln

1

Fisher Information Matrix

[ ] [ ] [ ]

∂∂−=

∂

∂

∂

∂=x

k

x

Tkk

x

xZLE

x

xZL

x

xZLE

2

2 ,ln,ln,ln:J

Fisher, Sir Ronald Aylmer 1890 - 1962

41

Fisher, Sir Ronald Aylmer (1890-1962)

The Fisher information is the amount of information that an observable random variable z carries about an unknown parameter x upon which the likelihood of z, L(x) = f(Z; x), depends. The likelihood function is the joint probability of the data, the Zs, conditional on the value of x, as a function of x. Since the expectation of the score is zero, the variance is simply the second moment of the score, the derivative of the lan of the likelihood function with respect to x. Hence the Fisher information can be written

( ) [ ]( ) [ ]( ) [ ]( ) x

k

xxx

Tk

x

k

x xZLExZLxZLEx ,ln,ln,ln: ∇∇−=∇∇=J

Table of Content

42

Estimators

( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT

xxx =−= &:

kkkk

kkkkkkk

vxHz

wuGxx

+=Γ++Φ= −−−−−− 111111

SOLO

Kalman Filter Discrete Case

Assume a discrete dynamic system

( ) ( ) ( ) ( ) ( ) ( ) lkT

www kQlekeEkwEkwke ,

0

&: δ=−=

kkkkkkk zKxKx += −1|| ˆ'ˆ

( ) ( ) ( ) ( ) ( ) ( ) lkT

vvv kRlekeEkvEkvke ,

0

&: δ=−= ( ) ( ) ( ) 1, −= lk

Tvw kMlekeE δ

Let find a Linear Filter that works in two stages:

s.t. will minimize (by choosing the optimal gains Kk and Kk’ )

( ) ( ) kkkkk

kkT

kkkkkT

kkkk

xxxwhere

xxExxxxEJ

−=

=−−=

||

||||

ˆ:~

~~ˆˆ

kkk xExE =|ˆ Unbiased Estimator 0ˆ~|| =−= kkkkk xExExE

=≠

=lk

lklk 1

0,δ

111|111| ˆˆ −−−−−− +Φ= kkkkkkk uGxxkz 1. One step prediction, before the measurement ,based on the estimation at step k-1 :1|ˆ −kkx

2. Update after the measurement is received:kz

43

Estimators

kkkkk xxx −= −− 1|1| ˆ:~

kkkkkkk zKxKx += −1|| ˆ'ˆ

SOLO

Kalman Filter Discrete Case (continue – 1)

Define

kkkkk xxx −= || ˆ:~

The Linear Estimator we want is:

Therefore

[ ] [ ] [ ] kkkkkkkkk

z

kkkk

x

kkkkkkk vKxKxIHKKvxHKxxKxx

kkk

++−+=++++−= −−

−

1|

ˆ

1||~''~'~

1|

Unbiaseness conditions: 0~~1|| == −kkkk xExE

gives: [ ] 0~''~

00

1|

0

| =++−+= − kkkkkkkkkkk vEKxEKxEIHKKxE

or: kkk HKIK −='

Therefore the Unbiased Linear Estimator is:

[ ]1|1|| ˆˆˆ −− −+= kkkkkkkkk xHzKxx

44

Estimators

+=Γ++Φ= −−−−−−

kkkk

kkkkkkk

vxHz

wuGxx 111111

SOLO


The discrete dynamic system

The Linear Filter (Linear Observer)[ ]

−++=

+Φ=

−−−−

−−−−−−

1|111||

111|111|

ˆˆˆ

ˆˆ

kkkkkkkkkkk

kkkkkkk

xHzKuGxx

uGxx

111|111|1|~ˆ:~

−−−−−−− Γ−Φ=−= kkkkkkkkkk wxxxx

Tkkk

Tkkkk

Tkkkkkk QPxxEP 11111|111|1|1|

~~: −−−−−−−−−− ΓΓ+ΦΦ==

0~

00

0~~

1111

1

1||

==

==

==

−−−−

−

−

Tkk

Tkk

kk

kkkk

wxEwxE

wEvE

xExE

[ ] [ ]

T

kT

kkkT

kT

kkk

Tk

Tkkk

Tk

Tkkk

Tk

Tk

Tk

Tkkkkk

Tkkkkkk

wwExwE

wxExxE

wxwxE

xxEP

11111

0

111

1

0

1111111

11111111

1|1|1|

~

~~~

~~

~~:

−−−−−−−−

−−−−−−−−

−−−−−−−−

−−−

ΓΓ+ΦΓ−

ΓΦ−ΦΦ=

Γ−ΦΓ−Φ=

=

1111

0

1|111|

1

~~−−−−−−−− Γ−=Γ−Φ=

−

kk

M

Tkkkkkkk

Tkkk MvwEvxEvxE

k

45

EstimatorsSOLO


kkkkkkkkkkkk vxHKxxxx −−=−= −− 1|1|||~~ˆ:~

( )[ ] ( )[ ] Tk

Tk

Tk

Tkk

Tkkkkkkkkk

Tkkkkkk KvHxxvxHKxExxEP −−−−== −−−− 1|1|1|1||||

~~~~~~:

111|~

−−− Γ−= kkT

kkk MvxE

( ) ( ) [ ] ( ) [ ]T

kT

kkkT

kT

kT

kkkk

Tk

Tkkk

Tk

Tk

Tkkkkkk

KxvEKHIxvEK

KvxEKHIxxEHKI

1|1|

1|1|1|

~~

~~~

−−

−−−

+−+

+−−=

( ) ( )

( ) ( )Tkk

Tk

Tkk

Tkkkkk

Tk

R

Tkkk

Tkk

P

Tkkkkkk

HKIMKKMHKI

KvvEKHKIxxEHKI

kkk

−Γ−Γ−−

+−−=

−−−−

−−

−

1111

1|1|

1|

~~

+=Γ++Φ= −−−−−−

kkkk

kkkkkkk

vxHz

wuGxx 111111The discrete dynamic system

The Linear Filter (Linear Observer)[ ]

−++=

+Φ=

−−−−

−−−−−−

1|111||

111|111|

ˆˆˆ

ˆˆ

kkkkkkkkkkk

kkkkkkk

xHzKuGxx

uGxx

46

EstimatorsSOLO


( ) ( ) ( ) ( ) T

kkT

kT

kkT

kkkkkT

kkkT

kkkkkk

Tkkkkkk

HKIMKKMHKIKRKHKIPHKI

xxEP

−Γ−Γ−−+−−=

=

−−−−− 11111|

|||~~:

( ) ( )( ) T

kT

kkkkT

kT

kT

kkkkkk

Tk

Tkkkkk

Tkkk

Tkkkkk

Tkkkkkk

KHPHHMMHRK

MPHKKMHPPxxEP

1|1111

111|111|1||||~~:

−−−−−

−−−−−−−

+Γ+Γ++

Γ+−Γ+−==

Completion of Squares

[ ]

[ ][ ] [ ]

+Γ+Γ+Γ+−

Γ+−

=−−−−−−−−

−−−−

Tk

C

Tkkkk

Tk

Tk

Tkkkkk

B

Tk

Tkkkk

B

kkT

kkk

A

kk

kkkK

I

HPHHMMHRMPH

MHPP

KIP

T

1|1111111|

111|1|

|

Joseph Form (true for all Kk)

47

Estimators

kkK

Tkk

Kk

Tk

Kk

KPtracexxEtracexxEJ

kkkk|min~~min~~minmin ===

SOLO


Completion of Squares

Use the Matrix Identity:

−

−

−=

−

−−∆

−−

IBC

I

C

BCBA

I

CBI

CB

BAT

T

T 1

11 0

0

0

0

[ ] [ ] ( )

−

+Γ+Γ+

∆−==

−−−−−−

−T

k

Tkkkk

Tk

Tk

Tkkkkk

k

kT

kkkkkkCBK

I

HPHHMMHRCBKIxxEP

11|1111

1|||

0

0~~:

to obtain

( ) ( ) ( )Tk

Tkkkk

Tkkkk

Tk

Tk

Tkkkkkkk

Tkkkkkk MPHHPHHMMHRMHPP 111|

1

1|1111111|1|: −−−

−

−−−−−−−−− Γ++Γ+Γ+Γ+−=∆

[ ]

[ ][ ] [ ]

+Γ+Γ+Γ+−

Γ+−

=−−−−−−−−

−−−−

Tk

C

Tkkkk

Tk

Tk

Tkkkkk

B

Tk

Tkkkk

B

kkT

kkk

A

kk

kkkK

I

HPHHMMHRMPH

MHPP

KIP

T

1|1111111|

111|1|

|

48

Estimators

kkT

kkkkkkT

kkk PtracexxEtracexxEJ |||||~~~~ ===

[ ] [ ]

1

1

1|1111111|..*

−

−

−−−−−−−− +Γ+Γ+Γ+==C

Tkkkk

Tk

Tk

Tkkkkk

B

kkT

kkkFK

kk HPHHMMHRMHPKK

SOLO


To obtain the optimal K (k) that minimizes J (k+1) we perform

[ ] [ ] 011| =−−+∆∂

∂=∂

∂=

∂∂ −− T

kkkkk

kk

k

k CBKCCBKtraceKK

Ptrace

K

J

Using the Matrix Equation: (see next slide) ( )TT BBAABAtraceA

+=∂∂

[ ] ( ) 01*| =+−=∂

∂=

∂∂ − T

kk

kk

k

k CCCBKK

Ptrace

K

Jwe obtain

or

Kalman Filter Gain

( ) ( ) ( ) ( )

( ) T

kkT

kkkkkk

B

T

kkT

kkk

C

Tkkkk

Tk

Tk

Tkkkkk

B

kkT

kkk

A

kkkkkK

MHPKPtrace

MHPHPHHMMHRMHPPtracetracePtracekJT

111|1|

111|

1

1|1111111|1||min

1

min

−−−−

−−−

−

−−−−−−−−−

Γ+−=

Γ++Γ+Γ+Γ+−=∆==−

( ) [ ]Tkkkk

Tk

Tk

Tkkkkk

T

k

kk

k

k HPHHMMHRCCK

Ptrace

K

J1|11112

|2

2

2

2 −−−−− +Γ+Γ+=+=∂

∂=

∂∂

49

MatricesSOLO

Differentiation of the Trace of a square matrix

[ ] ( )( )

∑∑∑∑∑∑=

==l p k

lkpklp

aa

l p k

Tklpklp

T abaabaABAtracelk

Tkl

[ ]TABAtraceA∂

∂ [ ] ∑∑ +=∂

∂p

pjipk

ikjkT

ij

baabABAtracea

[ ] ( )TTT BBABABAABAtraceA

+=+=∂∂

50

Estimators

( ) 1

1|1|*−

−− += Tkkkkk

Tkkkk HPHRHPK

( ) ( ) Tkkk

Tkkkkkkkk KRKHKIPHKIP ***** 1|| +−−= −

SOLO


we found that the optimal Kk that minimizes Jk is

( ) 1|

1

1|1|1| −

−

−−− +−= kkkT

kkkkkT

kkkkk PHHPHRHPP

( ) [ ] 1|

1111|

&*

11 −

−−−− −=+=

−− kkkkkkT

kkk

LemmaMatrixInverse

existRPPHKIHRHP

kk

When Mk = 0, where:

( ) ( ) 1, −= lkkT

vw MlekeE δ

51

EstimatorsSOLO


We found that the optimal Kk that minimizes Jk (when Mk-1 = 0 ) is

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] 11|1111|11*

−+++++++=+ kHkkPkHkRkHkkPkK TT

( ) ( ) 11111|

11

&

1

1| 11|

1

−−−−−

−−−

− +−=+−

−− k

Tkkk

Tkkkkkk

LemmaMatrixInverse

existPR

Tkkkkk RHHRHPHRRHPHR

kkk

( ) 11111|

11|

11|* −−−−

−−

−−

− +−= kT

kkkT

kkkkkT

kkkkT

kkkk RHHRHPHRHPRHPK

( ) ( ) 11111|

1111|1|

−−−−−

−−−−− +−+= k

Tkkk

Tkkkkk

Tkkk

Tkkkkk RHHRHPHRHHRHPP

[ ] 11|

11111|* −

−−−−−

− =+= kT

kkkkT

kkkT

kkkk RHPRHHRHPK

If Rk-1 and Pk|k-1

-1 exist:

Table of Content

52

EstimatorsSOLO


Properties of the Kalman Filter

0~ˆ || =Tkkkk xxE

Proof (by induction):

( )1111

00000001 0

vxHz

xxwuGxx

+==Γ++Φ=k=1:

( ) ( )( )0010|00110010010011000|00

0010|0011111000|00

00|00|1111000|001|1

ˆˆ

ˆˆ

ˆˆˆˆ

uGHxHvwHuGHxHKuGx

uGHxHvxHKuGx

xExxHzKuGxx

−Φ−+Γ++Φ++Φ=−Φ−+++Φ=

=−++Φ=

( ) 1100110|00110|0011|11|1~~ˆ~ vKwIHKxHKxxxx +Γ−+Φ−Φ=−=

( )[ ] 100110|001000|001|11|1~ˆ~ˆ vwHKxKuGxExxE T +Γ+Φ−+Φ=

( )[ ] TvKwIHKxHKx 1100110|00110|00~~ +Γ−+Φ−Φ

( ) ( )

T

R

T

TTT

Q

TTT

P

T

KvvEK

IKHwwEHKHKIxxEHK

1111

110000110110|00|0011

1

00|0

~~

+

−ΓΓ+Φ−Φ−=

1

53

EstimatorsSOLO


Properties of the Discrete Kalman Filter

0~ˆ || =Tkkkk xxE

Proof (by induction) (continue – 1):

k=1 :

( ) ( ) TTTTTTT KRKIKHQHKHKIPHKxxE 11111000110110|00111|11|1~ˆ +−ΓΓ+Φ−Φ−=

1

( ) ( ) TTT

P

TT

P

TT KRKKHQPHKQPHK 1111100000|001100000|0011

0|10|1

+ΓΓ+ΦΦ+ΓΓ+ΦΦ−=

[ ] [ ] 01

111|11

1|1

111|111111110|111

−=

=+−=+−−=RHPK

TTT

P

TT

T

T

KRPHKKRKKHIPHK

In the same way we continue for k > 1 and by induction we prove the result.

Table of Content

54

EstimatorsSOLO


Properties of the Kalman Filter

1,,10~| −== kjzxE T

jkk

Proof:

( )jjjj

kkkkkkkkk

vxHz

xHzKxx

+=

−+= −− 1|1|| ˆˆˆ

2

( ) ( ) kkkkkkkkkkkkkkkkkkkkk vKxHKIxxHvxHKxxxx +−=−−++=−= −−− 1|1|1|||~ˆˆˆ:~

( )[ ] ( ) ( ) ( )

jkkR

Tjkk

Tj

jk

Tjkk

jk

Tjkkkk

Tj

Tjkkkk

Tjjjkkkkkkjkk

vvEKHxvEKvxEHKIHxxEHKI

vxHvKxHKIEzxE

,00

1|1|

1||

~~

~~

δ

++−+−=

++−=

→>→>

−−

−

( )[ ] ( )

0

1|1||~~~

→>

−− +−=+−=jk

Tjkk

Tjkkkk

Tjkkkkkk

Tjkk zvEKzxEHKIzvKxHKIEzxE

55

Estimators


xxx =−= &:

kkkk

kkkkkkk

vxHz

wuGxx

+=Γ++Φ= −−−−−− 111111

SOLO

Kalman Filter Discrete Case - Innovation Assume a discrete dynamic system

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=

( ) ( ) lklekeE Tvw ,0 ∀=

=≠

=lk

lklk 1

0,δ

kkkkkkkk vxHzz +−=−= −− 1|1|~ˆ:ι

Innovation is defined as:

The Linear Filter (Linear Observer)

−+=

+Φ=

−

−−

−−−−−−

k

kkz

kkkkkkkkk

kkkkkkk

xHzKxx

uGxx

ι

1|ˆ

1|1||

111|111|

ˆˆˆ

ˆˆ

111|1111|1|~ˆ:~

−−−−−−−− Γ−Φ=−= kkkkkkkkkk wxxxx

0~

00

1| =+−= − kkkkk vExEHE ι

( ) 1

1|1|.. :

−

−− += kT

kkkkkkkFK

k RHPHPHK

2Properties of the Discrete Kalman Filter

56

Estimators

( )

[ ]

( )∑+=

+−++−

−−−−−−−−

−+

Γ−Φ+=

=

Γ−Φ+Γ−Φ+=

Γ−Φ+−Φ=Γ−Φ=

++

i

jkkkkkk

F

kiijj

F

jii

iiiiiiiiiiiiii

iiiiiii

F

iiiiiiiiii

wvKFFFxFFF

wvKwvKxFF

wvKxHKIwxx

kiji

i

111|111

111112|11

1|||1

1,1,

~

~

~~~

SOLOKalman Filter Discrete Case – Innovation (continue – 1)

Assume i > j:

∑+=

→+≥

+

→+≥

++

+

+++++

Γ−Φ+=

i

jkjk

Tjjkk

jk

Tjjkkkki

jPj

Tjjjjji

Tjjji xwExvEKFxxEFxxE

101

|1

01

|11,

|1

|1|11,|1|1~~~~~~

( )iiikiiki

iiii

FFFFFF

HKIF

==−Φ=

− :&:

:

,1,

( ) ( ) iiiiiiiiiiiiiii vKxHKIvxHKxx +−=−−= −−− 1|1|1||~~~~

( ) ( ) Tj

Tj

Tjjiiii

Tji vHxvxHEE +−+−= −− 1|1|

~~ιι

Tji

Tj

Tjji

Tjiii

Tj

Tjjiii vvEHxvEvxEHHxxEH +−−= −−−− 1|1|1|1|

~~~~

jjjjjjj wxx Γ−Φ=+ ||1~~

jjjiT

jjji PFxxE |11,|1|1~~

++++ =

57

Estimators

( )∑+=

++++ Γ−Φ+=i

jkkkkkkkijjjiji wvKFxFx

11,|11,|1

~~

SOLO

Kalman Filter Discrete Case – Innovation (continue – 2)

Assume i > j:

( )

0~~

0

1

0

|1|1|1

,

=Γ−Φ=→>⇒

+

→>

+

>

++T

j

jijM

Tji

Tj

ji

Tjji

jiT

jjji

jiT

wvExvExvE

δ

( )iiikiiki

iiii

FFFFFF

HKIF

==−Φ=

− :&:

:

,1,

1112,

10

111,

0

1|11,|1|1

1,1

~~

++++

+=++++++++

Φ=

Γ−Φ+= ∑

++

jjjji

i

jk

Tjkk

R

Tjkkkki

Tjjjji

Tjjji

RKF

vwEvvEKFvxEFvxE

jkj

δ

1,1111 +++++ = jijT

ji RvvE δ

Tji

Tj

Tjji

Tjiii

Tj

Tjjiii

Tji vvEHxvEvxEHHxxEHE 111|111|111|1|1111

~~~~++++++++++++++ +−−=ιι

jjjjjjj wxx Γ−Φ=+ ||1~~

58

Estimators

[ ] 1

11|111|11

.. −

+++++++ += jT

jjjjT

jjj

K

j RHPHHPKFK

SOLOKalman Filter Discrete Case – Innovation (continue – 3)

Assume i > j:

1,111112,111|11,1 +++++++++++++ +Φ−+= jijjjjjiiiT

jjjjii RRKFHHHPFH δ

Tji

Tj

Tjji

Tjiii

Tj

Tjjiii

Tji vvEHxvEvxEHHxxEHE 111|111|111|1|1111

~~~~++++++++++++++ +−−=ιι

( )1112,12,1, +++++++ −Φ== jjjjijjiji HKIFFFF

( ) 1,11111|11112,1 ++++++++++++ +−−Φ= jijjjT

jjjjjjjii RRKHPHKIFH δ

[ ]1,11

1,1111|1111|112,1

+++

+++++++++++++

=

++−Φ=

jij

jijjT

jjjjjT

jjjjjii

R

RRHPHKHPFH

δδ

1,1111

..

+++++ = jij

KT

ji REFK

διι 01 =+iE ιInnovation =

White Noise forKalman Filter Gain!!!

&

Table of Content

59

Kalman FilterState Estimation in a Linear System (one cycle)

SOLO

State vector prediction111|111| ˆˆ −−−−−− +Φ= kkkkkkk uGxx

Covariance matrix extrapolation111|111| −−−−−− +ΦΦ= kT

kkkkkk QPP

Innovation CovariancekT

kkkkk RHPHS += −1|

Gain Matrix Computation11|

−−= k

Tkkkk SHPK

Innovation1|ˆ

1|ˆ

−

−−=kkz

kkkkk xHzi

Filteringkkkkkk iKxx += −1|| ˆˆ

Covariance matrix updating

( )( ) ( ) T

kkkT

kkkkkk

kkkk

Tkkkkk

kkkkT

kkkkkkk

KRKHKIPHKI

PHKI

KSKP

PHSHPPP

+−−=

−=−=

−=

−

−

−

−−

−−

1|

1|

1|

1|1

1|1||

1+= kk

60

Kalman FilterState Estimation in a Linear System (one cycle)

Sensor DataProcessing andMeasurement

Formation

Observation -to - Track

Association

InputData Track Maintenance

( Initialization,Confirmationand Deletion)

Filtering andPrediction

GatingComputations

Samuel S. Blackman, " Multiple-Target Tracking with Radar Applications", Artech House,1986

Samuel S. Blackman, Robert Popoli, " Design and Analysis of Modern Tracking Systems",Artech House, 1999

SOLO

Rudolf E. Kalman( 1920 - )

61

1|1| ˆˆ: −− −=−= kkkkkkkk zzxHzi

Recursive Bayesian EstimationSOLO

Linear Gaussian Markov Systems (continue – 18)Innovation

The innovation is the quantity:

We found that:

( ) 0ˆ||ˆ| 1|1:11:11|1:1 =−=−= −−−−− kkkkkkkkkk zZzEZzzEZiE

[ ] [ ] kT

kkkkkkT

kkkT

kkkkkk SHPHRZiiEZzzzzE =+==−− −−−−− :ˆˆ 1|1:11:11|1|

Using the smoothing property of the expectation:

( ) ( ) ( ) ( )( )

( ) ( ) xEdxxpxdxdyyxpx

dxdyypyxpxdyypdxyxpxyxEE

x

X

x y

YX

x yyxp

YYX

y

Y

x

YX

YX

==

=

=

=

∫∫ ∫

∫ ∫∫ ∫

∞+

−∞=

∞+

−∞=

∞+

−∞=

∞+

−∞=

∞+

−∞=

∞+

−∞=

∞+

−∞=

,

||

,

,

||

,

1:1 −= kT

jkT

jk ZiiEEiiEwe have:

Assuming, without loss of generality, that k-1 ≥ j, and innovation I (j) is Independent on Z1:k-1, and it can be taken outside the inner expectation:

0

0

1:11:1 =

== −−T

jkkkT

jkT

jk iZiEEZiiEEiiE

62

1|1| ˆˆ: −− −=−= kkkkkkkk zzxHzi


Linear Gaussian Markov Systems (continue – 18)Innovation (continue – 1)

The innovation is the quantity:

We found that:

( ) 0ˆ||ˆ| 1|1:11:11|1:1 =−=−= −−−−− kkkkkkkkkk zZzEZzzEZiE

kT

kkkkkkT

kk SHPHRZiiE =+= −− :1|1:1

0=Tjk iiE

jikT

jk SiiE δ=

The uncorrelatedness property of the innovation implies that since they are Gaussian,the innovation are independent of each other and thus the innovation sequence isStrictly White. Without the Gaussian assumption, the innovation sequence is Wide Sense White.

Thus the innovation sequence is zero mean and white for the Kalman (Optimal) Filter.

The innovation for the Kalman (Optimal) Filter extracts all the available informationfrom the measurement, leaving only zero-mean white noise in the measurement residual.

63

kkT

kn iSiz

1

:2 −

=χ



Define the quantity:

Let use: kkk iSu2/1

:−

= Since is Gaussian (a linear combination of the nz components of )is Gaussian too with:

ki ku ki

0:0

2/1

==−

kkk iESuE z

k

nk

S

Tkkkk

Tkkk

Tkk ISiiESSiiSEuuE ===

−−−− 2/12/12/12/1

:

where Inz is the identity matrix of size nz. Therefore, since the covariance matrix ofu is diagonal, its components ui are uncorrelated and, since they are jointly Gaussianthey are also independent.

( )1,0;Pr:1

22 1

ii

n

iik

Tkkk

Tkn uuuuuiSi

z

zN==== ∑

=

−

χ

Therefore is chi-square distributed with nz degrees of freedom.2

znχ

Since Sk is symmetric and positive definite, it can be written as:

0,,& 1 >=== SiSSknH

kkH

kkkkznz

diagDITTTDTS λλλ H

kkkk TDTS 11 −− = 2/12/11

2/12/12/1 ,,& −−−−− ==znSSk

Hkkkk diagDTDTS λλ

64

SOLO Review of Probability

Chi-square Distribution

( ) ( ) xT

xT ePexExPxExq

11

:−−

=−−=

Assume a n-dimensional vector is Gaussian, with mean and covariance P, then we can define a (scalar) random variable:

x xE

Since P is symmetric and positive definite, it can be written as:

0,,& 1 >=== PiPPPnHH

P ndiagDITTTDTP λλλ

HP TDTP 11 −− = 2/12/1

12/12/12/1 ,,& −−−−− ==

nPPPH

P diagDTDTP λλ

Since is Gaussian (a linear combination of the n components of )is Gaussian too, with:

x u ( )xEx −

0:0

2/1

=−=−

xExEPuE n

P

Txx

Txx

T IPeeEPPeePEuuE ===−−−− 2/12/12/12/1

:

where In is the identity matrix of size n. Therefore, since the covariance matrix ofu is diagonal, its components ui are uncorrelated and, since they are jointly Gaussianthey are also independent.

( )1,0;Pr:1

21

ii

n

ii

Tx

Tx uuuuuePeq N==== ∑

=

−

Therefore q is chi-square distributed with n degrees of freedom.

Let use: ( ) xePxExPu 2/12/1: −− =−=

65


Derivation of Chi and Chi-square Distributions

Given k normal random independent variables X1, X2,…,Xk with zero men values and same variance σ2, their joint density is given by

( ) ( ) ( )

++−=

−

= ∏=

2

22

12/

12/1

2

2

1 2exp

2

1

2

2exp

,,1 σσπσπ

σk

kk

k

i

i

normal

tindependenkXX

xx

x

xxpk

Define

Chi-square 0:: 22

1

2 ≥++== kk xxy χ

Chi 0: 22

1 ≥++= kk xx χ

( )

+≤++≤=Χ kkkkkk dxxdpk

χχχχχ 22

1Pr

The region in χk space, where pΧk (χk) is constant, is a hyper-shell of a volume

(A to be defined)

χχ dAVd k 1−=

( ) ( )

Vd

kk

kkkkkkkk dAdxxdpk

χχσ

χσπ

χχχχχ 1

2

2

2/

22

1 2exp

2

1Pr −

Χ

−=

+≤++≤=

( ) ( )

−=

−

Χ 2

2

2/

1

2exp

2 σχ

σπχχ k

kk

k

k

Ap

k

Compute

1x

2x

3x

χ

χdχχπ ddV 24=

66


Derivation of Chi and Chi-square Distributions (continue – 1)

( ) ( ) ( )kk

kk

k

k UA

pk

χσ

χσπ

χχ

−=

−

Χ 2

2

2/

1

2exp

2

Chi-square 0: 22

1

2 ≥++== kk xxy χ

( ) ( ) ( ) ( ) ( )( )

<

≥

−

=

−+==

−

Χ

00

02

exp22

1 2

2/1

2/

0

2

22

y

yy

yy

A

ypyp

d

ydypp

k

kk

y

k

Yk kkk

σσπ

χ

χ χχ

A is determined from the condition ( ) 1=∫∞

∞−

dyypY

( ) ( ) ( ) ( ) ( )( )2/

212/

222exp

22

2/

2/20

2

2

2

22/ kAk

Ayd

yyAdyyp

k

k

k

kY Γ=→=Γ=

−

= ∫∫

∞−

∞

∞−

ππσσσπ

( ) ( )( )

( )

( )yUyy

kkyp

kk

Y

−

Γ=

−

2

2/2

2

2/

2exp

2/

2/1,;

σσσ

Γ is the gamma function ( ) ( )∫∞

− −=Γ0

1 exp dttta a

( ) ( ) ( )

( ) ( )kk

k

k

k

k

k Uk

pk

χσ

χσ

χχ

−

Γ=

−−−

Χ 2

212/2

2exp

2/

2/1

( )

<≥

=00

01:

a

aaU

Function ofOne Random

Variable

67



Chi-square 0: 22

1

2 ≥++== kk xxy χ

Mean Value 2 2 2 21k kE E x E x kχ σ= + + =

( ) ( ) 4

2 42 2 4

0

1, ,& 3

th

i

i i

Moment of aGauss Distribution

x i i i i

x E x

i kE x x E x xσ σ σ

= =

= = − = − =

( ) ( )

( )

( )

2

4

2 4

22 22 2 2 2 2 4 2 2 4

1

2 2 2 4 4 2 2 2 4

1 1 1 1 13

2 2 4 43 2

k

k

k k ii

k k k k k

i j i i ji j i i j

i j

k k

E k E k E x k

E x x k E x E x x k

k k k k k

χ

σ

σ

σ χ σ χ σ σ

σ σ

σ σ

=

= = = = =≠

−

= − = − = − ÷ = − = + − ÷ ÷

= + − − =

∑

∑ ∑ ∑ ∑∑

Variance ( ) 2

22 2 2 42k

kE k kχ

σ χ σ σ= − =

where xi

are Gaussianwith

Gauss’ Distribution

68



Tail probabilities of the chi-square and normal densities.

The Table presents the points on the chi-square distribution for a given upper tail probability

xyQ >= Pr

where y = χn2 and n is the number of degrees

of freedom. This tabulated function is also known as the complementary distribution.

An alternative way of writing the previousequation is: ( )QxyQ n −=≤=− 1Pr1 2χwhich indicates that at the left of the point xthe probability mass is 1 – Q. This is 100 (1 – Q) percentile point.

Examples

1. The 95 % probability region for χ22 variable

can be taken at the one-sided probabilityregion (cutting off the 5% upper tail): ( )[ ] [ ]99.5,095.0,0 2

2 =χ

.5 99

2. Or the two-sided probability region (cutting off both 2.5% tails): ( ) ( )[ ] [ ]38.7,05.0975.0,025.0 22

22 =χχ

.0 51

.0 975 .0 025.0 05

.7 38

3. For χ1002 variable, the two-sided 95% probability region (cutting off both 2.5% tails) is:

( ) ( )[ ] [ ]130,74975.0,025.0 2100

2100 =χχ

74130

69



Note the skewedness of the chi-square distribution: the above two-sided regions arenot symmetric about the corresponding means

nE n =2χ


For degrees of freedom above 100, thefollowing approximation of the points on thechi-square distribution can be used:

( ) ( )[ ]22 1212

11 −+−=− nQQn Gχ

where G ( ) is given in the last line of the Tableand shows the point x on the standard (zeromean and unity variance) Gaussian distributionfor the same tail probabilities.In the case Pr y = N (y; 0,1) and withQ = Pr y>x , we have x (1-Q) :=G (1-Q)

.5 99.0 51

.0 975 .0 025.0 05

.7 38

70



Table of Content

The fact that the innovation sequence is zero mean and white for the Kalman (Optimal) Filter, is very important and can be used in Tracking Systems:

1. when a single target is detected with probability 1 (no false alarms), the innovation can be used to check Filter Consistency (in fact the knowledge of Filter Parameters Φ (k), G (k), H (k) – target model, Q (k), R (k) – system and measurement noises)

2. when a single target is detected with probability 1 (no false alarms), and the target initiate a unknown maneuver (change model) at an unknown time the innovation can be used to detect the start of the maneuver (change of target model) by detecting a Filter Inconsistency and choose from a bank of models (see IMM method) (Φi (k), Gi (k), Hi (k) –i=1,…,n target models) the one with a white innovation.

3. when a single target is detected with probability less then 1 and false alarms are also detected, the innovation can be used to provide information of the probability of each detection to be the real target (providing Gating capability that eliminates less probable detections) (see PDAF method).

4. when multiple targets are detected with probability less then 1 and false alarms are also detected, the innovation can be used to provide Gating information for each target track and probability of each detection to be related to each track (data association). This is done by running a Kalman Filter for each initiated track. (see JPDAF and MTT methods)

71


Linear Gaussian Markov Systems (continue – 20)Evaluation of Kalman Filter Consistency

A state-estimator (filter) is called consistent if its state estimation error satisfy

( ) ( ) ( ) 0|~:|ˆ ==− kkxEkkxkxE

( ) ( )[ ] ( ) ( )[ ] ( ) ( ) ( )kkPkkxkkxEkkxkxkkxkxE TT ||~|~:|ˆ|ˆ ==−−

this is a finite-sample consistency property, that is, the estimation errors based on a finite number of samples (measurements) should be consistent with the theoreticalstatistical properties:

• Have zero mean (i.e. the estimates are unbiased).• Have covariance matrix as calculated by the Filter.

The Consistency Criteria of a Filter are:

1. The state errors should be acceptable as zero mean and have magnitude commensurate with the state covariance as yielded by the Filter.

2. The innovation should have the same property as in (1).

3. The innovation should be white noise.

Only the last two criteria (based on innovation) can be tested in real data applications.The first criterion, which is the most important, can be tested only in simulations.

72


Linear Gaussian Markov Systems (continue – 21)Evaluation of Kalman Filter Consistency (continue – 1)

When we design the Kalman Filter, we can perform Monte Carlo (N independent runs)Simulations to check the Filter Consistency (expected performances).

Real time (Single-Run Tests)

In Real Time, we can use a single run (N = 1). In this case the simulations are replacedby assuming that we can replace the Ensemble Averages (of the simulations) by theTime Averages based on the Ergodicity of the Innovation and perform only the tests(2) and (3) based on Innovation properties.

The Innovation bias and covariance can be evaluated using

( ) ( ) ( )∑∑== −

==K

k

TK

k

kikiK

SkiK

i11 1

1ˆ&1ˆ

73



Real time (Single-Run Tests) (continue – 1)

Test 2: ( ) ( ) ( ) ( ) ( ) ( )kSkikiEkiEkkzkzE T ===−− &0:1|ˆ

Using the Time-Average Normalized Innovation Squared (NIS) statistics

( ) ( ) ( )∑=

−=K

k

Ti kikSki

K 1

11:ε

must have a chi-square distribution with K nz degrees of freedom.

iK ε


The test is successful if [ ]21, rri ∈εwhere the confidence interval [r1,r2] is definedusing the chi-square distribution of iε

[ ] αε −=∈ 1,Pr 21 rri

For example for K=50, nz=2, and α=0.05, using the two tails of the chi-square distribution we get

( )( )

==→=

==→=→

6.250/130130925.0

5.150/7474025.0~50

22

100

12

1002100

r

ri

χ

χχε

.0 975 .0 025

74130

74



Real time (Single-Run Tests) (continue – 2)

Test 3: Whiteness of Innovation

Use the Normalized Time-Average Autocorrelation

( ) ( ) ( ) ( ) ( ) ( ) ( )2/1

111

:−

===

+++= ∑∑∑

K

k

TK

k

TK

k

Ti lkilkikikilkikilρ

In view of the Central Limit Theorem, for large K, this statistics is normal distributed.

For l≠0 the variance can be shown to be 1/K that tends to zero for large K.

Denoting by ξ a zero-mean unity-variance normalrandom variable, let r1 such that

[ ] αξ −=−∈ 1,Pr 11 rr

For α=0.05, will define (from the normal distribution) r1 = 1.96. Since has standard deviation ofThe corresponding probability region for α=0.05 will be [-r, r] where

iρ K/1

KKrr /96.1/1 ==Normal Distribution

75



Monte-Carlo Simulation Based Tests

The tests will be based on the results of Monte-Carlo Simulations (Runs) that provideN independent samples

( ) ( ) ( ) ( ) ( ) ( ) NikkxkkxEkkPkkxkxkkx Tiiiii ,,1|~|~|&|ˆ:|~ ==−=

Test 1:For each run i we compute at each scan k

And compute the Normalized (state) Estimation Error Squared (NEES)

( ) ( ) ( ) ( ) NikkxkkPkkxk iT

ixi ,,1|~||~: 1 == −ε

Under the Hypothesis that the Filter is Consistent and the Linear Gaussian,is chi-square distributed with nx (dimension of x) degrees of freedom. Then

( )kxiε

( ) xxi nkE =ε

The average, over N runs, of is( )kxiε

( ) ( )∑=

=N

ixix k

Nk

1

1: εε

76



Monte-Carlo Simulation Based Tests (continue – 1)

Test 1 (continue – 1):

The average, over N runs, of is( )kxiε

( ) ( )∑=

=N

ixix k

Nk

1

1: εε

The test is successful if [ ]21, rrx ∈εwhere the confidence interval [r1,r2] is definedusing the chi-square distribution of iε

[ ] αε −=∈ 1,Pr 21 rrx

For example for N=50, nx=2, and α=0.05, using the two tails of the chi-square distribution we get

( )( )

==→=

==→=→

6.250/130130925.0

5.150/7474025.0~50

22

100

12

1002100

r

ri

χ

χχε


.0 975 .0 025

74130

must have a chi-square distribution with N nx degrees of freedom.

xN ε

77




The test is successful if [ ]21, rri ∈εwhere the confidence interval [r1,r2] is definedusing the chi-square distribution of iε

[ ] αε −=∈ 1,Pr 21 rri

For example for N=50, nz=2, and α=0.05, using the two tails of the chi-square distribution we get

( )( )

==→=

==→=→

6.250/130130925.0

5.150/7474025.0~50

22

100

12

1002100

r

ri

χ

χχε


.0 975 .0 025

74130

must have a chi-square distribution with N nz degrees of freedom.

iN ε

Test 2: ( ) ( ) ( ) ( ) ( ) ( )kSkikiEkiEkkzkzE T ===−− &0:1|ˆ

Using the Normalized Innovation Squared (NIS) statistics, compute from N Monte-Carlo runs:

( ) ( ) ( ) ( )∑=

−=N

jjj

Tji kikSki

Nk

1

11:ε

78



Test 3: Whiteness of Innovation

Use the Normalized Sample Average Autocorrelation

( ) ( ) ( ) ( ) ( ) ( ) ( )2/1

111

:,

−

===

= ∑∑∑

N

jj

Tj

N

jj

Tj

N

jj

Tji mimikikimikimkρ

In view of the Central Limit Theorem, for large N, this statistics is normal distributed.

For k≠m the variance can be shown to be 1/N that tends to zero for large N.

Denoting by ξ a zero-mean unity-variance normalrandom variable, let r1 such that

[ ] αξ −=−∈ 1,Pr 11 rr

For α=0.05, will define (from the normal distribution) r1 = 1.96. Since has standard deviation ofThe corresponding probability region for α=0.05 will be [-r, r] where

iρ N/1

NNrr /96.1/1 ==Normal Distribution


79



Examples Bar-Shalom, Y, Li, X-R, “Estimation and Tracking: Principles, Techniques and Software”, Artech House, 1993, pg.242


Single Run, 95% probability

[ ]99.5,0∈xεTest (a) Passes if

A one-sided region is considered.For nx = 2 we have

( ) ( )[ ] [ ]99.5,095.0,02 22

22 == χχxn

( ) ( ) ( ) ( )∑=

−=K

k

Tx kkxkkPkkx

Kk

1

1 |~||~1:ε

( ) ( ) ( ) qkxkkx +−Φ= 1

See behavior of for various values of the process noise qfor filters that are perfectly matched.

80





Monte-Carlo, N=50, 95% probability

[ ] [ ]6.2,5.150/130,50/74 =∈xεTest (a) Passes if

( ) ( ) ( ) ( )∑=

−=N

jjj

Tjx kkxkkPkkx

Nk

1

1 |~||~1:ε(a)

( ) ( ) ( ) ( ) ( ) ( ) ( )2/1

111

:,

−

===

= ∑∑∑

N

jj

Tj

N

jj

Tj

N

jj

Tji mimikikimikimkρ(c)

The corresponding probability region for α=0.05 will be [-r, r] where

28.050/96.1/1 === Nrr

[ ] [ ]43.1,65.050/4.71,50/3.32 =∈iεTest (b) Passes if

( ) ( ) ( ) ( )∑=

−=N

jjj

Tji kikSki

Nk

1

11:ε(b)

( ) ( )[ ] [ ]130,74925.0,025.02 2100

2100 == χχxn

( ) ( )[ ] [ ]71,32925.0,025.01 2100

2100 == χχzn

81





Example Mismatched Filter

A Mismatched Filter is tested: Real System Process Noise q = 9 Filter Model Process Noise qF=1

( ) ( ) ( ) ( )∑=

−=K

k

Tx kkxkkPkkx

Kk

1

1 |~||~1:ε

( ) ( ) ( ) qkxkkx +−Φ= 1

(1) Single Run

(2) A N=50 runs Monte-Carlo with the 95% probability region

( ) ( ) ( ) ( )∑=

−=N

jjj

Tjx kkxkkPkkx

Nk

1

1 |~||~1:ε

[ ] [ ]6.2,5.150/130,50/74 =∈xεTest (2) Passes if

( ) ( )[ ] [ ]130,74925.0,025.02 2100

2100 == χχxn

Test Fails

Test Fails

[ ]99.5,0∈xεTest (1) Passes if

( ) ( )[ ] [ ]99.5,095.0,02 22

22 == χχxn

82





Example Mismatched Filter (continue -1)

A Mismatched Filter is tested: Real System Process Noise q = 9 Filter Model Process Noise qF=1

( ) ( ) ( ) qkxkkx +−Φ= 1



( ) ( ) ( ) ( )∑=

−=N

jjj

Tji kikSki

Nk

1

11:ε

[ ] [ ]43.1,65.050/4.71,50/3.32 =∈iεTest (3) Passes if

( ) ( )[ ] [ ]71,32925.0,025.01 2100

2100 == χχzn

( ) ( ) ( ) ( ) ( ) ( ) ( )2/1

111

:,

−

===

= ∑∑∑

N

jj

Tj

N

jj

Tj

N

jj

Tji mimikikimikimkρ

(c)

The corresponding probability region for α=0.05 will be [-r, r] where

28.050/96.1/1 === Nrr

Test Fails

Test Fails

83

Extended Kalman FilterSensor Data

Processing andMeasurement

Formation


Association




GatingComputations



SOLO

In the extended Kalman filter, (EKF) the state transition and observation models need not be linear functions of the state but may instead be (differentiable) functions.

( ) ( ) ( )[ ] ( )kwkukxkfkx +=+ ,,1

( ) ( ) ( )[ ] ( )11,1,11 +++++=+ kkukxkhkz νState vector dynamics

Measurements


xxx =−= &:

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=


=≠

=lk

lklk 1

0,δ

The function f can be used to compute the predicted state from the previous estimate and similarly the function h can be used to compute the predicted measurement from the predicted state. However, f and h cannot be applied to the covariance directly. Instead a matrix of partial derivatives (the Jacobian) is computed.

( ) ( ) ( )[ ] ( ) ( )[ ] ( )( )

( ) ( )( )

( ) ( )kekex

fkeke

x

fkekukxEkfkukxkfke wx

Hessian

kxE

Txx

Jacobian

kxE

wx ++∂∂+

∂∂=+−=+

2

2

2

1,,,,1

( ) ( ) ( )[ ] ( ) ( )[ ] ( )( )

( ) ( )( )

( ) ( )1112

1111,1,11,1,11

1

2

2

1

++++∂∂+++

∂∂=+++++−+++=+

++

kkex

hkeke

x

hkkukxEkhkukxkhke x

Hessian

kxE

Txx

Jacobian

kxE

z νν

Taylor’s Expansion:

84

Extended Kalman FilterState Estimation (one cycle)

SOLO

( )11|11| ,ˆ,1ˆ −−−− −= kkkkk uxkfxState vector prediction

Jacobians Computation

1|1|1 ˆˆ

1 &−−−

∂∂=

∂∂=Φ −

kkkk x

k

x

k x

hH

x

f

Covariance matrix extrapolation111|111| −−−−−− +ΦΦ= kT

kkkkkk QPP

Innovation CovariancekT

kkkkk RHPHS += −1|

Gain Matrix Computation11|

−−= k

Tkkkk SHPK

Innovation1|ˆ

1|ˆ

−

−−=kkz

kkkkk xHzi

Filteringkkkkkk iKxx += −1|| ˆˆ

Covariance matrix updating

( )( ) ( ) T

kkkT

kkkkkk

kkkk

Tkkkkk

kkkkT

kkkkkkk

KRKHKIPHKI

PHKI

KSKP

PHSHPPP

+−−=

−=−=

−=

−

−

−

−−

−−

1|

1|

1|

1|1

1|1||

1+= kk

85

Extended Kalman FilterState Estimation (one cycle)


Formation


Association




GatingComputations



SOLO

Rudolf E. Kalman( 1920 - )

86

Unscented Kalman FilterSOLO

Criticism of the Extended Kalman FilterUnlike its linear counterpart, the extended Kalman filter is not an optimal estimator. In addition, if the initial estimate of the state is wrong, or if the process is modeled incorrectly, the filter may quickly diverge, owing to its linearization. Another problem with the extended Kalman filter is that the estimated covariance matrix tends to underestimate the true covariance matrix and therefore risks becoming inconsistent in the statistical sense without the addition of "stabilising noise".Having stated this, the extended Kalman filter can give reasonable performance, and is arguably the de facto standard in navigation systems and GPS.

87

Uscented Kalman FilterSOLO

When the state transition and observation models – that is, the predict and update functions f and h (see above) – are highly non-linear, the extended Kalman filter can give particularly poor performance [JU97]. This is because only the mean is propagated through the non-linearity. The unscented Kalman filter (UKF) [JU97] uses a deterministic sampling technique known as the to pick a minimal set of sample points (called sigma points) around the mean. These sigma points are then propagated through the non-linear functions and the covariance of the estimate is then recovered. The result is a filter which more accurately captures the true mean and covariance. (This can be verified using Monte Carlo sampling or through a Taylor series expansion of the posterior statistics.) In addition, this technique removes the requirement to analytically calculate Jacobians, which for complex functions can be a difficult task in itself.

( ) ( ) ( )[ ] ( )kwkukxkfkx +=+ ,,1

( ) ( )[ ] ( )11,11 ++++=+ kkxkhkz νState vector dynamics

Measurements


xxx =−= &:

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=


=≠

=lk

lklk 1

0,δ

The Unscent Algorithm using ( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT

xxx =−= &:

Determines ( ) ( ) ( ) ( ) ( ) ( )kPkekeEkzEkzke zT

zzz =−= &:

88


( ) ( )[ ]

( )n

n

j jj

nx

nx

nx

x

xxx

fxn

xxf

∂

∂=∇⋅

∇⋅=+

∑

∑

=

∞

=

1

0ˆ

:

!

1ˆ

δδ

δδ

Develop the nonlinear function f in a Taylor series around

x

Define also the operator ( )[ ] ( )xfx

xfxfD

nn

j jjx

nx

nx

x

∂

∂=∇⋅= ∑=1

: δδδ

Propagating Means and Covariances Through Nonlinear Transformations

Consider a nonlinear function .( )xfy =

Let compute

Assume is a random variable with a probability density function pX (x) (known orunknown) with mean and covariance

x ( ) ( ) Txx xxxxEPxEx ˆˆ,ˆ −−==

( )

( )[ ] ∑ ∑∑

∑∞

= =

∞

=

∞

=

∂

∂=∇⋅=

=+=

0ˆ

10ˆ

0

!

1

!

1

!

1ˆˆ

nx

nn

j jj

nx

nx

n

nx

fx

xEn

fxEn

DEn

xxfEy

x

δδ

δ δ

( ) ( ) xxTT PxxxxExxE

xxExE

xxx

=−−=

=−=+=

ˆˆ

0ˆ

ˆ

δδ

δδ

89



Consider a nonlinear function .(continue – 1)

( )xfy = ( ) ( ) xxTT PxxxxExxE

xxExE

xxx

=−−=

=−=+=

ˆˆ

0ˆ

ˆ

δδ

δδ

( ) ( )

+

∂

∂+

∂

∂+

∂

∂+

∂

∂+=

∂

∂=+=

∑∑∑

∑∑ ∑

===

=

∞

= =

x

n

j jjx

n

j jjx

n

j jj

x

n

j jj

nx

nn

j jj

fx

xEfx

xEfx

xE

fx

xExffx

xEn

xxfEy

xxx

xx

ˆ

4

1ˆ

3

1ˆ

2

1

ˆ10

ˆ1

!4

1

!3

1

!2

1

ˆ!

1ˆˆ

δδδ

δδδ

Since all the differentials of f are computed around the mean (non-random) x

( )[ ] ( )[ ] ( )[ ] ( )[ ]xxxxT

xxxTT

xxxTT

xxx fPfxxEfxxEfxE ˆˆˆˆ2 ∇∇=∇∇=∇∇=∇⋅ δδδδδ

( )[ ] 0

ˆ1

0ˆ1

ˆ0

ˆ =

∂∂=

∂

∂=

∇⋅=∇⋅ ∑∑

==x

n

j jj

x

n

j jj

x

xxx fx

xEfx

xEfxEfxExx

δδδδ

( ) [ ] ( ) ( )[ ] [ ] [ ] +++∇∇+==+= ∑∞

=xxxxxx

xxTx

nx

nx fDEfDEfPxffDE

nxxfEy ˆ

4ˆ

3ˆ

0ˆ

!4

1

!3

1

!2

1ˆ

!

1ˆˆ δδδδ

90

Simon J. Julier



Consider a nonlinear function .(continue - 2)

( )xfy = ( ) ( ) xxTT PxxxxExxE

xxExE

xxx

=−−=

=−=+=

ˆˆ

0ˆ

ˆ

δδ

δδ

Unscented Transformation (UT), proposed by Julier and Uhlmannuses a set of “sigma points” to provide an approximation ofthe probabilistic properties through the nonlinear function

Jeffrey K. Uhlman

A set of “sigma points” S consists of p+1 vectors and their associatedweights S = i=0,1,..,p: x(i) , W(i) . (1) Compute the transformation of the “sigma points” through the nonlinear transformation f:

( ) ( )( ) pixfy ii ,,1,0 ==(2) Compute the approximation of the mean: ( ) ( )∑

=

≈p

i

ii yWy0

ˆ

The estimation is unbiased if:( ) ( ) ( ) ( ) ( ) yWyyEWyWE

p

i

ip

i y

iip

i

ii ˆˆ00 ˆ0

===

∑∑∑

===

( ) 10

=∑=

p

i

iW

(3) The approximation of output covariance is given by

( ) ( )( ) ( )( )∑=

−−≈p

i

Tiiiyy yyyyWP0

ˆˆ

91



Consider a nonlinear function (continue – 3)( )xfy =

One set of points that satisfies the above conditions consists of a symmetric set of symmetric p = 2nx points that lie on the covariance contour Pxx:

th

xn

( ) ( )

( )

( )

( ) ( ) ( )( ) ( ) ( )

x

xni

xi

xxxni

i

xxxi

ni

nWW

nWW

PW

nxx

PW

nxx

WWxx

x

x ,,1

2/1

2/1

1ˆ

1ˆ

ˆ

0

0

0

0

000

=

−=

−=

−

−=

−

+=

==

+

+

where is the row or column of the matrix square root of nx Pxx /(1-W0)(the original covariance matrix Pxx multiplied by the number of dimensions of x, nx/(1-W0)). This implies:

( )( )i

xxx WPn 01/ −

xxxn

i

T

i

xxx

i

xxx PW

nP

W

nP

W

nx

01 00 111 −=

−

−∑

=

Unscented Transformation (UT) (continue – 1)

92



Consider a nonlinear function (continue – 3)( )xfy =

Unscented Transformation (UT) (continue – 2)

( ) ( )( )( )

( )

( )

+=

=

=

==

∑

∑∞

=−

∞

=

0

0

2,,1ˆ!

1

,,1ˆ!

1

0ˆ

nxx

nx

nx

nx

ii

nnixfDn

nixfDn

ixf

xfy

i

i

δ

δ1

2

Unscented Algorithm:

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )∑∑

∑

∑ ∑∑ ∑∑

==

=

=

∞

=−

=

∞

==

++−+−+=

++++−+=

−+−+==

x

ii

x

i

x

iii

x

i

x

i

x

n

ixx

x

n

ix

x

n

ixxx

x

n

i n

nx

x

n

i n

nx

x

n

i

iiUT

xfDxfDn

WxfD

n

Wxf

xfDxfDxfDxfn

WxfW

xfDnn

WxfD

nn

WxfWyWy

1

640

1

20

1

64200

1 0

0

1 0

00

2

0

ˆ!6

1ˆ

!4

11ˆ

2

11ˆ

ˆ!6

1ˆ

!4

1ˆ

!2

1ˆ

1ˆ

ˆ!

1

2

1ˆ

!

1

2

1ˆˆ

δδδ

δδδ

δδ

( )

i

xxxi

i PW

nxxxx

−

±=±=01

ˆˆ δ

Since ( ) ( )( )( )

−=

∂

∂−= ∑=

−oddnxfD

evennxfDxf

xxxfD

nx

nx

nn

j jij

nx

i

ix

i ˆ

ˆˆˆ

1 δ

δδ δ

93

Unscented Kalman Filter

( ) ( ) ( ) ( )∑=

++−+∇∇+=

x

ii

n

ixx

x

xxTUT xfDxfD

n

WxfPxfy

1

640 ˆ!6

1ˆ

!4

11ˆ

2

1ˆˆ δδ

( )

i

xxxi

i PW

nxxxx

−

±=±=01

ˆˆ δ

SOLO


Consider a nonlinear function (continue – 4)( )xfy =Unscented Transformation (UT) (continue – 3)

Unscented Algorithm:

( ) ( )

( ) ( ) ( )xfPxfPW

n

n

WxfP

W

nP

W

n

n

W

xfPW

nP

W

n

n

WxfD

n

W

xxTxxxT

x

n

i

T

i

xxx

i

xxxT

x

n

i

T

i

xxx

i

xxxT

x

n

ix

x

x

xx

i

ˆ2

1ˆ

12

11ˆ

112

11

ˆ112

11ˆ

2

11

0

0

1 00

0

1 00

0

1

20

∇∇=∇

−

∇−=∇

−

−

∇−=

∇

−

−

∇−=−

∑

∑∑

=

==δ

Finally:

We found

( ) [ ] ( ) ( )[ ] [ ] [ ] +++∇∇+==+= ∑∞

=xxxxxx

xxTx

nx

nx fDEfDEfPxffDE

nxxfEy ˆ

4ˆ

3ˆ

0ˆ

!4

1

!3

1

!2

1ˆ

!

1ˆˆ δδδδ

We can see that the two expressions agree exactly to the third order.

94



Consider a nonlinear function (continue – 5)( )xfy =Unscented Transformation (UT) (continue – 4)

Accuracy of the Covariance:

( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( )[ ] [ ] [ ]

( ) ( )[ ] [ ] [ ] T

xxxxxxxxT

x

xxxxxxxxT

x

T

m

mxx

n

nxx

TTTyy

fDEfDEfPxf

fDEfDEfPxf

fDm

xfDxffDn

xfDxfE

yyyyEyyyyEP

+++∇∇+⋅

⋅

+++∇∇+−

++

++=

−=−−=

∑∑∞

=

∞

=

ˆ4

ˆ3

ˆ

ˆ4

ˆ3

ˆ

22

!4

1

!3

1

!2

1ˆ

!4

1

!3

1

!2

1ˆ

!

1ˆˆ

!

1ˆˆ

ˆˆˆˆ

δδ

δδ

δδδδ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( )

+

++

++=

∑∑

∑∑

∞

=

∞

=

∞

=

∞

=

T

m

mx

n

nx

T

n

nx

Tx

T

n

nx

Tx

T

fDm

fDn

E

xfxfDn

ExfxfDExfDn

ExfxfDExfxfxf

22

20

20

!

1

!

1

ˆˆ!

1ˆˆˆ

!

1ˆˆˆˆˆ

δδ

δδδδ

95


96


( ) ( )∑∑ −−==N

Tiiiz

N

ii zzPz2

0

2

0

ψψβψβ

x

xPα

xP

zP

( )f

iβ

iβ

iψ

z

[ ]xxi PxPxx ααχ −+=

Weightedsample mean

Weightedsample

covariance

Table of Content

97

Uscented Kalman FilterSOLOUKF Summary

Initialization of UKF

( ) ( ) TxxxxEPxEx 00000|000 ˆˆˆ −−==

[ ] ( ) ( )

=−−===

R

Q

P

xxxxEPxxExTaaaaaTTaa

00

00

00

ˆˆ00ˆˆ0|0

00000|0000

[ ]TTTTa vwxx =:

For ∞∈ ,,1 k

Calculate the Sigma Points ( )( )

λγ

γ

γ +=

=−=

=+=

=

−−−−+

−−

−−−−−−

−−−−

L

LiPxx

LiPxx

xx

ikkkk

Likk

ikkkk

ikk

kkkk

,,1ˆˆ

,,1ˆˆ

ˆˆ

1|11|11|1

1|11|11|1

1|10

1|1

State Prediction and its Covariance

System Definition( ) ( )

==+=

==+−= −−−−−−−

lkkT

lkkkkk

lkkT

lkkkkkk

RvvEvEvxkhz

QwwEwEwuxkfx

,

,1111111

&0,

&0,,1

δ

δ

( ) Liuxkfx ki

kki

kk 2,,1,0,ˆ,1ˆ 11|11| =−= −−−−

( ) ( ) ( )( ) LiL

WL

WxWx mi

mL

i

ikk

mikk 2,,1

2

1&ˆˆ 0

2

01|1| =

+=

+== ∑

=−− λλ

λ

0

1

2

( ) ( ) ( ) ( ) ( )( ) LiL

WL

WxxxxWP ci

cL

i

T

kki

kkkki

kkc

ikk 2,,12

1&1ˆˆˆˆ 2

0

2

01|1|1|1|1| =

+=+−+

+=−−= ∑

=−−−−− λ

βαλ

λ

98

Uscented Kalman FilterSOLOUKF Summary (continue – 1)

Measure Prediction

( ) Lixkhz ikk

ikk 2,,1,0ˆ,ˆ 1|1| == −−

( ) ( ) ( )( ) LiL

WL

WzWz mi

mL

i

ikk

mikk 2,,1

2

1&ˆˆ 0

2

01|1| =

+=

+== ∑

=−− λλ

λ

3

Innovation and its Covariance4

1|ˆ −−= kkkk zzi

( ) ( ) ( ) ( ) ( )( ) LiL

WL

WzzzzWPS ci

cL

i

T

kki

kkkki

kkc

izzkkk 2,,1

2

1&1ˆˆˆˆ 2

0

2

01|1|1|1|1| =

+=+−+

+=−−== ∑

=−−−−− λ

βαλ

λ

Kalman Gain Computations5( ) ( ) ( ) ( ) ( )

( ) LiL

WL

WzzxxWP ci

cL

i

T

kki

kkkki

kkc

ixzkk 2,,1

2

1&1ˆˆˆˆ 2

0

2

01|1|1|1|1| =

+=+−+

+=−−= ∑

=−−−−− λ

βαλ

λ

1

1|1|

−−−= zz

kkxzkkk PPK

Update State and its Covariance6kkkkkk iKxx += −1|| ˆˆ

Tkkkkkkk KSKPP −= −1||

k = k+1 & return to 1

99

Unscented Kalman FilterState Estimation (one cycle)


Formation


Association




GatingComputations



SOLO

Simon J. Julier Jeffrey K. Uhlman

100

Estimators


xxx =−= &:

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( ) ( )kkvkkv

kvkxkHkz

kwkkukGkxkkx

ξ+Ψ=++=

Γ++Φ=+

1

1

SOLO


Assume a discrete dynamic system

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=

( ) ( ) ( ) ( ) ( ) ( ) lkT kRlekeEkvEkvke ,

0

&: δξξξ =−=

( ) ( ) 0=lekeE Tw ξ

=≠

=lk

lklk 1

0,δ

Solution

Define a new “pseudo-measurement”:

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]kvkxkHkkvkxkHkzkkzk +Ψ−++++=Ψ−+= 1111:ζ

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( )( )

( ) ( ) ( )kxkHkkvkkvkwkkukGkxkkHk

Ψ−Ψ−++Γ++Φ+=

ξ

11

( ) ( ) ( ) ( )[ ]( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( )

kkH

kkwkkHkukGkHkxkHkkkHε

ξ+Γ++++Ψ−Φ+= 111*

( ) ( ) ( ) ( ) ( ) ( ) ( )kkukGkHkxkHk εζ +++= 1*

101

Estimators


xxx =−= &:

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )111211*1

1

++++++++=+Γ++Φ=+

kkukGkHkxkHk

kwkkukGkxkkx

εζ

SOLO


The new discrete dynamic system:

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) lklk

TTT kRlHlkQkkHlekeE

kEkwEkkHkke

,,11&

1:

δδ

ξε

εε

ε

++ΓΓ+=

+Γ+−=

( ) ( ) 0=lekeE Tw ξ

=≠

=lk

lklk 1

0,δ

Solution (continue – 1)

( ) ( ) ( ) ( ) ( )kkwkkHk ξε +Γ+= 1:

( ) ( ) ( ) ( ) ( )kHkkkHkH Ψ−Φ+= 1:*

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) lkTTTTTTT kHkkQllHllwkwElkwE ,11 δξε +Γ=++Γ=

To decorrelate measurements and system noises write the discrete dynamic system:

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]

0

1*

1

kkukGkHkxkHkkD

kwkkukGkxkkx

εζ ++−−+Γ++Φ=+

102

Estimators

( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]kRkHkkQkkHkDkHkkQkkkkDkwkE TTTTT ++ΓΓ+−+ΓΓ==−Γ 1110εε

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) lklkTTT kRkRkHkkQkkHlkE ,, *:11 δδεε =++ΓΓ+=

SOLO


The new discrete dynamic system: Solution (continue – 2)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )111211*1

1*1

++++++++=+−Γ+

+−−++Φ=+

kkukGkHkxkHk

kkDkwk

kukGkHkxkHkkDkukGkxkkx

εζε

ζ

To de-correlate measurement and system noises choose D (k) such that:

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) lkTTTTTTT kHkkQllHllwkwElkwE ,11 δξε +Γ=++Γ=

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] 1111

−++ΓΓ++ΓΓ= kRkHkkQkkHkHkkQkkD TTTT

The Discrete Kalman Filter Estimator is: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( ) ( ) 000|0ˆ

1|ˆ*|ˆ|1ˆ

xxEx

kukGkHkkxkHkkDkukGkkxkkkx

==+−−++Φ=+ ζ

( ) ( ) ( ) ( ) ( )kHkkkHkH Ψ−Φ+= 1:*

The Aprior Covariance Update is:

( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( )( ) 00|0

**|1*|1

PP

kDkRkDkkQkkHkDkkkPkHkDkkkP TTT

=+ΓΓ+−Φ+−Φ=+

103

Estimators

( ) ( ) ( ) ( )[ ] ( ) 0=−Γ kkkDkwkE Tεε( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) lklk

TTT kRkRkHkkQkkHlkE ,, *:11 δδεε =++ΓΓ+=

SOLO


The discrete dynamic system: Solution (continue – 3)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( ) ( ) ( ) ( ) ( ) ( ) ( )111211*1

1*1

++++++++=++−−++Φ=+

kkukGkHkxkHk

kukGkHkxkHkkDkukGkxkkx

εζζ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] 1111

−++ΓΓ++ΓΓ= kRkHkkQkkHkHkkQkkD TTTT

The Discrete Kalman Filter Estimator is: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( ) ( ) 000|0ˆ

1/ˆ*|ˆ|1ˆ

xxEx


==+−−++Φ=+ ζ

( ) ( ) ( ) ( ) ( )kHkkkHkH Ψ−Φ+= 1:*

( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( )( ) 00|0

**|1*|1

PP

kDkRkDkkQkkHkDkkkPkHkDkkkP TTT

=+ΓΓ+−Φ+−Φ=+

( ) ( ) ( ) ( ) ( )[ ]( ) ( ) ( ) ( )kRkHkkPkK

kHkRkHkkPkkPT

T

1

111

**1|11

***|11|1−

−−−

++=+++=++

( ) ( ) ( ) ( ) ( ) ( )[ ]kkxkHkkKkkxkkx |1ˆ1*11|1ˆ1|1ˆ ++−++++=++ ζ

Summary:

104

Estimators

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( ) ( ) 000|0ˆ

1|ˆ*|ˆ|1ˆ

xxEx


==+−−++Φ=+ ζ

SOLO

Kalman Filter Discrete Case & Colored Measurement NoiseSolution (continue – 4)

Summary:

( ) ( ) ( ) ( ) ( ) ( )[ ]kkxkHkkKkkxkkx |1ˆ1*11|1ˆ1|1ˆ ++−++++=++ ζ

( ) ( ) ( ) ( )kzkkzk Ψ−+= 1ζ

Table of Content

105

Estimators

( ) ( ) ( ) ( )[ ]∫ −+−=t

t

dtntHty0

λλλλ s

SOLO

Optimal State Estimation in Linear Stationary Systems

The output of the Stationary Filter is given by:

Hnxn (t) is the impulse response matrix of the Stationary Filter

( ) ( )[ ] ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )tytteteteEtyttytE iT

iT

i −==−− yyy :

We want to estimate a vector signal that, after be corrupted by noise , passes trough a Linear Stationary Filter. We want to design the filter in order to estimate the signal using only the measured filter output vector .( )tyn 1×

( )tsn 1× ( )tnn 1×

( )tsn 1×

nnx1 (t) is a noise with autocorrelationand uncorrelated to the signal

( ) ( ) ( ) ( )τττ −=+= tRtntnER nnT

nn

( ) ( ) ( ) ( ) 0=+=+ ττ tstnEtntsE TT

( ) ( ) ( ) ( ) teteEtraceteteE TT =Where the trace of a square matrix A = ai,j is the sum of the diagonal terms

∑=

=× ==n

iiinjijinn aatraceAtrace

1,,,1,, :

( ) ( ) ( )∫ −=t

t

i dtIty0

λλλ s

The uncorrupted signal is observed through a linear system, with impulse response I (t) and output yi (t):We want to choose a Stationary Filter that minimizes:

106

EstimatorsSOLO

Optimal State Estimation in Linear Stationary Systems (continue – 1)

The Autocorrelation of the error is:

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ] ( )

( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( )

−+−−+−−+

−−−−−=

−++−−+

+−−−=

+=

∫∫∫∫

∫∫∫∫∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

22222221111111

22222221111111

ξξτξξξτξτξξξξξξξξ

ξξτξξξξτξξξξξξξξ

ττ

dtHndtHtIdntHdtHtIE

dtHndtIdntHdtIE

teteER

TTTTT

TTTT

Tee

ss

ssss

Therefore

( ) ( ) ( )[ ] ( ) ( )[ ]( )

( ) ( )[ ]

( ) ( ) ( )[ ]( )

( )∫ ∫

∫ ∫∞+

∞−

∞+

∞− −

+∞

∞−

+∞

∞− −

−+−+

−+−−+−−−=

212211

21222111

21

21

ξξξτξξξ

ξξξτξτξξξξτ

ξξ

ξξ

ddtHnnEtH

ddtHtIEtHtIR

T

R

T

TT

R

Tee

nn

ss

ss

107

Estimators

( ) ( ) ( )[ ] ( ) ( )[ ]( )

( ) ( )[ ]

( ) ( ) ( )[ ]( )

( )∫ ∫

∫ ∫∞+

∞−

∞+

∞− −

+∞

∞−

+∞

∞− −

−+−+

−+−−+−−−=

212211

21222111

21

21

ξξξτξξξ


ξξ

ξξ

ddtHnnEtH

ddtHtIEtHtIR

T

R

T

TT

R

Tee

nn

ss

ss

( ) ( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( )sSssssSsssS TTT −+−−−−= HHHIHI nnssee

SOLOOptimal State Estimation in Linear Stationary Systems (continue – 2)


Using the Bilateral Laplace Transform we obtain:( ) ( ) ( )

( ) ( )[ ] ( ) ( ) ( )[ ] ( )

( ) ( ) ( )∫ ∫

∫ ∫ ∫

∫

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

+∞

∞−

−+−−+

−−+−−+−−−−=

−=

212211

21222111 exp

exp

ξξξτξξξ

τξξτξτξτξξξξ

τττ

ddtHRtH

dddstHtIRtHtI

dsRsS

Tnn

TT

eeee

ss

( ) ( )[ ] ( )[ ] ( ) ( )[ ] ( ) ( )[ ] ( )[ ]

( ) ( )

( ) ( )[ ] ( ) ( )[ ] ( ) ( )[ ]

( )

∫ ∫ ∫

∫ ∫ ∫

∞+

∞−

∞+

∞−

−

∞+

∞−

+∞

∞−

+∞

∞−

−−

+∞

∞−

−+−+−−−−−−+

−+−+−−+−−−−−−−−=

222212111

122222121111

expexpexp

expexpexp

ξτξτξτξξξξξξ

ξξτξτξτξτξξξξξξξ

ddsttHsRsttH

dddsttHtIsRsttHtI

s

T

ss

TT

T

TT

H

nn

H-I

ss

108

Estimators

( ) ( ) ( )[ ] ( ) ( )[ ]( )

( ) ( )[ ]

( ) ( ) ( )[ ]( )

( )∫ ∫

∫ ∫∞+

∞−

∞+

∞− −

+∞

∞−

+∞

∞− −

−+−+

−+−−+−−−=

212211

21222111

21

21

ξξξτξξξ


ξξ

ξξ

ddtHnnEtH

ddtHtIEtHtIR

T

R

T

TT

R

Tee

nn

ss

ss

( ) ( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( )sSssssSsssS TTT −+−−−−= HHHIHI nnssee

SOLO



Using the Bilateral Laplace Transform we finally obtained:

where

( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( ) ( )

( ) ( ) ( )( )

( ) ( ) ( )ns

rrrrrrrr

rrrrrrrrrr

rrrr

rrrr

,

expexp

expexpexp

=

=−=−=

−==−−=−=

∫∫

∫∫∫∞+

∞−

=∞+

∞−

+ ∞

∞−

−=+ ∞

∞−

−=+ ∞

∞− r

sSdsRdsRsS

sSdsRdsRdsRsS

RRTT

RR

T

ττττττ

υυυττττττ

τ

τυττ

109

EstimatorsSOLO


( )( ) ( )

( )( ) ( )

( )( )0minminmin === τee

tH

T

tH

T

tHRtraceteteEtraceteteE

( ) ( ) ( ) ( )∫∫∞+

∞−=

∞+

∞−

===j

j

ee

j

j

eeee dssSj

dsssSj

Rπ

τπ

ττ

2

1exp

2

10

0

We want to find the Optimal Stationary Filter, ,that minimizes:( )tH

( )( ) ( )

( )( )

( )( )

( )( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( ) ∫

∫∞+

∞−

∞+

∞−

−+−−−=

===

j

j

TTT

s

eetH

j

j

eetH

T

tH

dssSssssSssj

trace

RtracedssSj

traceteteE

HHHIHI nnssH π

τπ

2

1min

0min2

1minmin

Using Calculus of Variation we write ( ) ( ) ( ) 0ˆ →Ψ+= εε sss HH

( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ] ( ) ( )[ ] ( ) ( )[ ]

( ) ( ) ( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) 0ˆˆ2

1ˆˆ2

1

ˆˆˆˆ2

10

=−Ψ+−−+−+−−−−Ψ=

−Ψ+−Ψ++−Ψ−−−−Ψ−−∂∂

∫∫

∫∞+

∞−

∞+

∞−

∞+

∞−→

j

j

Tj

j

TTT

j

j

TTTTT

dsssSssSssj

tracedssSsssSsj

trace

dsssSssssssSsssj

trace

επ

επ

εεεεεπ ε

nnssnnss

nnss

HHIHHI

HHHIHI

110

EstimatorsSOLO


( ) ( ) ( ) ( )[ ] ( )

( ) ( )[ ] ( ) ( ) ( ) ( ) 0ˆˆ2

1

ˆˆ2

1

=−Ψ+−−+

−+−−−−Ψ

∫

∫∞+

∞−

∞+

∞−

j

j

T

j

j

TTT

dsssSssSssj

trace

dssSsssSsj

trace

επ

επ

nnss

nnss

HHI

HHI

Since by tacking –s instead of s in one of the integrals we obtain the other, they are equaland have zero value:

( ) ( )[ ] ( ) ( ) ( ) ( ) 0ˆˆ2

1 =−Ψ+−−∫∞+

∞−

j

j

T dsssSssSssj

trace επ nnss HHI

This integral is zero for all if and only if: ( ) 0≠−Ψ sT

( ) ( )[ ] ( ) ( ) ( ) 0ˆˆ =+−− sSssSss nnss HHI ( ) ( ) ( )[ ] ( ) ( )sSssSsSs ssnnss IH =+ˆ

Since we can perform a Spectral Decomposition: ( ) ( ) ( ) ( )[ ] TsSsSsSsS −+−=+ nnssnnss

( ) ( ) ( ) ( )sssSsS T −∆∆=+ nnss( )s∆ - All poles and zeros are in L.H.P s.

- All poles and zeros are in R.H.P s.( )sT −∆

( ) ( ) ( ) ( ) ( )sSssss TssIH =−∆∆ˆ ( ) ( ) ( ) ( )[ ] ( )sssSss T 1

PartRealizable

ˆ −− ∆−∆= ssIH

111

Estimators

( ) ( ) ( ) 1&11

3

1

3

1

32

==+−

=−

= sIsSsss

sS nnss

( ) ( ) ( ) ( )[ ] ( )sssSss T 1

PartRealizable

ˆ −− ∆−∆= ssIH

SOLO


Example 8.3-2 Sage, “Optimum System Control”, Prentice Hall, 1968, pp.191-192

( ) ( )( ) ( )

ss T

s

s

s

s

s

s

ssSsS

−∆∆

−−

++=

−−=+

−=+

1

2

1

2

1

41

1

32

2

2nnss

( ) ( ) ( )[ ] ( ) ( )

Partrealizable-Un

PartRealizable

2 2

1

1

1

21

3

2

1

1

3

sssss

s

sssSs T

−+

+=

−+=

−−

−=−∆−

ssI

( )ss

s

ss

+=

++

+=

2

1

2

1

1

1H

Solution:

( )( ) ( )

( )( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( )

( ) ( ) ( ) ( ) 12

4

2

4

22

4

2

1

ˆˆˆˆ2

1

2

1minmin

=+

=−

=−+

=

−+−−−==

∫∫∫

∫∫∞+

∞−

∞+

∞−

∞+

∞−

RHPLHP

j

j

j

j

TTTj

js

T

tH

dss

dss

dsssj

dssSssssSssj

tracedsSj

traceteteE

π

ππHHHIHI nnssee

H

112

Estimators

vxy

wBxAx

+=+=

SOLO



Solution:

( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) 21

21

21

2121

2121 ,0

tttvtwE

twtvE

ttRtvtvE

ttQtwtwET

T

T

T

∀

==

−=

−=

δδ

( ) ( ) ( ) ( ) ( ) 1&& === tItvtntxts

( ) ( ) ( )sWBAsIsS 1−−= ( ) ( ) ( ) ( ) ( ) TTT AsIBQBAsIsSsSsS −− −−−=−= 1ss

( ) RsS =nn

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )[ ] ( )( ) [ ] [ ] ( ) TT

TTT

TTT

AsIRsRsAsI

AsIAsIRAsIBQBAsI

AsIBQBAsIsssSsS

−−

−−

−−

−−−−−−=

−−−−−+−=

−−−=−∆∆=+

ΤΤ

nnss

2/12/11

1

1

where

RAARRR

ARABQBTT

TTT

+−=−+=

2/12/1 ΤΤ

ΤΤPRAR

PPPARR TTT

+−==−−=

2/1

2/1 &

Τ

Τ

( ) ( ) ( ) ( ) ( )( ) ( )[ ] ( ) ( )( ) ( )T

TT

−−=

+−−=−−−=

−−=−−=∆

−

−−−−

−−−

2/11

2/1112/111

2/112/112/11

RsAsI

RRPAsIAsIRRPRAIsAsI

RRRIsAsIRsAsIs

113

Estimators

vxy

wBxAx

+=+=

( ) ( ) ( ) ( ) ( ) [ ] [ ] ( ) TTT AsIRsRsAsIsssSsS −− −−−−−−=−∆∆=+ ΤΤnnss2/12/11

( ) ( ) ( )[ ] ( ) ( )( )

( ) ( )( )

( ) ( )

realizableUn

12/1

Realizable

1

12/11

−

−−

−∆

−−−−

−−−=

−−−−−−−=−∆−

TT

s

TT

sS

TTT

sRBQBAsI

sRAsIAsIBQBAsIssSsT

T

TI

ss

ss

SOLO



Solution (continue - 1):

( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) 21

21

21

2121

2121 ,0

tttvtwE

twtvE

ttRtvtvE

ttQtwtwET

T

T

T

∀

==

−=

−=

δδ

( ) ( ) ( ) ( ) ( ) 1&& === tItvtntxts

Let decompose the last expression in the Realizable and Un-realizable parts:

( ) ( ) ( ) ( )TTTT

TTT

ARABQBPRPAPPAARA

PARRPRARRR

+=−−−=−−==

−

−−

1

12/112/1 ΤΤΤΤ

01 =+−+ − BQBPRPAPPA T

( ) ( ) ( ) ( )

realizableUn

12/1

Realizable

1

realizableUn

12/1

Realizable

1

−

−−

−

−− −−+−=−−− TTT sRNMAsIsRBQBAsI TT where M and N must be defined

114

Estimators

vxy

wBxAx

+=+=

01 =+−+ − BQBPRPAPPA T

SOLO




( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) 21

21

21

2121

2121 ,0

tttvtwE

twtvE

ttRtvtvE

ttQtwtwET

T

T

T

∀

==

−=

−=

δδ

( ) ( ) ( ) ( ) ( ) 1&& === tItvtntxts

Let decompose the last expression in the Realizable and Un-realizable parts:( ) ( ) ( ) ( )

realizableUn

12/1

Realizable

1

realizableUn

12/1

Realizable

1

−

−−

−

−− −−+−=−−− TTT sRNMAsIsRBQBAsI TTwhere M and N must be defined

Pre-multiply this equality by (sI-A) and post-multiply by (-s R1/2 –TT) to obtain

( ) ( ) NAsIsRMBQB TT −+−−= T2/1

2/12/1 0 RMNNRM =⇒=−2/1RMAMNAMBQB TTT −−=−−= TT

PPPARR TTT =−−= &2/1 Τ

( ) 2/12/12/11 RMAPRARMPRPAPPA TT −−−−=+−− −−

( ) ( ) ( ) 012/12/12/1 =−+−−−− − PRRMPARMPRMPA T 2/1−= RPM PN =

115

Estimators

vxy

wBxAx

+=+=

( ) ( ) ( ) ( )[ ] ( ) ( )( ) ( ) ( )[ ]

( ) ( )( )

sssSs

T AsIRPAIsRRPAsIsssSssT 1

PartRealizable

112/12/111

PartRealizable

ˆ−− ∆

−−−

−∆

−−−− −+−−=∆−∆=ssI

ssIH

( ) ( ) ( ) ( ) ( )2/12/12/112/11 −−− +−−=−−=∆ RPRARsAsIRsAsIs Τ

SOLO




( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) 21

21

21

2121

2121 ,0

tttvtwE

twtvE

ttRtvtvE

ttQtwtwET

T

T

T

∀

==

−=

−=

δδ

( ) ( ) ( ) ( ) ( ) 1&& === tItvtntxts

Decompose the last expression in the Realizable and Un-realizable parts:

( ) ( ) ( )[ ] ( ) ( ) ( ) ( )

realizableUn

12/1

Realizable

2/11

realizableUn

12/1

Realizable

1

−

−−−

−

−−− −−+−=−−−=−∆ TTTT sRPRPAsIsRBQBAsIssSs TTI ss

PRAR +−=2/1Τ

116

Estimators

vxy

wBxAx

+=+=

( ) ( ) ( ) ( )[ ] ( ) ( )( ) ( ) ( )[ ]

( ) ( )( )

sssSs

T AsIRPAIsRRPAsIsssSssT 1

PartRealizable

112/12/111

PartRealizable

ˆ−− ∆

−−−

−∆

−−−− −+−−=∆−∆=ssI

ssIH

SOLO




( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) 21

21

21

2121

2121 ,0

tttvtwE

twtvE

ttRtvtvE

ttQtwtwET

T

T

T

∀

==

−=

−=

δδ

( ) ( ) ( ) ( ) ( ) 1&& === tItvtntxts

( ) ( ) ( ) ( )[ ] ( ) ( )[ ]( ) ( )[ ] ( ) ( ) ( ) ( )[ ]

( ) ( )[ ] ( ) ( ) ( )[ ]( ) ( )[ ] ( ) 111

1111

1111

111

11111

111111

1111111111ˆ

−−−−

−−−

−−−−−−−

−−−−−−−−−−−

−−−−−−−−−−

+−=+−=

+−=−−+=

−+−=−+−=

−+−=+−−−=

RPRPAsIRPAsIRP

IAsIRPAsIAsIRP

AsIRPAsIRPRPAsIIAsI

RPAsIIRPAsIRPAsIAsIRPAsIsH

Finally: ( ) ( ) 111ˆ −−−+−= RPRPAsIsH

01 =+−+ − BQBPRPAPPA Twhere P is given by:Continuous Algebraic

Riccati Equation (CARE)

( )xyRPxAx ˆˆˆ 1 −+= −

These solutions are particular solutions of the Kalman Filter algorithm for aStationary System and infinite observation time (Wiener Filter) Table of Content

117

Estimators

( ) ( ) ( ) ( ) ( ) ( )twtGtxtFtxtxtd

d +==

SOLO

Kalman Filter Continuous Time Case

Assume a continuous time linear dynamic system

( ) vxtHz +=( ) ( ) ( ) ( ) ( ) ( )tPteteEtxEtxte T

xxx =−= &:( ) ( ) ( ) ( ) ( ) ( ) ( )21121

0

&: tttQteteEtwEtwte Twww −=−= δ

( ) ( ) ( ) ( ) ( )∫+=t

t

dztAtxttBtx0

,ˆ,ˆ 00 τττ

( ) ( ) ( ) ( ) ( ) ( ) ( )21121

0

&: tttRteteEtvEtvte Tvvv −=−= δ

( ) ( ) 021 =teteE T

wv

Let find a Linear Filter with the state vector that is a function of Z (t) (the historyof z for t0 < τ < t )

( )tx

s.t. will minimize

( ) ( )[ ] ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )txtxtxwheretxtxEtxtxtxtxEJ TT −==−−= ˆ:~~~ˆˆ

( ) ( ) txEtxE =ˆ Unbiased Estimator

( ) ( ) ( ) 0ˆ~ =−= txEtxEtxE

118

EstimatorsSOLO

Kalman Filter Continuous Time Case (continue – 1)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) txtxEdtxzEtAttBtxtxE

dtAztxEttBtxtxEttBttBtxtxEttB

dtxzEtAdtAztxEddtAzzEtA

txdztAtxttBtxdztAtxttBEtxtxE

Tt

t

TTT

t

t

TTTTT

t

t

Tt

t

TTt

t

t

t

TT

Tt

t

t

t

T

+−−

−−+

−−=

−+

−+=

∫

∫

∫∫∫ ∫

∫∫

0

0

000 0

00

0

00

0

0

0

00

0

000000

0000

ˆ,,ˆ

,ˆ,ˆ,,ˆˆ,

,,,,

,ˆ,,ˆ,~~

τττ

λλλ

ττττττλτλλττ

ττττττ

( ) ( )[ ] ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )txtxtxwheretxtxEtxtxtxtxEJ TT −==−−= ˆ:~~~ˆˆ

( ) ( ) ( ) 0ˆ~ =−= txEtxEtxE

119

EstimatorsSOLO


( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )0000

0000

,ˆˆ,,,

,,

,ˆ,,ˆ,~~

0 0

00

00

ttBtxtxEttBddtAzzEtA

dtxzEtAdtAztxEtxtxE

dztAtxttBtxdztAtxttBtxEtxtxEJ

TTt

t

t

t

TT

t

t

Tt

t

TTT

Tt

t

t

t

T

++

−−=

−−

−−==

∫ ∫

∫∫

∫∫

λτλλττ

ττττττ

ττττττ

Let use Calculus of Variation to find the minimum of J:

( ) ( ) ( ) ( ) ( ) ( )τνετττηεττ ,,ˆ,&,,ˆ, ttBtBttAtA +=+=

( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 0,ˆˆ,ˆ,ˆˆˆ,

,,ˆ,ˆ,

,,

000000000000

0

0 00 0

00

=++

++

−−=∂

∂

∫ ∫∫ ∫

∫∫=

tttxtxEttBttBtxtxEtt

ddtzzEtAddtAzzEt

dtxzEtdtztxEJ

TTTT

t

t

t

t

TTt

t

t

t

TT

t

t

Tt

t

TT

νν

λτληλττλτλλττη

τττηττητεε

ε

120

( ) ( ) ( ) ( ) ( ) ( ) ( ) λ

λλττλλ

<<

=−= ∫tt

dzzEtAztxEztxEt

t

TTT

0

0,ˆ~

0

EstimatorsSOLO


( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 0,ˆˆ,ˆ,,ˆ

,ˆˆ,ˆ,,ˆ

000000

000000

0

0 0

0 0

=

+

−−+

+

−−=

∂∂

∫ ∫

∫ ∫=

T

TTt

t

Tt

t

TT

TTt

t

Tt

t

TT

tttxtxEttBdtdzzEtAztxE

tttxtxEttBdtdzzEtAztxEJ

νλλητλττλ

νλλητλττλεε

ε

This is possible for all η (t,τ), ν (t,t0) iff

( ) 0,ˆ& 0 =ttB

From this we can see that: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )∫−−=−=t

t

dztAtxttBtxtxtxtx0

,ˆˆ,ˆˆ~0

0

0 τττ

Orthogonal Projection Theorem

Wiener-Hopf Equation


Eberhard Frederich Ferdinand Hopf

1902 - 1983

( ) ( ) ( ) ( ) ( ) λτλττλ <<= ∫ ttdzzEtAztxEt

t

TT0

0

,ˆ

121

EstimatorsSOLO


Solution of Wiener-Hopf Equation ( ) ( ) ( ) ( ) ( ) λτλττλ <<= ∫ ttdzzEtAztxEt

t

TT0

0

,ˆ

Let Differentiate the Wiener-Hopf Equation relative to t:

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( ) ( )

0

λλλλτ TTTTT ztwEtGztxEtFztwtGtxtFEztxtd

dEztxE

t+=+=

=∂∂

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∫∫ ∂∂+=

∂∂ t

t

TTt

t

T dzzEtAt

ztzEttAdzzEtAt

00

,ˆ,ˆ,ˆ τλττλτλττ

therefore( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∫ ∂

∂+=t

t

TTT dzzEtAt

ztzEttAztxEtF0

,ˆ,ˆ τλττλλ

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

0

,ˆ,ˆ,ˆ,ˆ λλλλ TTTT ztvEttAztxEtHttAztvtxtHEttAztzEttA +=+=

Now ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∫=t

t

TT dzzEtAtFztxEtF0

,ˆ τλττλ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 0,ˆ,ˆ,ˆ,ˆ

0

=

∂∂−−∫

t

t

T dzzEtAt

tAtHttAtAtF τλττττ

122

EstimatorsSOLO


Solution of Wiener-Hopf Equation(continue – 1)


t

TT0

0

,ˆ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 0,ˆ,ˆ,ˆ,ˆ

0 0

=

∂∂−−∫

≠

t

t

T dzzEtAt

tAtHttAtAtF τλττττ

( ) ( ) ( ) ( ) ( ) ( ) 0,ˆ,ˆ,ˆ,ˆ =∂∂−− τττ tAt

tAtHttAtAtFThis is true only if

Define ( ) ( )ttAtK ,ˆ:=

The Optimal Filter was found to be: ( ) ( ) ( )∫=t

t

dztAtx0

,ˆˆ τττ

( ) ( ) ( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( )( )

( ) ( ) ( )

( ) ( ) ( ) ( ) ( )[ ] ( ) ( )

( )

( ) ( ) ( ) ( ) ( ) ( )[ ]txtHtztKtxtFdztAtHtKtFtztK

dztAtHttAtAtFtzttAdztAt

tzttAtxtd

d

tx

t

t

t

t tKtK

t

t

ˆˆ,ˆ

,ˆ,ˆ,ˆ,ˆ,ˆ,ˆˆ

ˆ

0

00

−+=−+=

−+=

∂∂+=

∫

∫∫

τττ

τττττττ

Therefore the Optimal Filter is given by: ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]txtHtztKtxtFtxtd

dˆˆˆ −+=

123

EstimatorsSOLO


Solution of Wiener-Hopf Equation(continue – 2)


t

TT0

0

,ˆ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∫ ∂∂+=

t

t

TTT dzzEtAt

ztzEttAztxEtF0

,ˆ,ˆ τλττλλ

( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( )λλλλλλ TTTT HxtxEvxHtxEztxE =+=Now

( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( )

0→<

−+=++=λ

λδλλλνλλνλt

TTTT ttRHxtxEtHxHttxtHEztzE

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )∫+=t

TTTTT dGQGttxxEtxtxEλ

γγλϕγγγγϕλϕγγγϕλ ,,,,

( ) ( ) ( )∫=t

t

dztAtx0

,ˆˆ τττ ( ) ( ) ( ) ( ) ( ) λτλττλ <<= ∫ ttdzzEtAztxEt

t

TT0

0

,ˆˆ

Must prove that

( ) ( ) ( ) ( ) ( )tRtHtPttAtK T 1,ˆ −==

Table of Content

124

Eberhard Frederich Ferdinand Hopf

1902 - 1983

In 1930 Hopf received a fellowship from the Rockefeller Foundation to study classical mechanics with Birkhoff at Harvard in the United States. He arrived Cambridge, Massachusetts in October of 1930 but his official affiliation was not the Harvard Mathematics Department but, instead, the Harvard College Observatory. While in the Harvard College Observatory he worked on many mathematical and astronomical subjects including topology and ergodic theory. In particular he studied the theory of measure and invariant integrals in ergodic theory and his paper On time average theorem in dynamics which appeared in the Proceedings of the National Academy of Sciences is considered by many as the first readable paper in modern ergodic theory. Another important contribution from this period was the Wiener-Hopf equations, which he developed in collaboration with Norbert Wiener from the Massachusetts Institute of Technology. By 1960, a discrete version of these equations was being extensively used in electrical engineering and geophysics, their use continuing until the present day. Other work which he undertook during this period was on stellar atmospheres and on elliptic partial differential equations.

On 14 December 1931, with the help of Norbert Wiener, Hopf joined the Department of Mathematics of the Massachusetts Institute of Technology accepting the position of Assistant Professor. Initially he had a three years contract but this was subsequently extended to four years (1931 to 1936). While at MIT, Hopf did much of his work on ergodic theory which he published in papers such as Complete Transitivity and the Ergodic Principle (1932), Proof of Gibbs Hypothesis on Statistical Equilibrium (1932) and On Causality, Statistics and Probability (1934). In this 1934 paper Hopf discussed the method of arbitrary functions as a foundation for probability and many related concepts. Using these concepts Hopf was able to give a unified presentation of many results in ergodic theory that he and others had found since 1931. He also published a book Mathematical problems of radiative equilibrium in 1934 which was reprinted in 1964. In addition of being an outstanding mathematician, Hopf had the ability to illuminate the most complex subjects for his colleagues and even for non specialists. Because of this talent many discoveries and demonstrations of other mathematicians became easier to understand when described by Hopf.

http://www-groups.dcs.st-and.ac.uk/~history/Biographies/Hopf_Eberhard.html

125

Estimators

( ) ( ) ( ) ( ) ( ) ( )twtGtxtFtxtxtd

d +==

SOLO

Kalman Filter Continuous Time Case (Second Way)

Assume a continuous time dynamic system

( ) vxtHz +=( ) ( ) ( ) ( ) ( ) ( )tPteteEtxEtxte T

xxx =−= &:( ) ( ) ( ) ( ) ( ) ( ) ( )21121

0

&: tttQteteEtwEtwte Twww −=−= δ

( ) ( ) ( ) ( ) ( ) ( ) ( )21121

0

&: tttRteteEtvEtvte Tvvv −=−= δ

( ) ( ) 021 =teteE T

wv

Let find a Linear Filter with the state vector that is a function of Z (t) (the historyof z for t0 < τ < t ). Assume the Linear Filter:

( )tx

( ) ( ) ( ) ( ) ( ) ( )tztKtxtKtxtxtd

d +== ˆ'ˆˆ

where K’(t) and K (t) will be chosen such that:

1 The Filter is Unbiased: ( ) ( ) txEtxE =ˆ

2 The Filter will yield a maximum rate of decrease of the error by minimizingthe scalar cost function:

( ) ( )[ ] ( ) ( )[ ] ( )tPdt

dtracetxtxtxtxE

dt

dtraceJ

KK

T

KKKK ',',',minˆˆminmin =−−=

126

Estimators

( ) ( ) ( ) ( ) ( )twtGtxtFtx +=

SOLO

Kalman Filter Continuous Time Case (Second Way – continue - 1)

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]tvtxtHtKtxtKtx ++= ˆ'

1 The Filter is Unbiased: ( ) ( ) txEtxE =ˆ

Solution

Define ( ) ( ) ( )txtxtx −= ˆ:~

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )twtGtvtKtxtFtHtKtKtxtKtx −+−++= '~'~

( ) ( ) ( ) 0ˆ~ =−= txEtxEtxE

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )

0000

'~'~ twEtGtvEtKtxEtFtHtKtKtxEtKtxE −+−++=

We can see that the necessary condition for an unbiased estimator is:

( ) ( ) ( ) ( )tHtKtFtK −='

Therefore: ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )twtGtvtKtxtHtKtFtx −+−= ~~

and the Unbiased Filter has the form:

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]txtHtztKtxtFtx ˆˆˆ −+=

127

EstimatorsSOLO

Kalman Filter Continuous Time Case (Second Way – continue - 2)

Solution

where: ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )twtGtvtKtxtHtKtFtx −+−= ~~

2 The Filter will yield a maximum rate of decrease of the error by minimizingthe scalar cost function:

( ) ( )[ ] ( ) ( )[ ] ( )tPdt

dtracetxtxtxtxE

dt

dtraceJ

K

T

KKminˆˆminmin =−−=

( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )[ ]( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )tGtwtwEtGtKtvtvEtK

tHtKtFtxtxEtHtKtFtxtxETTTT

TTT

++−−= ~~~~

( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) ( )tGtQtGtKtRtKtHtKtFtPtHtKtFtP TTT ++−−=

To obtain the optimal K (t) that minimize J (t) we perform: ( ) 0=∂∂=

∂∂

tPtraceKK

J

Using the Matrix Equation: we obtain ( )TT BBAABAtraceA

+=∂∂

( ) ( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) 022 =+−−=∂∂=

∂∂

tRtKtHtPtHtKtFtPtraceKK

J T

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] 1−+= tRtHtPtHtHtPtFtK TT

Table of Content

128

EstimatorsSOLO

Applications

Table of Content

129

EstimatorsSOLO

Multi-sensor Estimate

Consider a system comprised of two sensors,each making a single measurement, zi (i=1,2),of a constant, but unknown quantity, x, in thepresence of random, dependent, unbiasedmeasurement errors, vi (i=1,2). We want to design an optimal estimator that combines the two measurements.

( ) ( ) ( ) ( ) 11

0

01122112

22

22222

21

211111 ≤≤−=−−

=−=+=

=−=+=ρσσρ

σ

σvEvvEvE

vEvEvEvxz

vEvEvEvxz

In absence of any other information, we chose an estimator that combines, linearly,the two measurements:

2211ˆ zkzkx += where k1 and k2 must be found such that:

1. The Estimator is Unbiased: 0~ˆ ==− xExxE ( ) ( )

( ) ( ) 011

~ˆ

2121

0

22

0

11

2211

=−+=−+++=−+++==−

xkkxEkkvEkvEk

xvxkvxkExExxE

x

121 =+ kk

130

EstimatorsSOLO

Multi-sensor Estimate (continue – 1)

2211ˆ zkzkx +=

where k1 and k2 must be found such that:

1. The Estimator is Unbiased: 0~ˆ ==− xExxE 121 =+ kk

2. Minimize the Mean Square Estimation Error: ( ) 2

,

2

,

~minˆmin2121

xExxEkkkk

=−( ) ( ) ( ) ( )[ ] ( )[ ]

( ) ( ) ( ) ( )[ ]21112

22

12

12

121112

22

12

12

1

22111

22111

2

,

121min121min

1min1minˆmin

1

212

22

1

1

1121

σσρσσσσρσσ

kkkkvvEkkvEkvEk

vkvkExvxkvxkExxE

kk

kkkk

−+−+=

−+−+=

−+=−+−++=−

( ) ( )[ ] ( ) ( ) 0212122121 2112

212

1121112

22

12

12

11

=−+−−=−+−+∂∂ σσρσσσσρσσ kkkkkkkk

212

22

1

212

112

212

22

1

212

21

2ˆ1ˆ&

2ˆ

σσρσσσσρσ

σσρσσσσρσ

−+−=−=

−+−= kkk

( ) 22

21

212

22

1

222

212 ,

2

1~min σσσσρσσ

ρσσ ≤−+

−=xE Reduction of Covarriance Error

Estimator:

131

EstimatorsSOLO


212

11

22

21

12

11

22

112

11

22

21

12

11

21

2

212

22

1

212

11

212

22

1

212

2

22

22ˆ

zz

zzx

−−−−

−−−

−−−−

−−−

−+−+

−+−=

−+−+

−+−=

σσρσσσσρσ

σσρσσσσρσ

σσρσσσσρσ

σσρσσσσρσ

( ) ( ) 22

211

21

12

22

1

2

212

22

1

222

212 ,

2

1

2

1~min σσσσρσσ

ρσσρσσ

ρσσ ≤−+

−=−+

−= −−−−xE

1. Uncorrelated Measurement Noises (ρ =0)

( ) ( ) 2

122

21

221

122

21

21ˆ zzx

−−−−−−−− +++= σσσσσσ

0~min 2 =xE

2. Fully Correlated Measurement Noises (ρ =±1)

3.Perfect Sensor (σ 1 = 0)

1ˆ zx = 0~min 2 =xE The estimator will use the perfect sensor as expected.

212

11

12

112

11

11ˆ zzx −−

−

−−

−

+=σσ

σσσ

σ

132

EstimatorsSOLO


Consider a system comprised of n sensors,each making a single measurement, zi (i=1,2,…,n),of a constant, but unknown quantity, x, in thepresence of random, dependent, unbiasedmeasurement errors, vi (i=1,2,…,n). We want to design an optimal estimator that combines the n measurements.

nivEvxz iii ,,2,10 ==+=

or

[ ] [ ] RVEVVEVEVE

v

v

v

x

z

z

z

nnnnn

nn

nn

T

V

n

UZ

n

=

=−−=

+

=

22211

222

22112

1121122

1

2

1

2

1

0

1

1

1

σσσρσσρ

σσρσσσρ

σσρσσρσ

[ ] ZK

z

z

z

kkkzkzkzkx T

n

nnn =

=+++=

2

1

212211 ,,,ˆEstimator:

133

EstimatorsSOLO


ZKx T=ˆEstimator:

1. The Estimator is Unbiased:

( ) 01ˆ~

0

=+−=−+=−=VEKxUKxVKxUKExxExE TTTT

01 =−UK T

2. Minimize the Mean Square Estimation Error: ( ) 2

1

2

1

ˆmin~min xxExEUK

KUK

KTT

−===

( ) ( ) KRKKVVEKVKVKExE T

UKK

TT

UKK

TTT

UKK

UKK

TTTT 111

2

1

minminmin~min====

===

Use Lagrange multiplier λ (to be determined) to include the constraint 01 =−UK T

( ) ( )1−−= UKKRKKJ TT λ ( ) 0=−=∂∂

UKRKJK

λ

11 == − URUUK TT λ( ) URURUK T 111 −−−= ( ) 112

1

~min−−

=

= URUxE T

UKK

T

=

1

1

1

:

U

URK 1−= λ

Table of Content

134

SOLO RADAR Range-Doppler

Target Acceleration Models

Equation of motion of a point mass object are described by:

AIV

RI

V

R

td

d

x

x

xx

xx

+

=

33

33

3333

3333 0

00

0

A

V

R

- Range vector

- Velocity vector

- Acceleration vector

=

A

V

R

I

I

A

V

R

td

d

xxx

xxx

xxx

333333

333333

333333

000

00

00

or:

Since the target acceleration vector is not measurable, we assume that it is a random process defined by one of the following assumptions:

A

1. White Noise Acceleration Model .

3. Piecewise (between samples) Constant White Noise Acceleration Model .

5. Singer Acceleration Model .

2. Wiener Process acceleration model .

4. Piecewise (between samples) Constant Wiener Process Acceleration Model .

135


Target Acceleration Models (continue – 1)

1. White Noise Acceleration Model – Second Order Model

( ) ( ) ( ) ( ) ( )τδτ −==

+

=

tqwtwEtwEtw

IV

RI

V

R

td

d T

B

x

x

A

xx

xx

x

,0&0

00

0

33

33

3333

3333

Discrete System ( ) ( ) ( ) ( ) ( )kwkkxkkx Γ+Φ=+1

( ) [ ]

=+===Φ ∑∫

∞

= 3333

333366

00 0!

1exp:

xx

xxx

i

iiT

I

TIITAITA

idAT ττ

200

00

00

00

00

0

00

0

00

0

3333

3333

3333

3333

3333

3333

3333

33332

3333

3333 ≥∀

=→→

=

=→

= nA

IIA

IA

xx

xxn

xx

xx

xx

xx

xx

xx

xx

xx

( ) ( ) ( ) ( ) ( ) ( )∫ −Φ−Φ=ΓΓT

TTT dTBBTqkkwkwEk0

τττ ( ) ( ) ( )τδτ −= tqwtwE T

136



1. White Noise Acceleration Model (continue – 1)

( ) [ ] ( ) ττ

τd

ITI

II

II

TIIq

xx

xxxx

T

x

x

xx

xx

−

−= ∫

3333

33333333

0 33

33

3333

3333 00

0

0

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )∫ −Φ−Φ=ΓΓ=ΓΓT

TTTTT dTBBTqkkQkkkwkwEk0

τττ

( ) ( )[ ] ( ) ( )( )

ττ

τττττ

dITI

TITIqdITI

I

TIq

T

xx

xxxx

T

x

x ∫∫

−−−

=−

−=

0 3333

332

333333

0 33

33 2/

( ) ( ) ( )

=ΓΓ

TITI

TITIqkkQk

xx

xxT

332

33

233

333

2/

2/3/

Guideline for Choice of Process Noise Intensity

The change in velocity over a sampling period T are of the order of TqQ =22

For a nearly constant velocity assumed by this model, the choice of q must be suchto give small changes in velocity compared to the actual velocity . V

137



2. Wiener Process acceleration model – Third Order Model

( ) ( ) ( ) ( ) ( )τδτ −==

+

=

tIqwtwEtwEtw

IA

V

R

I

I

A

V

R

td

dx

T

B

x

x

x

A

xxx

xxx

xxx

x

33

33

33

33

333333

333333

333333

,0&0

0

000

00

00

Discrete System ( ) ( ) ( ) ( ) ( )kwkkxkkx Γ+Φ=+1

( ) [ ]

=++===Φ ∑∫

∞

=333333

333333

2333333

2299

00 00

0

2/

2

1

!

1exp:

xxx

xxx

xxx

xi

iiT

I

TII

TITII

TATAITAi

dAT ττ

2

000

000

000

000

000

00

000

00

00

333333

333333

333333

333333

333333

333333

2

333333

333333

333333

>∀

=→→

=→

= nA

I

AI

I

A

xxx

xxx

xxx

n

xxx

xxx

xxx

xxx

xxx

xxx

( ) ( ) ( ) ( ) ( ) ( )∫ −Φ−Φ=ΓΓT

TTT dTBBTqkkwkwEk0

τττ

Since the derivative of acceleration is the jerk, this model is also called White Noise Jerk Model.

( ) ( ) ( )τδτ −= tIqwtwE xT

33

138



2. Wiener Process Acceleration Model (continue – 1)

( ) ( )( ) [ ] ( )

( ) ( )τ

ττ

ττττ

d

ITITI

ITI

I

I

II

TII

TITII

q

xxx

xxx

xxx

xxx

T

x

x

x

xxx

xxx

xxx

−−

−

−−−

= ∫3333

233

333333

333333

333333

033

33

33

333333

333333

2333333

2/

0

00

000

0

00

0

2/

( ) ( ) ( ) ( ) ( ) ( )∫ −Φ−Φ=ΓΓT

TTT dTBBTqkkwkwEk0

τττ

( )( ) ( ) ( )[ ]

( ) ( ) ( )( ) ( ) ( )( ) ( )

τ

ττ

τττ

τττ

τττττ

d

ITITI

TITITI

TITITI

qdITITI

I

TI

TI

qT

xxx

xxx

xxx

xxx

T

x

x

x

∫∫

−−

−−−

−−−

=−−

−−

=0

33332

33

332

333

33

233

333

433

33332

33

033

33

233

2/

2/

2/2/4/

2/

2/

( ) ( ) ( )

=ΓΓ

TITITI

TITITI

TITITI

qkkQk

xxx

xxx

xxx

T

332

333

33

233

333

433

333

433

533

2/6/

2/3/8/

6/8/20/

Guideline for Choice of Process Noise Intensity The change in acceleration over a sampling period T are of the order of TqQ =33

For a nearly constant acceleration assumed by this model, the choice of q must be suchto give small changes in velocity compared to the actual acceleration . A

( ) ( ) ( )τδτ −= tIqwtwE xT

33

139



3. Piecewise (between samples) Constant White Noise Acceleration Model – 2nd Order

( ) ( ) ,0&0

00

0

33

33

3333

3333 =

+

=

twEtw

IV

RI

V

R

td

d

B

x

x

A

xx

xx

x

Discrete System

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) klTTT lqkllwkwEkkwkkxkkx δΓΓ=ΓΓΓ+Φ=+ 01

( ) [ ]

=+===Φ ∑∫

∞

= 3333

333366

00 0!

1exp:

xx

xxx

i

iiT

I

TIITAITA

idAT ττ

200

00

00

00

00

0

00

0

00

0

3333

3333

3333

3333

3333

3333

3333

33332

3333

3333 ≥∀

=→→

=

=→

= nA

IIA

IA

xx

xxn

xx

xx

xx

xx

xx

xx

xx

xx

( ) ( ) ( ) ( )( )

( ) ( ) ( )kwTI

TIkw

Id

I

TIIdkTwBTkwk

x

x

x

xT

xx

xxT

kw

=

−=+−Φ=Γ ∫∫

33

233

33

33

0 3333

3333

0

2/0

0: τ

ττττ

140



3. Piecewise (between samples) Constant White Noise Acceleration Model

( ) ( ) ( ) ( ) ( ) ( ) [ ] klxx

x

xkl

TTT TITITI

TIqlqkllwkwEk δδ 33

233

33

233

00 2/2/

=ΓΓ=ΓΓ

( ) ( ) ( ) ( ) lk

xx

xxTT

TITI

TITIqllwkwEk ,2

333

33

333

433

02/

2/2/δ

=ΓΓ


For this model q should be of the order of maximum acceleration magnitude aM.

A practical range is 0.5 aM ≤ q ≤ aM.

141



4. Piecewise (between samples) Constant Wiener Process Acceleration Model

( ) ( ) 0&0

0

000

00

00

33

33

33

333333

333333

333333

=

+

=

twEtw

IA

V

R

I

I

A

V

R

td

d

B

x

x

x

A

xxx

xxx

xxx

x

Discrete System( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) lk

TTT lqkllwkwEkkwkkxkkx ,01 δΓΓ=ΓΓΓ+Φ=+

( ) [ ]

=++===Φ ∑∫

∞

=333333

333333

2333333

2299

00 00

0

2/

2

1

!

1exp:

xxx

xxx

xxx

xi

iiT

I

TII

TITII

TATAITAi

dAT ττ

2

000

000

000

000

000

00

000

00

00

333333

333333

333333

333333

333333

333333

2

333333

333333

333333

≥∀

=→→

=→

= nA

I

AI

I

A

xxx

xxx

xxx

n

xxx

xxx

xxx

xxx

xxx

xxx

( ) ( ) ( ) ( )( )

( ) ( )( ) ( ) ( )kw

I

TI

TI

kwd

I

TII

TITII

dkTwBTkwk

x

x

xT

x

x

x

xxx

xxx

xxxT

kw

=

−−−

=+−Φ=Γ ∫∫33

33

233

033

33

33

333333

333333

2333333

0

2/

0

0

0

00

0

2/

: ττττ

τττ

142



4. Piecewise (between samples) Constant White Noise acceleration model

( ) ( ) ( ) ( ) ( ) ( ) [ ] lkxxx

x

x

x

lkTTT ITITI

I

TI

TI

qlqkllwkwEk ,33332

33

33

33

233

0,0 2/

2/

δδ

=ΓΓ=ΓΓ

( ) ( ) ( ) ( ) lk

xxx

xxx

xxx

TT

ITITI

TITITI

TITITI

qllwkwEk ,

33332

33

332

333

33

233

333

433

0

2/

2/

2/2/2/

δ

=ΓΓ


For this model q should be of the order of maximum acceleration increment over asampling period ΔaM.

A practical range is 0.5 ΔaM ≤ q ≤ ΔaM.

143

SOLO

Singer Target Model

R.A. Singer, “Estimating Optimal Tracking Filter Performance for Manned ManeuveringTarget”, IEEE Trans. Aerospace & Electronic Systems”, Vol. AES-6, July 1970, pp. 437-483

The target acceleration is modeled as a zero-mean random process with exponential autocorrelation ( ) ( ) ( ) TetataER mTT

ττσττ /2 −=+= where σm

2 is the variance of the target acceleration and τT is the time constant of itsautocorrelation (“decorrelation time”).

The target acceleration is assumed to:1. Equal to the maximum acceleration value amax

with probability pM and to – amax

with the same probability.2. Equal to zero with probability p0.3. Uniformly distributed between [-amax, amax]

with the remaining probability 1-2 pM – p0 > 0.

( ) ( ) ( )[ ] ( ) ( ) ( )[ ]max

0maxmax0maxmax 2

210

a

ppaauaauppaaaaap M

M

−−−−+++−++= δδδ

RADAR Range-Doppler


144

SOLO

Singer Target Model (continue 1)

( ) ( ) ( )[ ] ( ) ( ) ( )[ ]max

0maxmax0maxmax 2

210

a

ppaauaauppaaaaap M

M

−−−−+++−++= δδδ

( ) ( ) ( )[ ] ( )

( ) ( )[ ]

( ) ( )[ ] 022

210

2

21

0

max

max

max

max

max

max

max

max

2

max

00maxmax

max

0maxmax

0maxmax

=−−+⋅++−=

−−−−++

+−++==

+

−

−

−−

∫

∫∫

a

a

MM

a

a

M

a

a

M

a

a

a

a

ppppaa

daaa

ppaauaau

daappaaaadaapaaE δδδ

( ) ( ) ( )[ ] ( )

( ) ( )[ ]

( ) ( )[ ]

( )0

2max

3

max

02max

2max

2

max

0maxmax

20maxmax

22

413

32

21

2

21

0

max

max

max

max

max

max

max

max

ppa

a

a

pppaa

daaa

ppaauaau

daappaaaadaapaaE

M

a

a

MM

a

a

M

a

a

M

a

a

−+=

−−+−++=

−−−−++

+−++==

+

−

−

−−

∫

∫∫ δδδ

( )0

2max

0

222 413

ppa

aEaE Mm −+=−=

σ

Use

( ) ( ) ( )

max0max

00

max

max

aaa

afdaafaaa

a

+≤≤−

=−∫−

δ

RADAR Range-Doppler


145

SOLO

Target Acceleration Approximation by a Markov Process

w (t) x (t)

( )tF

( )tG ∫x (t)

( ) ( ) ( ) ( ) ( ) ( )twtGtxtFtxtxtd

d +== Given a Continuous Linear System:

Let start with the first order linear system describing Target Acceleration :

( ) ( ) ( )twtata TT

T +−=τ1

( ) ( ) T

T

tta ett τφ /

00, −−=

( ) ( ) [ ] ( ) ( ) [ ] ( )τδττ −=−− tqwEwtwEtwE( ) ( ) [ ] ( ) ( ) [ ] ( )ttRtaEtataEtaE

TT aaTTTT ,τττ +=−+−+

( ) ( ) [ ] ( ) ( ) [ ] ( )τττ +=+−+− ttRtaEtataEtaETT aaTTTT ,

( ) ( ) [ ] ( ) ( ) [ ] ( ) ( ) 2,TTTTT aaaaaTTTT ttRtVtaEtataEtaE σ===−−

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )tGtQtGtFtVtVtFtVtd

d TT

xxx ++= ( ) ( ) qtVtVtd

dTTTT aa

Taa +−=

τ2

( ) ( )00 ,1

, tttttd

dTT a

Ta φ

τφ −=

where


RADAR Range-Doppler

146

SOLO

( ) ( ) qtVtVtd

dTTTT aa

Taa +−=

τ2

( ) ( )

−+=

−−TT

TTTT

t

T

t

aaaa eq

eVtV ττ τ 22

12

0

( )( ) ( ) ( )

( ) ( ) ( )

<+=+Φ+

>=+Φ=+

−

−

0,

0,,

ττττ

τττ

ττ

ττ

tVetttV

tVetVttttR

TT

T

TTT

TT

T

TTT

TT

aaT

aaa

aaaaa

aa

( )( ) ( ) ( )

( ) ( ) ( )

<+=++Φ

>=+Φ=+

−

−

0,

0,,

ττττ

τττ

ττ

ττ

tVetVtt

tVetttVttR

TT

T

TTT

TT

T

TTT

TT

aaaaa

aaT

aaa

aa

For ( ) ( )2

5 Tstatesteadyaaaaaa

T

qVtVtV

TTTTTT

ττττ ==+≈⇒> −

( ) ( ) ( ) TT

TTTTTTTTe

qeVVttRttR

TT

statesteadyaaaaaaaaττ

ττ τττττ −−

− =≈≈+≈+⇒>2

,,5

Target Acceleration Approximation by a Markov Process (continue – 1)Target Acceleration Models (continue – 12)

RADAR Range-Doppler

147

SOLO

( ) 2

0 22 T

Taa qde

qdVArea T

TTτττττ τ

τ

=== ∫∫+∞ −+∞

∞−

τT is the correlation time of the noise w (t) and defines in Vaa (τ) the correlation time corresponding to σa

2 /e.One other way to find τT is by tacking the double sides Laplace Transform L 2 on τ of:

( ) ( ) ( ) qdetqtqs sww =−=−=Φ ∫

+∞

∞−

− ττδτδ ττ2L

( ) ( )

( ) ( ) ( )sHqsHs

q

deeq

Vs

T

T

sTssaaaa

T

TTTT

−=−

=

==Φ ∫+∞

∞−

−−−

2

2

/2

1

2

ττ

τττ ττττL

τT defines the ω1/2 of half of the power spectrum

q/2 and τT =1/ ω1/2.

( ) ( ) ( ) TT

TTTTTTTe

qeVttRttR

TT

aaaaaaaττ

ττ τσττττ −−

=≈≈+≈+⇒>2

,,5 2

T

aTqτσ 22

=

Target Acceleration Approximation by a Markov Process (continue – 2)

RADAR Range-Doppler


148

SOLO

Constant Speed Turning Model

RADAR Range-Doppler


Denote by and the constant velocity and turning rate vectors.Ptd

dVVV

== 1 ωωω 1=

VVVVtd

dVVV

td

dV

td

dA

×=×=+

== ωω 111:

0

( ) ( ) VVVVVtd

dV

td

dA

td

d

22

0:

0

ωωωωωωωω −=−⋅=××=×+×

=

=

Define( ) ( )

2

00:

V

AV

×=ω

Denote the position vector of the vehicle relative to an Inertial system..P

Therefore A

IA

V

P

I

I

A

V

P

td

d

+

−

=

Λ

0

0

00

00

00

2ω

We want to find ф (t) such that ( ) ( ) ( )TTT ΦΛ=Φ

Continuous TimeConstant Speed

Target Model

149

SOLO

Constant Speed Turning Model (continuous – 1)

RADAR Range-Doppler


AB

C

O

θ

φφ

n

v

1v

Let rotate the vector around by a large angle , to obtain the new vector

→= OAPT

n

Tωθ =→

=OBP

From the drawing we have:→→→→

++== CBACOAOBP

TPOA

=→

( ) ( )θcos1ˆˆ −××=→

TPnnAC Since direction of is: ( ) ( ) φsinˆˆ&ˆˆ TTT PPnnPnn

=××××

and it’s length is:

AC→

( )θφ cos1sin −TP

( ) θsinˆ TPnCB

×=→ Since has the direction and the

absolute valueCB

→

TPn

×ˆθφsinsinv

( ) ( ) ( ) θθ sinˆcos1ˆˆ TTT PnPnnPP

×+−××+=

( ) ( )[ ] ( ) ( )TPnTPnnPP TTT ωω sinˆcos1ˆˆ

×+−××+=

We will find ф (T) by direct computation of a rotation:

150

SOLO

Constant Speed Turning Model (continuous – 2)

RADAR Range-Doppler


( ) ( ) ( ) ( )TPnnTPnTd

PdV TT ωωωω

sinˆˆcosˆ ××+×==

( ) ( )TT PnTVV

×=== ˆ0 ω

( ) ( ) ( ) ( )TPnnTPnTd

VdA TT ωωωω cosˆˆsinˆ 22

××+×−==

( ) ( )TT PnnTAA

××=== ˆˆ0 2ω

( ) ( )[ ]( ) ( )

( ) ( )

+−=

+=

−++=−

−−

TT

TT

TTT

ATVTA

ATVTV

ATVTPP

ωωω

ωωω

ωωωω

cossin

sincos

cos1sin1

21

( ) ( ) ( ) ( )[ ]TPnnTPnPP TTT ωω cos1ˆˆsinˆ −××+×+=

151

SOLO

Constant Speed Tourning Model (continuous – 3)

RADAR Range-Doppler


( ) ( )[ ]( ) ( )

( ) ( )

+−=

+=

−++=−

−−

TT

TT

TTT

ATVTA

ATVTV

ATVTPP

ωωω

ωωω

ωωωω

cossin

sincos

cos1sin1

21

( ) ( )[ ]( ) ( )

( ) ( )( )

−

−=

Φ

−

−−

T

T

T

T

A

V

P

TT

TT

TTI

A

V

P

ωωωωωω

ωωωω

cossin0

sincos0

cos1sin1

21

Discrete TimeConstant Speed

Target Model

152

SOLO

Constant Speed Tourning Model (continuous – 4)

RADAR Range-Doppler


( )( ) ( )[ ]

( ) ( )( ) ( )

−

−=Φ −

−−

TT

TT

TTI

T

ωωωωωω

ωωωω

cossin0

sincos0

cos1sin1

21

( )( ) ( )[ ]

( ) ( )( ) ( )

−−−

=Φ −

−−

−

TT

TT

TTI

T

ωωωωωω

ωωωω

cossin0

sincos0

cos1sin1

21

1( )( ) ( )

( ) ( )( ) ( )

−−−=Φ

−

TT

TT

TT

T

ωωωωωωω

ωωω

sincos0

cossin0

sincos0

2

1

We want to find Λ (t) such that

( ) ( ) ( )TTT ΦΛ=Φ therefore ( ) ( ) ( )TTT 1−ΦΦ=Λ

( ) ( ) ( )( ) ( )

( ) ( )( ) ( )

( ) ( )[ ]( ) ( )( ) ( )

−−−

−−−=ΦΦ=Λ −

−−−

−

TT

TT

TTI

TT

TT

TT

TTT

ωωωωωω

ωωωω

ωωωωωωω

ωωω

cossin0

sincos0

cos1sin

sincos0

cossin0

sincos01

21

2

1

1

−=

00

100

010

2ωWe recovered the transfer matrix for the continuouscase.

153

SOLO

Force Equations

( )g1

mTFm

A A ++=

Lzgg 1= where

Fixed Wing Air Vehicle Acceleration Model

RADAR Range-Doppler


( ) ( ) WWA zLxDF 11 αα −−= -Drag and Lift Aerodynamic Forces as functions

of angle of attack αBxTT 1=

- Thrust Force

For small angle of attack α the wind (W) coordinates and body (B) coordinates coincide, therefore we will use only wind (W) and Local Level Local North (L) coordinates, related by:

WLC - Transformation Matrix from (L) to (W)

- earth gravitation

( )LWW zgz

m

Lx

m

DTA 111 +−−≈

Force Equations

By measuring Air Vehicle trajectory we can estimate its, position, velocity and accelerationvectors, , CL

W matrix and (T – D)/m and L / m. ( )AVP

,,

WxVV 1= - Air Vehicle Velocity Vector

154

SOLO

Fixed Wing Air Vehicle Acceleration Model (continue – 1)

RADAR Range-Doppler


( ) WWWWWWWWWWWWW

L

WW

L

zVqyVrxVxzryqxpVxVtd

xdVxV

td

Vd11111111

11 −+=×+++=+=

( )( ) ( )

( )( )

+−

+

+

=

+

−

−=

−=

=

=

gCl

gC

gCf

gC

L

DT

mVq

Vr

V

A

A

A

td

VdA

WL

WL

WL

WL

W

W

zW

yW

xWW

L

W

3,3

3,20

3,1

1

0

0

01

( )[ ]( ) VgCr

VgClqW

LW

WLW

/3,2

/3,3

=

−=

Therefore the Air vehicle Acceleration in it’s Wind (W) Coordinates is given by:

( ) ( )WWWWWWWWWWWWWW

I

xqyplyrzqfzlxfzlxfzlxftd

AdA 1111111111: +−−+−+−=−+−==

⋅⋅

( ) ( )( ) gCAmLl

gCAmDTfW

LzW

WLxW

3,3/:

3,1/:

+−==

−=−=

( )

−−

+−

=

W

WW

W

W

qfl

rfpl

qlf

A

155

SOLO


RADAR Range-Doppler


( )[ ]( ) VgCr

VgClqW

LW

WLW

/3,2

/3,3

=

−=We found:

( )mLl

mDTf

/:

/:

=−=

( )

−−

+−

=

W

WW

W

W

qfl

rfpl

qlf

A

, pW are pilot controlled and are modeled as zero mean random variables

lf ,

( ) ( )[ ]

( )[ ] ( )( )[ ] ( )[ ]

−−−

−

−−

=

−

−=

VgClgCV

VgCgCV

VgCll

qf

rf

ql

AEW

LW

L

WL

WL

WL

W

W

W

W

/3,31,3

/2,31,3

/3,3

( ) ( )( )[ ]

( )[ ] ( )( )[ ] ( )[ ]

−−−

−

−−

=

gClgCV

gCgCV

gCll

CV

AEW

LW

L

WL

WL

WL

TWL

L

3,31,3

2,31,3

3,31

( ) ( ) ( )

−

=−

l

pl

f

CAEA W

TWL

LL

( ) ( ) ( ) ( ) ( ) WL

l

p

f

TWL

TLLLL ClCAEAAEAE

W

=

−

−

2

22

2

00

00

00

σ

σ

σ

156

SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 22)

( )tA

IA

V

R

I

I

A

V

R

td

d

B

x

x

x

A

xxx

xxx

xxx

x

+

=

33

33

33

333333

333333

333333

0

0

000

00

00

Discrete System

( ) ( ) ( ) ( ) ( )kAkkxkkx Γ+Φ=+1

( ) [ ]

=++===Φ ∑∫

∞

=333333

333333

2333333

2299

00 00

0

2/

2

1

!

1exp:

xxx

xxx

xxx

xi

iiT

I

TII

TITII

TATAITAi

dAT ττ

2

000

000

000

000

000

00

000

00

00

333333

333333

333333

333333

333333

333333

2

333333

333333

333333

≥∀

=→→

=→

= nA

I

AI

I

A

xxx

xxx

xxx

n

xxx

xxx

xxx

xxx

xxx

xxx

( ) ( ) ( ) ( )( )

( ) ( )( ) ( ) ( )kA

TI

TI

TI

kAd

II

TII

TITII

dkTABTkAk

x

x

xT

x

x

x

xxx

xxx

xxxT

kA

=

−−−

=+−Φ=Γ ∫∫33

33

333

033

33

33

333333

333333

2333333

0

2/

6/

0

0

00

0

2/

: ττττ

τττ


157


( )

( ) ( )

( )

( )L

B

x

x

x

kx

L

A

xxx

xxx

xxx

L

kx

A

TI

TI

TI

A

V

R

I

TII

TITII

A

V

R

+

=

+

33

233

333

333333

333333

2333333

1

4/

6/

00

0

2/

Discrete System


( ) ( ) ( )

−

=−

l

pl

f

CAEA W

TWL

LL

( ) ( ) ( ) ( ) ( ) WL

l

p

f

TWL

TLLLL ClCAEAAEAE

W

=

−

−

2

22

2

00

00

00

σ

σ

σ

158


Fixed Wing Air Vehicle Acceleration Model (continue – 5)We need to defined the matrix CL

W. For this we see that is along and is alongWx1 Wz1V

L

( ) ( ) ( ) ( )( )( )( )

=

==

3,1

2,1

1,1

0

0

1

11W

L

WL

WL

TWL

W

W

TWL

L

W

C

C

C

CxCx( ) ( ) ( ) ( )

( )( )( )

=

==

3,3

2,3

1,3

1

0

0

11W

L

WL

WL

TWL

W

W

TWL

L

W

C

C

C

CzCz

Therefore ( ) ( ) ( ) ( ) ( ) ( )[ ]LLVLVLVVC LLLLTWL ///

×=

LWW zgzlxfA 111 +−=

Azgxfzl LWW

−+= 111

( ) ( ) ( ) ( )[ ] ( ) ( ) gCVgCACACACgCAf WL

WL

V

zW

LyW

LxW

LW

LxW 3,13,13,12,11,13,1 −=−++=−=

( ) ( ) ( ) ( )[ ] ( )( )( )( )

−

+

−++=

z

y

x

WL

WL

WL

WL

V

zW

LyW

LxW

L

L

W

A

A

A

g

C

C

C

gCACACACzl

1

0

0

3,1

2,1

1,1

3,13,12,11,11

( ) ( ) ( )[ ] [ ] 222/3,12,11,1 zyxzyxW

LW

LW

L VVVVVVCCC ++=

( ) ( ) ( )[ ] [ ] VAVAVAVACACACAV zzyyxxzW

LyW

LxW

LzW /3,12,11,1 ++=++==

159



CLW, f, l , qW, rW Computation from Vectors ( ) ( )LL AV

,

Compute:

( ) ( ) ( )[ ] [ ] 222/3,12,11,1 zyxzyxW

LW

LW

L VVVVVVCCC ++= 1

( ) ( ) ( )[ ] [ ] VAVAVAVACACACAV zzyyxxzW

LyW

LxW

LzW /3,12,11,1 ++=++==2

( ) ( )( )( )( )

( )( )( )

Abs

AgVVVgVV

AVVVgVV

AVVVgVV

C

C

C

zL

L

zzz

yyz

xxz

WL

WL

WL

L

W

L

/

//

//

//

3,3

2,3

1,3

1

−+−

−−

−−

=

==

3

( )[ ] ( )[ ] ( )[ ]222//////: zzzyyzxxz AgVVVgVVAVVVgVVAVVVgVVAbs −+−+−−+−−=

( ) ( ) ( ) ( ) ( ) ( )[ ]LLVLVLVVC LLLLTWL ///

×=4

( )

( ) ( ) ( )( )

×=

LL

VLVL

VV

CL

LL

L

WL

/

/

/

or

( )[ ]( ) VgCr

VgClqW

LW

WLW

/3,2

/3,3

=

−=( )( ) ( ) ( )[ ] ( ) gCACACACl

gCVfW

LzW

LyW

LxW

L

WL

3,33,32,31,3

3,1

+++−=

−=

5

160


Ballistic Missile Acceleration Model

( )g1

mTFm

A A ++=

( ) ( )( ) ( ) ( )[ ]WLWDref

WWA

zVCxVCSVZ

zVLxVDF

1,1,2

1,1,2

ααραα

−−=

−−= - Drag and Lift Aerodynamic Forces as

functions of angle of attack α and

air pressure ρ (Z)

BxTT 1=

- Thrust ForceFor small angle of attack α the wind (W) coordinates and body (B) coordinates coincide, therefore we will use only wind (W) and Local Level Local North (L) coordinates, related by:

WLC - Transformation Matrix from (L) to (W)

L

T

L zR

zgg 11 2

µ== where - earth gravitation

( )LWW zgz

m

Lx

m

DTA 111 +−−≈

Force Equations

WxVV 1= - Air Vehicle Velocity Vector

MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

161


Ballistic Missile Acceleration Model (continue – 1)

MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

( )( )

2

0

0

0

0

0

1

0

0

sin

cos1

0

0

0

T

WL

W

W

zW

yW

xWW

L

W

RC

L

L

DT

mVq

Vr

V

A

A

A

td

VdA

µ

ϕϕ

+

−−

−=

−=

=

=

Therefore the Air vehicle Acceleration in it’s Wind0 (W0 – for which φ =0 ) Coordinates is given by:

( ) WWWWWWWWWWWWW

L

WW

L

zVqyVrxVxzryqxpVxVtd

xdVxV

td

Vd11111111

11 −+=×+++=+=

Define:

m

Tt =: ( )

m

CSdd

VZ

m

D DrefCC == :&

2:

2ρ

( ) ( ) ( ) ( )tzztm

CSzz

VZt

m

LCC

LrefCC ωωωρω sin:&cos:&

2:cos

2

−===

We assume that the ballistic missile performs a barrel-roll motion with constant rotation rate ω. Therefore at each instant the aerodynamic lift force will be at an

angle φ = ω t.

Assuming constant CL/m: (barrel-roll model)02 =+ CC zz ωAssuming constant ω (barrel-roll model)0=ω

162



CLW0 Computation:

( ) 2/1222 ZYXV ++=( )

=Z

Y

X

V L

Define: ψ - trajectory azimuth angle ( )XY ,tan 1−=ψγ - trajectory pitch angle ( )221 ,tan YXZ += −γ

[ ] [ ]

−

−=

−

−==

γψγψγψψ

γψγψγ

ψψψψ

γγ

γγψγ

cossinsincossin

0cossin

sinsincoscoscos

100

0cossin

0sincos

cos0sin

010

sin0cos

320W

LC

163



( )( )

( ) ( ) 2

2

2

2

1

0

0

2

12

12

1

0

ZRz

V

zV

dVt

C

Z

Y

X

td

VdA

c

C

C

C

TWL

L

L

L

+

+

+

−

−

=

=

= µ

ωρ

ρ

ρ

where:

Assuming constant CL/m (barrel-roll model)02 =+ CC zz ω

0=Cd Assuming constant CD/m

( ) 2/1222 ZYXV ++=

Assuming constant ω (barrel-roll model)0=ω

164



MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

( )( )( ) ( )

++

+

−

−−

−−

−−

=

0

0

0

0

3,1

2,1

1,1

0

0

0

0000000000

0000000000

000000000

0000000000

02

13,1

2

13,3

2

13,2000000

02

12,1

2

12,3

2

12,2000000

02

11,1

2

11,3

2

11,2000000

0000100000

0000010000

0000001000

2

2

222

222

222

ZRtC

tC

tC

d

z

z

Z

Y

X

Z

Y

X

VCVCVC

VCVCVC

VCVCVC

d

z

z

Z

Y

X

Z

Y

X

td

d

C

WL

WL

WL

C

C

C

WL

WL

WL

WL

WL

WL

WL

WL

WL

C

C

C

µ

ωω

ρρω

ρ

ρρω

ρ

ρρω

ρ

ω

System Dynamics is given by:

165

SOLOTarget Acceleration Models (continue – 31)


MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

166



MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

167



MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

168



MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

169



MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

170



MV

Bx

By

BzWz

Wy

Wx

αβ

αβ

Bp

Wp

Bq

WqBrWr

Table of Content

171171

EstimatorsSOLOKalman Filter for Filtering Position and Velocity Measurements

Assume a Cartezian Model of a Non-maneuvering Target:

wx

x

x

x

td

d

wx

xx

BA

+

=

⇒

==

1

0

00

10

( ) [ ]

=+=+++++==Φ ∫ 10

1

!

1

2

1exp: 22

0

TTAITA

nTATAIdAT nn

T

ττ

200

00

00

00

00

10

00

10

00

10 2 ≥∀

=→→

=

=→

= nAAA n

+

=+=

2

1

v

v

x

xvxz

Measurements

( ) ( ) ( )

=

−−=

−=−Φ=Γ ∫∫ T

TTd

TdBTT

TTT 2/2/

1

0

10

1:

2

0

2

00 τττ

τττ

Discrete System

+=Γ+Φ=

++++

+

1111

1

kkkk

kkkkk

vxHz

wxx

==+

=

==

+

=

++++++

ΓΦ

+

+

kj

V

PTjkkkk

H

k

kjqTjkkkkk

vvERvxz

wwEQwT

Tx

Tx

k

kk

δσ

σ

δσ

2

2

111111

22

1

0

0&

10

01

&2/

10

1

1

172172

EstimatorsSOLOKalman Filter for Filtering Position and Velocity Measurements (continue – 1)

The Kalman Filter:

( )

−+=

Φ=

+++++++

+

kkkkkkkkk

kkkkk

xHzKxx

xx

|1111|11|1

||1

ˆˆˆ

ˆˆ

Tkkk

Tkkkkkk QPP ΓΓ+ΦΦ=+ ||1

[ ]TTT

T

Tpp

ppT

pp

ppP q

kkkk

kk 2/2/

1

01

10

1 222

|2212

1211

|12212

1211|1 σ

+

=

=

++

[ ]TTT

T

Tpp

TppTpp

pp

ppP q

kkkk

kk 2/2/

1

01 222

|2212

22121211

|12212

1211|1 σ

+

++=

=

++

( ) ( )( )

kkqq

qq

q

kkkk

kk

TpTTpp

TTppTTpTpp

TT

TT

pTpp

TppTpTpp

pp

ppP

|

2222

232212

232212

242221211

2

23

34

|222212

22122

221211

|12212

1211|1

2/

2/4/2

2/

2/4/2

+++

+++++=

+

+

+++=

=

++

σσ

σσ

σ

173173


The Kalman Filter:

( )

−+=

Φ=

+++++++

+

kkkkkkkkk

kkkkk

xHzKxx

xx

|1111|11|1

||1

ˆˆˆ

ˆˆ

[ ] 1

11|111|11

−

+++++++ += kT

kkkkT

kkkk RHPHHPK

( ) ( )kkP

V

VPkk

V

P

pp

pp

ppppp

pp

pp

pp

pp

pp

|1

21112

122

22

212

222

2112212

1211

|1

1

22212

122

11

2212

1211 1

++

−

+−

−+−++

=

+

+

=

σ

σσσσ

σ

( ) ( )( )

( )kkPV

PV

VP pppppppp

pppppppp

ppp/1

212

211222212

2122212

21212111211

212

22211

212

222

211

1

+

−+−+

++−−+−++

=σσ

σσσσ

( ) ( )( )

( )kkPV

PV

VP pppp

pppp

ppp|1

212

21122

212

212

212

22211

212

222

211

1

+

−+

−+−++

=σσ

σσσσ

174174


The Kalman Filter:

[ ] 1

11/111/11

−

+++++++ += kT

kkkkT

kkkk RHPHHPK

( ) ( )( )

( )kkPV

PV

VP pppp

pppp

ppp/1

212

21122

212

212

212

22211

212

222

211

1

+

−+

−+−++

=σσ

σσσσ

2

23

34

/222212

22122

221211

/12212

1211/1

2/

2/4/2q

kkkk

kkTT

TT

pTpp

TppTpTpp

pp

ppP σ

+

+

+++=

=

++

( ) ( )( ) ( ) ( )( ) ( ) ( ) ( ) 4///2//1

2////1

//1

42222121111

32221212

222222

TTkkpTkkpkkpkkp

TTkkpkkpkkp

Tkkpkkp

q

q

q

σ

σ

σ

+++=+

++=+

+=+

175175


The Kalman Filter:

[ ] 1

11/111/11

−

+++++++ += kT

kkkkT

kkkk RHPHHPK

( ) ( ) ( )( ) ( ) ( )

+−−−

−−=+

++++++++

++++ T

kkkT

kkkkk

kkk

kKRKHKIPHKI

PHKIP

11111111

111

1

( ) ( )( )

kkPV

PV

VPk

kppppp

pppp

pppKK

KKK

/1

2222211

212

212

212

212

22211

212

222

21112221

12111

1

+++

++−

−+−++

=

=

σσ

σσσσ

( ) ( )( )

( )kkVPV

PPV

VP

kkpp

pp

pppHKI

/1

2211

212

212

2222

212

222

211

11

1

+

++

+−

−+−++

=−σσσ

σσσσσ

( ) ( ) ( )( )

( )kkVPV

PPV

VP

kkkkkk pp

pp

pp

pp

pppPHKIP

/12212

1211

2211

212

212

2222

212

222

211

/1111/1

1

+

+++++

+−

−+−++

=−=σσσ

σσσσσ

( ) ( )( )[ ]

( )[ ]

=

=

−+

−+−++

=

+

++

++

2

2

12221

1211

1

222

221

212

211

/1

21222

211

22212

2212

21211

222

2

212

222

211

1/1

0

0

1

V

P

k

kVP

VP

kkPVVP

VPVP

VP

kk

KK

KK

KK

KK

pppp

pppp

pppP

σ

σ

σσ

σσ

σσσσ

σσσσσσ

176

Estimators

wx

x

x

x

td

d

BA

+

=

1

0

00

10

SOLO

We want to find the steady-state form of the filter for

Assume that only the position measurements are available

x

x

- position

- velocity

[ ] kjjkkk

k

kkkk RvvEvEvx

xvxHz δ==+

=+= ++++

+++++ 1111

1

1111 0&01

Discrete System

+=Γ+Φ=

++++

+

1111

1

kkkk

kkkkk

vxHz

wxx

[ ]

==+=

==

+

=

++++++

ΓΦ

+

+

kjPT

jkkkk

H

k

kjwTjkkkkk

vvERvxz

wwEQwT

Tx

Tx

k

kk

δσ

δσ

2111111

22

1

&01

&2/

10

1

1

α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model

177

EstimatorsSOLO

Discrete System

+=Γ+Φ=

++++

+

1111

1

kkkk

kkkkk

vxHz

wxx

[ ]

==+=

==

+

=

++++++

ΓΦ

+

+

kjPT

jkkkk

H

k

kjwTjkkkkk

vvERvxz

wwEQwT

Tx

Tx

k

kk

δσ

δσ

2111111

22

1

&01

&2/

10

1

1

( ) ( ) ( ) ( ) ( )11/111 +++++=+ kRkHkkPkHkS T

( ) ( ) ( ) ( ) 111/11 −+++=+ kSkHkkPkK T

When the Kalman Filter reaches the steady-state

( ) ( )

=++=

∞→∞→2212

12111/1lim/limpp

ppkkPkkP

kk( )

=+

∞→2212

1211/1limmm

mmkkP

k

[ ] 211

2

1212

1211

0

101 PP m

mm

mmS σσ +=+

=

( )( )

+

+=

+

=

=

21112

21111

2112212

1211

12

11

/

/1

0

1

P

P

P mm

mm

mmm

mm

k

kK

σ

σσ

( ) ( ) ( )[ ] ( )kkPkHkKIkkP /1111/1 +++−=++[ ]

−

=

2212

1211

12

11

2212

1211 0110

01

mm

mm

k

k

pp

pp

( ) ( )( )

( ) ( )( ) ( )

+−+

++=

−−

−−=

211

21222

21112

2

21112

221111

2

1212221211

12111111

//

//

1

11

PPP

PPPP

mmmmm

mmmm

mkmmk

mkmk

σσσ

σσσσ

α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 1)

178

EstimatorsSOLO

From ( ) ( ) ( ) ( ) ( )kQkkkPkkkP T +ΦΦ=+ //1

we obtain ( ) ( ) ( ) ( )[ ] ( )kkQkkPkkkP T−− Φ−+Φ= /1/ 1

( ) ( )

=++=

∞→∞→2212

12111/1lim/limpp

ppkkPkkP

kk( )

=+

∞→2212

1211/1limmm

mmkkP

k

T

TTT

TT

mm

mmT

pp

pp

Q

w

−− ΦΦ

−

−

−=

1

01

2/

2/4/

10

1 2

23

34

2212

1211

2212

1211

1

σ

For Piecewise (between samples) Constant White Noise acceleration model

( ) ( )( )

−+−

+−−+−=

−−

−−22

2223

2212

232212

2422

21211

1212221211

12111111

2/

2/4/2

1

11

ww

ww

TmTmTm

TmTmTmTmTm

mkmmk

mkmk

σσ

σσ

221212

23221211

2422

2121111

2/

4/2

w

w

w

Tmk

TmTmk

TmTmTmk

σ

σ

σ

=

−=

+−=


179

EstimatorsSOLO

( )112

1111 1/ kkm P −= σ

1222

12 / kTm wσ=

( ) 12121122

121122 2//2// mkTkTTmkm w +=+= σ

We obtained the following 5 equations with 5 unknowns: k11, k12, m11, m12, m22

( )112

1212 1/ kkm P −= σ( )2

111111 / Pmmk σ+=1

( )2111212 / Pmmk σ+=2

4/2 2422

2121111 wTmTmTmk σ+−=3

2/23221211 wTmTmk σ−=4

221212 wTmk σ=5

Substitute the results obtained from and in1 2 34 5

( ) ( ) ( ) ( )

4/

11

22

12

2

11

2

1212112

11

2

1211

22

11

24

121222

22

12121111

141212

1

w

w

T

mkT

P

m

m

P

m

P

mk

P

kk

T

kk

k

T

kT

kkT

kk

σ

σ

σσσσ

=

−+

−

+−

−=

−3

04

12 2

122

1211122

11 =++− kTkkTkTk


180

EstimatorsSOLO

We obtained: 04

12 2

122

1211122

11 =++− kTkkTkTk

Kalata introduced the α, β parameters defined as: Tkk 1211 :: == βα

and the previous equation is written as function of α, β as:

04

12 22 =++− ββαβα

which can be used to write α as a function of β: 22

ββα −=

( ) 12

22

11

212

12 1 k

T

k

km wP σσ =

−=

We obtained:

( )T

TTm wP

βσ

α

σβ22

2

12 1=

−= ( )

22

242

:1

λσσ

αβ ==− P

wT

P

wT

σσλ

2

:= Target Maneuvering Index proportional to the ratio of:

Motion Uncertainty:2

22Twσ

Observation Uncertainty: 2Pσ


181

EstimatorsSOLO

02

=−+ λβλβ

The positive solution for from the above equation is:β ( )λλλβ 822

1 2 ++−=

Therefore: ( ) ( )λλλλλλλλλβ 844

844

1 222 +−+=+−+=

and:

( )( )λλλλλλλλβα 8428168

16

111 222

2

2

++−++++−=−=

( )( )λλλλλα 8488

1 22 ++−+−=

22

ββα −=We obtained: ( )2

2

242

:1

λσ

σα

β ==− P

wTand:

( ) ( )2

222

2/12/21 ββ

βββλ

−=

+−=


182

EstimatorsSOLO

We found

( ) ( )( )

−−

−−=

1212221211

12111111

2212

1211

1

11

mkmmk

mkmk

pp

pp( )11

21111 1/ kkm P −= σ

( )112

1212 1/ kkm P −= σ

( ) 121211

22121122

2//

2//

mkTk

TTmkm w

+=+= σ

( ) 211111111 1 Pkmkp σ=−=

( ) 212121112 1 Pkmkp σ=−=

( )( )

α

σββα

−

−=

−=−+=

12

2//

2//

2

121211

121212121122

PTTT

mkTk

mkmkTkp

211 Pp σα=

212 PT

p σβ=

( )( )

2

222 1

2/PT

p σα

βαβ−

−=

&


183

Estimators

( )( )λλλλλα 8488

1 22 ++−+−=

SOLO

We found

( ) ( )λλλλλλλλλβ 844

844

1 222 +−+=+−+=

α, β gains, as function of λ in semi-log and log-log scales


184

EstimatorsSOLO

T

Tq

TT

TT

mm

mmT

pp

pp

Q −− ΦΦ

−

−

−=

1

01

2/

2/3/

10

12

23

2212

1211

2212

1211

1

For White Noise acceleration model

( ) ( )( )

−+−

+−−+−=

−−

−−

qTmqTmTm

qTmTmqTmTmTm

mkmmk

mkmk

222

2212

22212

322

21211

1212221211

12111111

2/

2/3/2

1

11

qTmk

qTmTmk

qTmTmTmk

=−=

+−=

1212

2221211

322

2121111

2/

3/2

α - β (2-D) Filter with White Noise Acceleration Model

( )

=

TT

TTqkQ

2/

2/3/2

23

185

EstimatorsSOLO

( )112

1111 1/ kkm P −= σ

1212 / kqTm =

( ) 121211121122 2//2// mkTkqTTmkm +=+=

We obtained the following 5 equations with 5 unknowns: k11, k12, m11, m12, m22

( )112

1212 1/ kkm P −= σ( )2

111111 / Pmmk σ+=1

( )2111212 / Pmmk σ+=2

3/2 322

2121111 qTmTmTmk +−=3

2/2221211 qTmTmk −=4

qTmk =12125

Substitute the results obtained from and in1 2 34 5

( ) ( ) ( ) ( )

3/

11

22

12

2

11

2

1212112

11

2

1211

22

11

3

1212

22

12121111

131212

1

qT

mkqT

P

m

m

P

m

P

mk

P

kk

T

kk

k

T

kT

kkT

kk

=

−+

−

+−

−=

−σσσσ3

06

12 2

122

1211122

11 =++− kTkkTkTk

α - β (2-D) Filter with White Noise Acceleration Model (continue – 1)

186

EstimatorsSOLO

We obtained: 06

12 2

122

1211122

11 =++− kTkkTkTk

The α, β parameters defined as: Tkk 1211 :: == βα

and the previous equation is written as function of α, β as:

06

12 22 =++− ββαβα

which can be used to write α as a function of β:212

22 βββα −+=

αβσ

β −=

−===

1

/

1/ 11

212

1212

T

k

k

T

qT

k

qTm P

We obtained:

2

2

32

:1 c

P

qT λσα

β ==−

α - β (2-D) Filter with White Noise Acceleration Model (continue – 2)

2

2

22

:

122

21

1 cλβββ

βα

β =+−+

=−The equation for solving β is:

which can be solved numerically.

187

EstimatorsSOLO

We found

( ) ( )( )

−−

−−=

1212221211

12111111

2212

1211

1

11

mkmmk

mkmk

pp

pp( )11

21111 1/ kkm P −= σ

( )112

1212 1/ kkm P −= σ

( ) 12121122 2// mkTkm +=

( ) 211111111 1 Pkmkp σ=−=

( ) 212121112 1 Pkmkp σ=−=

( )( )

α

σββα

−

−=

−=−+=

12

2//

2//

2

121211

121212121122

PTTT

mkTk

mkmkTkp

211 Pp σα=

212 PT

p σβ=

( )( )

2

222 1

2/PT

p σα

βαβ−

−=

&

α - β Filter with White Noise Acceleration Model (continue – 3)

188

Estimators

w

x

x

x

x

x

x

td

d

BA

+

=

1

0

0

000

100

010

SOLO

We want to find the steady-state form of the filter for

Assume that only the position measurements are available

[ ] kjjkkk

k

kkkk RvvEvEv

x

x

x

vxHz δ==+

=+= ++++

+

++++ 1111

1

1111 0&001

Discrete System

+=Γ+Φ=

++++

+

1111

1

kkkk

kkkkk

vxHz

wxx

[ ]

==+=

==

+

=

++++++

ΓΦ

+

+

kjPT

jkkkk

H

k

kjwTjkkkkk

vvERvxz

wwEQwT

T

xT

TT

x

k

kk

δσ

δσ

2111111

2

22

1

&001

&

1

2/

100

10

2/1

1

α – β - γ (3-D) Filter with Piecewise Constant Wiener Process Acceleration Model

x

x

x

- position- velocity

- acceleration

189

SOLO Estimators

Piecewise (between samples) Constant White Noise acceleration model

( ) ( ) ( ) ( ) ( ) ( ) [ ]12/

1

2/2

2

00 TTT

T

qlqkllwkwEk klTTT

=ΓΓ=ΓΓ δ

( ) ( ) ( ) ( )

=ΓΓ12/

2/

2/2/2/

2

23

234

0

TT

TTT

TTT

qllwkwEk TT


For this model q should be of the order of maximum acceleration increment over asampling period ΔaM.

A practical range is 0.5 ΔaM ≤ q ≤ ΔaM.

α – β - γ (3-D) Filter with Piecewise Constant Wiener Process Acceleration Model (continue – 1)

190

SOLO Estimators

The Target Maneuvering Index is defined as for α – β Filter as:P

wT

σσλ

2

:=


The three equations that yield the optimal steady-state gains are:

( )2

2

14λ

αγ =−

( ) ααβ −−−= 1422 or: 2/2 ββα −=

αβγ

2

=

This system of three nonlinear equations can be solved numerically.

The corresponding update state covariance expressions are:

( )( )

( )( )

( )( )

2

4332

213

2

3232

12

2

2222

11

14

2

14

2

18

428

PP

PP

PP

Tp

Tp

Tp

Tp

Tpp

σαγβγσγ

σαγββσβ

σα

αβγβασα

−−==

−−==

−−−+==

191

SOLO Estimators

α – β - γ Filter gains as functions of λ in semi-log and log-log scales:


Table of Content

192

SOLO Estimators

Optimal Filtering

An “Optimal Filter” is said to be optimal in some specific sense.

1. Minimum Mean-Square Error (MMSE)

( )∫ −=− nnnnnx

nnnx

xdZxpxxZxxEnn

:0

2

:0

2|ˆmin|ˆmin

Solution: ( )∫== nnnnnnn xdZxpxZxEx :0:0 ||ˆ

2. Maximum a Posteriori (MAP)

( ) ( ) nxxxxnn

xxIEZxp

nnnnn

ς≤−−⇔ ˆ::0 1min|modemin

Where is an indicator function and ζ is a small scalar. ( )nxI

3. Maximum Likelihood (ML) ( )nny

xypn

|max

4. Minimax: Median of Posterior ( )nn Zxp :0|

5. Minimum Conditional Inaccuracy

( ) ( ) ( ) ( )∫=− ydxdyxp

yxpyxpEx

yxpx |ˆ

1log|ˆmin|ˆlogmin ,

193

SOLO Estimators

Optimal Filtering

An “Optimal Filter” is said to be optimal in some specific sense.

6. Minimum Conditional KL Divergence

( ) ( )( ) ( )∫= ydxd

xpyxp

yxpyxpKL

|ˆ,

log,

7. Minimum Free Energy: It is a lower bound of maximum log-likelihood, which is aimed to minimize

( ) ( ) ( ) ( )( )

( ) ( ) ( ) xQEyxP

xQEyxPEPQ xQxQxQ log

|log|log, −

=−=F

where Q (x) is an arbitrary distribution of x.

The first term is called Kulleback – Leibler (KL) divergence between distribution Q (x)and P (x|y), the second term is entropy w.r.t. Q (x).

Table of Content

194

SOLO EstimatorsContinuous Filter-Smoother Algorithms

Problem - Choose w(t) and x(t0) to minimize:

( ) ( ) ∫ −− −+−+−+−=f

f

t

tQRSffS

dtwwxHzxtxxtxJ0

110

2222

00 2

1

2

1

2

1

subject to: ( ) ( ) ( ) ( ) ( ) ( )twtGtxtFtxtxtd

d +==

( ) ( ) ( ) ( )tvtxtHtz +=

and given: ( ) ( ) ( ) ( ) ( ) ( ) ( )tGtFtHtQtRSSxxtwtz ff ,,,,,,,,,, 00

Smoothing Interpretation

are noisy observations of Hx, i.e.:

v(t) is zero-mean white noise vector with density matrix R(t).

w(t) are random forcing functions, i.e., white noise vector with prior mean w(t) and density matrix Q(t).

(x0, P0) are mean and covariance of initial state vector from independent observations before test

(xf, Pf) are mean and covariance of final state vector from independent observations after test

( ) ( )[ ] ( )[ ]TS

xtxSxtxxtx 00000

2

00 2

1:

2

10

−−=−where

195


Solution to the Problem :

( ) nHamiltonia:H =++−+−= −− wGxFwwxHz T

QRλ22

11

2

1

2

1

Euler-Lagrange equations:

( )

( )

+−=∂∂=

−−=∂∂−=

−

−

GQwww

H

FHRxHzx

H

TT

TTT

λ

λλ

1

1

0

Two-Point Boundary Value Problem

Define:

( ) ( )[ ]

( ) ( )[ ]

−=∂∂=

−−=∂∂−=

fT

ff

t

fT

T

t

T

Sxtxx

Jt

Sxtxx

Jt

f

λ

λ 0000

0

Boundary equations:

λTGQww −=

( ) ( )( ) ( )

( ) ( ) ( ) ( )ttPtxtxtSxtx

tSxtxFF

tt

SP

ffff λλ

λ−=⇒

−=

+= →

=−

−

−

0

100:

01

000

1

zRHFxHRH TTT 11 −− +−−= λλ

( ) ( )

w

TGQwGxFtx λ−+=

td

d

( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

Assumed solution

Forward

196


Solution to the Problem (continue – 1) :

Differentiate and use previous equations

( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )[ ]( )

( ) ( )

( ) ( ) ( )[ ]( )

( ) ( )twGtGQGttPtxF

tzRHtFttPtxHRHtPttPtx

ttPttPtxtx

T

tx

FF

TT

tx

FFT

FFF

FFF

+−−=

+−−−⋅−−=

−−=

−−

λλ

λλλ

λλ

11

( ) ( ) ( ) ( ) ( )[ ] ( )( ) ( ) ( ) ( ) ( )[ ] ( )ttPHRHtPtPFFtPtP

twGtxHtzRHtPtxFtx

FT

FFT

FF

FT

FFF

λ1

1

−

−

+−−=

−−−−

( ) ( ) ( ) ( )ttPtxtx FF λ−=First Way, Assumption 1 .

( ) ( )( ) ( )

+=

−=−

−

ffff tSxtx

tSxtx

λλ1

01

000

or

197



( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

( ) ( ) ( ) ( ) ( )[ ] ( )( ) ( ) ( ) ( ) ( )[ ] ( )ttPHRHtPtPFFtPtP

twGtxHtzRHtPtxFtx

FT

FFT

FF

FT

FFF

λ1

1

−

−

+−−=

−−−−

( ) ( )( ) ( )

+=

−=−

−

ffff tSxtx

tSxtx

λλ1

01

000

We want to have xF(t) independent on λ(t). This is obtain by choosing

( ) ( ) ( ) ( ) ( ) ( ) 1000

1 −− ==−+= SPtPtPHRHtPtPFFtPtP FFT

FFT

FF

( ) ( ) ( ) ( ) ( )[ ] ( ) ( )( ) ( ) 1

00

: −=

=+−+=

RHtPtK

xtxtwGtxHtztKtxFtxT

FF

FFFFFTherefore

Let substitute the results in the equation( )tλ( ) ( ) ( ) ( )[ ] ( ) ( )

( ) ( ) ( ) ( )[ ]

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( )[ ]ffFffFfffffFffFffF

FT

T

RHtP

F

TTFF

T

xtxPtPttPxtxtxtxttP

txHtzRHtHKF

tzRHtFttPtxHRHt

TF

−+=⇒−−=−=

−+

−−=

+−−−=

−

−

−−

−

1

1

11

1

λλλ

λ

λλλ

( ) ( ) ( ) ( )ttPtxtx FF λ−=First Way, Assumption 1 (continue – 1) .

198



( ) ( ) ∫ −− −+−+−+−=f

f

t

tQRSffS

dtwwxHzxtxxtxJ0

110

2222

00 2

1

2

1

2

1


d +==

( ) ( ) ( ) ( )tvtxtHtz +=

Forward Covariance Filter

( ) ( ) ( ) ( ) ( )[ ] ( ) ( )( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) 1

001

00

: −

−

=

=−+=

=+−+=

RHtPtKwhere

PtPtPHRHtPtPFFtPtP

xtxtwGtxHtztKtxFtx

TFF

FFT

FFT

FF

FFFFF

Store xF(t) and PF(t)

Backward Information Filter (τ = tf – t)

( )[ ] ( ) ( ) ( )[ ] ( ) ( )[ ] ( )[ ]ffFffFfFTT

F xtxPtPttxHtzRHtHtKFtd

d

d

d −+=−−−−=−= −− 11 λλλτλ

Summary of First Assumption – Forward then Backward Algorithms

where = Estimate of w(t)( ) ( ) ( )tGQtwtw Tλ−=

= Smoothed Estimate of x(t)( ) ( ) ( )tPtxtx FF λ−=

199




QRλ22

11

2

1

2

1


( )

( )

+−=∂∂=

−−=∂∂−=

−

−

GQwww

H

FHRxHzx

H

TT

TTT

λ

λλ

1

1

0


Define:

( ) ( )[ ]

( ) ( )[ ]

−=∂∂=

−−=∂∂−=

fT

ff

t

fT

T

t

T

Sxtxx

Jt

Sxtxx

Jt

f

λ

λ 0000

0

Boundary equations:

λTGQww −=


( ) ( )[ ]

( ) ( )[ ]( ) ( ) ( ) ( )txtStt

Sxtxx

Jt

Sxtxx

Jt

FF

fT

ff

t

fT

T

t

T

f

−=⇒

−=∂∂=

−−=∂∂−=

λλλ

λ 0000

0

Second Way, Assumption 2:

Forward

200




( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( )

( ) ( ) ( ) ( )[ ] ( )tzRHtxtStFtxHRH

twGtxtStGQGtxFtStxtSt

txtStxtStt

TFF

TT

FFT

FFF

FFF

11 −− +−−−=

+−−⋅−−=

−−=

λλλ

λλ

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )[ ] ( )txHRHtSGQGtStSFFtStS

twGtStzRHtGQGtStFtT

FT

FFT

FF

FT

FT

FFT

F

1

1

−

−

−+++=

−−++

λλλ

( ) ( ) ( ) ( )txtStt FF −=λλSecond Way, Assumption 2

( ) ( )[ ]( ) ( )[ ]

−=

−−=

fT

fffT

TT

Sxtxt

Sxtxt

λλ 0000

or

201



( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ


twGtStzRHtGQGtStFtT

FT

FFT

FF

FT

FT

FFT

F

1

1

−

−

−+++=

−−++

λλλ( ) ( ) ( ) ( )txtStt FF −=λλSecond Way, Assumption 2

( ) ( )[ ]( ) ( )[ ]

−=

−−=

fT

fffT

TT

Sxtxt

Sxtxt

λλ 0000

We want to have λF(t) independent on x(t). This is obtain by choosing( ) ( ) ( ) ( ) ( ) ( )( ) ( )tSQGtC

StSHRHtCQtCtSFFtStS

FT

F

FT

FFFT

FF

=

=+−−−= −−

:

0011

Therefore( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) 000

1 xSttwGtStzRHttCGFt FFT

FT

FF =+++−= − λλλ

Let substitute the results in the equation( )tx

( ) ( ) ( ) ( ) ( )[ ] ( )( )[ ] ( ) ( ) ( )[ ]

( ) ( ) ( ) ( )[ ] ( ) ( )[ ] ( )[ ]fffFffFfffFfFffff

FT

F

FFT

xStStStxtxtStxStxS

tQGtwGtxtCGF

twGtxtStGQGtxFtx

++=⇒−+=

−++=

+−−=

− λλ

λλ

1

( ) ( )[ ] ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )tSQGtC

StSHRHtCQtCtSFFtStS

xSttwGtStzRHttCGFt

FT

F

FT

FFFT

FF

FFT

FT

FF

=

=+−−−=

=+++−=−−

−

:

0011

0001

λλλ

202



( ) ( ) ∫ −− −+−+−+−=f

f

t

tQRSffS

dtwwxHzxtxxtxJ0

110

2222

00 2

1

2

1

2

1


d +==

( ) ( ) ( ) ( )tvtxtHtz +=

Forward InformationFilter

Store λF(t) and SF(t)Backward Information Smoother (τ = tf – t)

Summary of Second Assumption – Forward then Backward Algorithms

( )[ ] ( ) ( ) ( )[ ] ( ) ( )[ ] ( )[ ]fffFffFfFT

F xStStStxtQGtwGtxtCGFtd

xd

d

xd ++=⇒−−+−=−= − λλτ

1



203




QRλ22

11

2

1

2

1


( )

( )

+−=∂∂=

−−=∂∂−=

−

−

GQwww

H

FHRxHzx

H

TT

TTT

λ

λλ

1

1

0


Define:

( ) ( )[ ]

( ) ( )[ ]

−=∂∂=

−−=∂∂−=

fT

ff

t

fT

T

t

T

Sxtxx

Jt

Sxtxx

Jt

f

λ

λ 0000

0

Boundary equations:

λTGQww −=


( )[ ] ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )ttPtxtx

tPtSxtx

tPtSxtxBB

ffffff

λλλ

λλ+=⇒

==−

==−−−

−

1

0001

000

Third Way, Assumption 3:

Backward

204




( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( ) ( )

( ) ( ) ( )[ ] ( ) ( )twGtGQGttPtxF

tzRHtFttPtxHRHtPttPtx

ttPttPtxtx

TBB

TTBB

TBBB

BBB

+−+=

+−+−⋅++=

++=−−

λλλλλ

λλ11

( ) ( ) ( ) ( ) ( )[ ] ( )( ) ( ) ( ) ( ) ( )[ ] ( )ttPHRHtPGQGFtPtPFtP

twGtxHtzRHtPtFxtx

BT

BTT

BBB

BT

BBB

λ1

1

−

−

+−++−=

−−+−

( ) ( ) ( ) ( )ttPtxtx BB λ+=Third Way, Assumption 3

( ) ( )[ ]( ) ( )[ ]

−=

−−=

fT

fffT

TT

Sxtxt

Sxtxt

λλ 0000

or

205



( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ ( ) ( )[ ]

( ) ( )[ ]

−=

−−=

fT

fffT

TT

Sxtxt

Sxtxt

λλ 0000

We want to have xB(t) independent on λ(t). This is obtain by choosing

Therefore

Let substitute the results in the equation( )tλ

( ) ( ) ( ) ( )ttPtxtx BB λ+=Third Way, Assumption 3

( ) ( ) ( ) ( ) ( )[ ] ( )( ) ( ) ( ) ( ) ( )[ ] ( )ttPHRHtPGQGFtPtPFtP

twGtxHtzRHtPtFxtx

BT

BTT

BBB

BT

BBB

λ1

1

−

−

+−++−=

−−+−

( ) ( ) ( ) ( ) ( ) ( )( ) 1: −=

=−+−−=−

RHtPK

PtPtKRtKGQGFtPtPFtPT

BB

ffBBBTT

BBB

( ) ( ) ( ) ( ) ( )[ ] ( ) ( ) ffBBBBB xtxtwGtxHtztKtFxtx =−−+−=−

( ) ( ) ( ) ( )[ ] ( ) ( )

( ) ( ) ( ) ( )[ ]

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( )[ ]001

00000000000

1

11

1

xtxPtPttPxtxtxtxttP

txHtzRHtHKF

tzRHtFttPtxHRHt

BBBBB

BT

T

RHtP

B

TTBB

T

TB

−+−=⇒−+−=+−=

−+

+−=

+−+−=

−

−

−−

−

λλλ

λ

λλλ

( ) ( ) ( ) ( ) ( )[ ] ( ) ( )( ) ( ) ( ) ( ) ( ) ( )

( ) 1: −=

=−+−−=−

=−−+−=−

RHtPK

PtPtKRtKGQGFtPtPFtP

xtxtwGtxHtztKtFxtx

TBB

ffBBBTT

BBB

ffBBBBB

206



( ) ( ) ∫ −− −+−+−+−=f

f

t

tQRSffS

dtwwxHzxtxxtxJ0

110

2222

00 2

1

2

1

2

1


d +==

( ) ( ) ( ) ( )tvtxtHtz +=

Backward Covariance Filter (τ = tf – t)

Store xB(t) and PB(t)

Forward Covariance Smoother

Summary of Third Assumption – Backward then Forward Algorithms

( ) ( ) ( ) ( ) ( )[ ] ( ) ( )[ ] ( )[ ]001

0001 xtxPtPttxHtzRHtHKFt BBB

TTB −+−=−++−= −− λλλ



207




QRλ22

11

2

1

2

1


( )

( )

+−=∂∂=

−−=∂∂−=

−

−

GQwww

H

FHRxHzx

H

TT

TTT

λ

λλ

1

1

0


Define:

( ) ( )[ ]

( ) ( )[ ]

−=∂∂=

−−=∂∂−=

fT

ff

t

fT

T

t

T

Sxtxx

Jt

Sxtxx

Jt

f

λ

λ 0000

0

Boundary equations:

λTGQww −=


( ) ( )[ ]

( ) ( )[ ]( ) ( ) ( ) ( )txtStt

Sxtxx

Jt

Sxtxx

Jt

BB

fT

ff

t

fT

T

t

T

f

+=⇒

−=∂∂=

−−=∂∂−=

λλλ

λ 0000

0

Fourth Way, Assumption 4:

Backward

208




( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( )

( ) ( ) ( ) ( )[ ] ( )tzRHtxtStFtxHRH

twGtxtStGQGtxFtStxtSt

txtStxtStt

TBB

TT

BBT

BBB

BBB

11 −− ++−−=

++−⋅++=

++=

λλλ

λλ


twGtStzRHtGQGtStFtT

BT

BBT

BB

BT

BT

BBT

B

1

1

−

−

−+−−−=

+−−+

λλλ

( ) ( ) ( ) ( )txtStt BB +=λλFourth Way, Assumption 4

( ) ( )[ ]( ) ( )[ ]

−=

−−=

fT

fffT

TT

Sxtxt

Sxtxt

λλ 0000

or

209



( )( )

( )( )

+

−−−

=

−− zRH

wG

t

tx

FHRH

GQGF

t

txTTT

T

11 λλ

( ) ( ) ( ) ( )txtStt BB +=λλFourth Way, Assumption 4

( ) ( )[ ]( ) ( )[ ]

−=

−−=

fT

fffT

TT

Sxtxt

Sxtxt

λλ 0000

We want to have λF(t) independent on x(t). This is obtain by choosing

Therefore( ) ( )[ ] ( ) ( ) ( ) ( ) ( ) fffBB

TF

TBB xSttwGtStzRHttCGFt −=+−−=− − λλλ 1

Let substitute the results in the equation( )tx

( ) ( ) ( ) ( ) ( )[ ] ( )( )[ ] ( ) ( ) ( )[ ]

( ) ( ) ( ) ( )[ ] ( ) ( )[ ] ( )[ ]0001

0000000000 xStStStxtxtStxStxS

tQGtwGtxtCGF

twGtxtStGQGtxFtx

BBBB

BT

B

BBT

+−+=⇒+−=

−+−=

++−=

− λλ

λλ


twGtStzRHtGQGtStFtT

BT

BBT

BB

BT

BT

BBT

B

1

1

−

−

−+−−−=

+−−+

λλλ

( ) ( ) ( ) ( ) ( ) ( )( )tSQGC

StSHRHtCQtCtSFFtStS

BT

B

ffBT

BT

BBT

BB

=

=+−=− −−

:

11

( ) ( )[ ] ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )

( )tSQGC

StSHRHtCQtCtSFFtStS

xSttwGtStzRHttCGFt

BT

B

ffBT

BT

BBT

BB

fffBBT

FT

BB

=

=+−=−

−=+−−=−−−

−

:

11

1

λλλ

210



( ) ( ) ∫ −− −+−+−+−=f

f

t

tQRSffS

dtwwxHzxtxxtxJ0

110

2222

00 2

1

2

1

2

1


d +==

( ) ( ) ( ) ( )tvtxtHtz +=

Backward InformationFilter (τ = tf – t)

Store λB(t) and SB(t)

Forward Information Smoother

Summary of Fourth Assumption – Backward then Forward Algorithms

( ) ( )[ ] ( ) ( ) ( )[ ] ( ) ( )[ ] ( )[ ]0001

000 xStStStxtQGtwGtxtCGFtx BBBT

B +−+=−+−= − λλ



Table of Content

211

EstimatorsSOLO

References

Minkoff, J., “Signals, Noise, and Active Sensors”, John Wiley & Sons, 1992

Sage, A. P., Melsa, J. L., “Estimation Theory with Applications to Communication and Control”, McGraw Hill, 1971

Gelb, A.,Ed., written by the Technical Staff, The Analytic Sciences Corporation, “Applied Optimal Estimation”, M.I.T. Press, 1974

Bryson, A.E. Jr., Ho, Y-C., “Applied Optimal Control”, Ginn & Company, 1969

Kailath, T., Sayed, A.H., Hassibi, B, “Linear Estimators”, Prentice Hall, 2000

Sage, A. P., “Optimal Systems Control”, Prentice-Hall, 1968, 1st Ed., Ch.8, Optimal State Estimation

Sage, A. P., White, C.C., III “Optimal Systems Control”, Prentice-Hall, 1977, 2nd Ed.,Ch.8, Optimal State Estimation

Y. Bar-Shalom, T.E. Fortmann, “Tracking and Data Association”, Academic Press, 1988

Y. Bar-Shalom, Xiao-Rong Li., “Multitarget-Multisensor Tracking: Principles and Techniques”, YBS Publishing, 1995

Haykin, S. “Adaptive Filter Theory”, Prentice Hall, 4th Ed., 2002

212

EstimatorsSOLO

References (continue – 1(

Minkler, G., Minkler, J., “Theory and Applications of Kalman Filters”, Magellan, 1993

Stengel, R. F., “Stochastic Optimal Control – Theory and Applications”, John Wiley & Sons, 1986

Kailath, T., “Lectures on Wiener and Kalman Filtering”, Springer-Verlag, 1981

Anderson, B. D. O., Moore, J. B., “Optimal Filtering”, Prentice-Hall, 1979

Deutch, R., “System Analysis Techniques”, Prentice Hall, 1969, ch. 6

Chui, C. K., Chen, G., “Kalman Filtering with Real Time Applications”, Springer-Verlag, 1987

Catlin, D. E., “Estimation, Control, and the Discrete Kalman Filter”, Springer-Verlag, 1989

Haykin, S., Ed., “Kalman Filtering and Neural Networks”, John Wiley & Sons, 2001

Zarchan, P., Musoff, H., “Fundamentals of Kalman Filtering – A Practical Approach”, AIAA, Progress in Astronautics & Aeronautics, vol. 190, 2000

Brookner, E., “Tracking and Kalman Filtering Made Easy”, John Wiley & Sons, 1998

213

EstimatorsSOLO

214

EstimatorsSOLO

References

Arthur E. Bryson Jr.Professor Emeritus

Aeronautics and AstronauticsPhone:650.857.1354

E-mail:[email protected]

Andrew P. Sage Thomas Kailath1935 -

From left-to-right: Sam Blackman, Oliver Drummond, Yaakoov Bar-Shalom and Rabinder Madan

Dr. Simon HaykinUniversity ProfessorDirector Adaptive Systems Laboratory

McMaster University, CRL-1051280 Main Street WestHamilton, ONCanada L8S 4L7Tel: (905) 525-9140 ext. 24809Fax: (905) 521-2922

Table of Content

January 10, 2015 215

SOLO

TechnionIsraeli Institute of Technology

1964 – 1968 BSc EE1968 – 1971 MSc EE

Israeli Air Force1970 – 1974

RAFAELIsraeli Armament Development Authority

1974 – 2013

Stanford University1983 – 1986 PhD AA

216


Normal (Gaussian) Distribution

Karl Friederich Gauss1777-1855

( )( )

σπ

σµ

σµ2

2exp

,;2

2

−−=

x

xp

( ) ( )∫∞−

−−=x

duu

xP2

2

2exp

2

1,;

σµ

σπσµ

( ) µ=xE

( ) σ=xVar

( ) ( )[ ]( ) ( )

−=

−−=

=Φ

∫∞+

∞−

2exp

exp2

exp2

1

exp

22

2

2

σωµω

ωσµ

σπ

ωω

j

duuju

xjE

Probability Density Functions

Cumulative Distribution Function

Mean Value

Variance

Moment Generating Function

217


Moments

Normal Distribution ( ) ( ) ( )[ ]σπ

σσ2

2/exp;

22xxpX

−=

[ ] ( ) −⋅

=oddnfor

evennfornxE

nn

0

131 σ

[ ]( )

+=

=−⋅=

+ 12!22

2131

12 knfork

knforn

xEkk

n

n

σπ

σ

Proof:

Start from: and differentiate k time with respect to a( ) 0exp 2 >=−∫∞

∞−

aa

dxxaπ

Substitute a = 1/(2σ2) to obtain E [xn]

( ) ( )0

2

1231exp

1222 >−⋅=− +

∞

∞−∫ a

a

kdxxax

kkk π

[ ] ( ) ( )[ ] ( ) ( )[ ]( ) ( ) 12

!

0

122/

0

222221212

!22

exp2

22

2/exp2

22/exp

2

1

2

+∞+=

∞∞

∞−

++

=−=

−=−=

∫

∫∫

kk

k

k

kxy

kkk

kdyyy

xdxxxdxxxxE

σπσ

σπ

σσπ

σσπ

σ

Now let compute:

[ ] [ ]( )2244 33 xExE == σ

Chi-square

218


Normal (Gaussian) Distribution (continue – 1)

Karl Friederich Gauss1777-1855

( ) ( ) ( )

−−−= −−

xxPxxPPxxpT 12/1

2

1exp2,; π

A Vector – Valued Gaussian Random Variable has theProbability Density Functions

where

xEx

= Mean Value

( ) ( ) TxxxxEP −−= Covariance Matrix

If P is diagonal P = diag [σ12σ2

2 … σk2] then the components of the random vector

are uncorrelated, andx

( )

( ) ( ) ( ) ( )

∏=

−

−

−−=

−−

−−

−−=

−

−−

−

−−

−=

k

i i

i

ii

k

k

kk

kkk

T

kk

xxxxxxxx

xx

xx

xx

xx

xx

xx

PPxxp

1

2

2

2

2

2

22

222

1

21

211

22

11

1

2

22

21

22

11

2/1

2

2exp

2

2exp

2

2exp

2

2exp

0

0

2

1exp2,;

σπσ

σπσ

σπσ

σπσ

σ

σ

σ

π

therefore the components of the random vector are also independent

219

SOLO Review of ProbabilityMonte Carlo Method

Monte Carlo methods are a class of computational algorithms that rely on repeated random sampling to compute their results. Monte Carlo methods are often used when simulating physical and mathematical systems. Because of their reliance on repeated computation and random or pseudo-random numbers, Monte Carlo methods are most suited to calculation by a computer. Monte Carlo methods tend to be used when it is infeasible or impossible to compute an exact result with a deterministic algorithm.

The term Monte Carlo method was coined in the 1940s by physicists Stanislaw Ulam, Enrico Fermi, John von Neumann, and Nicholas Metropolis, working on nuclear weapon projects in the Los Alamos National Laboratory

Stanislaw Ulam1909 - 1984

Enrico - Fermi1901 - 1954

John von Neumann1903 - 1957 Nicholas Constantine Metropolis

(1915 –1999)

220

SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (Unknown Statistics)

jimxExE ji ,∀==

DefineEstimation of thePopulation mean

∑=

=k

iik x

km

1

1:ˆ

A random variable, x, may take on any values in the range - ∞ to + ∞.Based on a sample of k values, xi, i = 1,2,…,k, we wish to compute the sample mean, ,and sample variance, , as estimates of the population mean, m, and variance, σ2.

2ˆkσkm

( )

( ) ( ) ( )[ ] ( ) ( )[ ]2

1

2

1

2222

22222

1 112

1

2

2

11

2

1

2

111

1

11

121

112

1

ˆˆ21

ˆ1

σσ

σσσ

k

k

kk

mkmkkk

mmkk

mk

xxk

Exk

xExEk

mxmxEk

mxk

E

k

i

k

i

k

i

k

ll

k

jj

k

jjii

k

k

iik

k

ii

k

iki

−=

−=

++−+++−−+=

+

−=

+−=

−

∑

∑

∑ ∑∑∑

∑∑∑

=

=

= ===

===

jimxExE ji ,2222 ∀+== σ

mxEk

mEk

iik == ∑

=1

1ˆ

jimxExExxE ji

tindependenxx

ji

ji

,2,

∀==

Compute

Biased

Unbiased

221

SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 1)

jimxExE ji ,∀==

DefineEstimation of thePopulation mean

∑=

=k

iik x

km

1

1:ˆ


2ˆkσkm

( ) 2

1

2 1ˆ

1 σk

kmx

kE

k

iki

−=

−∑

=

jimxExE ji ,2222 ∀+== σ

mxEk

mEk

iik == ∑

=1

1ˆ

jimxExExxE ji

tindependenxx

ji

ji

,2,

∀==

Biased

Unbiased

Therefore, the unbiased estimation of the sample variance of the population is defined as:

( )∑=

−−

=k

ikik mx

k 1

22 ˆ1

1:σ since ( ) 2

1

22 ˆ1

1:ˆ σσ =

−

−= ∑

=

k

ikik mx

kEE

Unbiased

222



2ˆkσkm

mxEk

mEk

iik == ∑

=1

1ˆ

( ) 2

1

22 ˆ1

1:ˆ σσ =

−

−= ∑

=

k

ikik mx

kEE

223


mxEk

mEk

iik == ∑

=1

1ˆ ( ) 2

1

22 ˆ1

1:ˆ σσ =

−

−= ∑

=

k

ikik mx

kEE

We found:

Let Compute:

( ) ( )

( ) ( ) ( )

( ) ( ) ( ) k

mxEmxEmxEk

mxmxEmxEk

mxk

Emxk

EmmE

k

i

k

ijj

ji

k

ii

k

i

k

ijj

ji

k

ii

k

ii

k

iikmk

2

1 100

1

2

2

1 11

2

2

2

1

2

1

22ˆ

2

1

1

11ˆ:

σ

σ

σ

=

−−+−=

−−+−=

−=

−=−=

∑ ∑∑

∑∑∑

∑∑

=≠==

=≠==

==

( ) k

mmE kmk

222

ˆ ˆ:σσ =−=

224


Let Compute:

( ) ( ) ( )

( ) ( ) ( ) ( )[ ]

( ) ( ) ( ) ( )

−−

−+−

−−+−

−=

−−+−−+−

−=

−−+−

−=

−−

−=−=

∑∑

∑

∑∑

==

=

==

2

22

11

2

2

2

1

22

2

2

1

22

2

1

22222

ˆ

ˆ11

ˆ2

1

1

ˆˆ21

1

ˆ1

1ˆ

1

1ˆ:2

σ

σ

σσσσσσ

k

k

ii

kk

ii

k

ikkii

k

iki

k

ikik

mmk

kmx

k

mmmx

kE

mmmmmxmxk

E

mmmxk

Emxk

EEk

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

k

k

k

ii

kk

ii

k

k

k

ii

k

k

ii

k

kk

ii

k

k

k

k

ii

k

kk

i

k

ijj

ji

k

k

ii

mmEk

kmxE

k

mmEmxE

k

mmEk

mxEk

mxEk

mmEkmxE

k

mmE

mmEk

kmxE

k

mmEmxEmxEmxE

kk

/

22

10

2

0

10

2

3

1

22

1

2

2

/

2

1

3

2

0

44

2

2

1

2

2

/

2

1 1

22

1

4

2

2

ˆ

2

222

22

22

4

2

ˆ1

2

1

ˆ4

1

ˆ4

1

2

1

ˆ2

1

ˆ4

ˆ11

ˆ4

1

1

σ

σσσ

σσ

σσµ

σ

σσ

σ

σσ

−−

−−−

−−−−

−+

−−

−−−

−+−−

−+

+−−

+−−−+

−−+−−

≈

∑∑

∑∑∑

∑∑ ∑∑

==

===

==≠==

Since (xi – m), (xj - m) and are all independent for i ≠ j:( )kmm ˆ−

225


Since (xi – m), (xj - m) and are all independent for i ≠ j:( )kmm ˆ−

( )( )( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) 4

2

24

224

44

2

4

44

2

2

2

4

2

4

242

ˆ

ˆ11

7

11

2

1

2

1

2

ˆ11

4

1

1

12

k

k

mmEk

k

k

k

k

k

kk

k

k

k

mmEk

k

kk

kk

k

kk

−−

+−+−+

−=

−−

−−

−+

+−−

+−

+−−+

−≈

σµσσσ

σσσµσσ

kk

442

ˆ 2

σµσσ

−≈ ( ) 44 : mxE i −=µ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

k

k

k

ii

kk

ii

k

k

k

ii

k

k

ii

k

kk

ii

k

k

k

k

ii

k

kk

i

k

ijj

ji

k

k

ii

mmEk

kmxE

k

mmEmxE

k

mmEk

mxEk

mxEk

mmEkmxE

k

mmE

mmEk

kmxE

k

mmEmxEmxEmxE

kk

/

22

10

2

0

10

2

3

1

22

1

2

2

/

2

1

3

2

0

44

2

2

1

2

2

/

2

1 1

22

1

4

2

2

ˆ

2

222

22

22

4

2

ˆ1

2

1

ˆ4

1

ˆ4

1

2

1

ˆ2

1

ˆ4

ˆ11

ˆ4

1

1

σ

σσσ

σσ

σσµ

σ

σσ

σ

σσ

−−

−−−

−−−−

−+

−−

−−−

−+−−

−+

+−−

+−−−+

−−+−−

≈

∑∑

∑∑∑

∑∑ ∑∑

==

===

==≠==

226


mxEk

mEk

iik == ∑

=1

1ˆ

( ) 2

1

22 ˆ1

1:ˆ σσ =

−

−= ∑

=

k

ikik mx

kEE

We found:

( ) k

mmE kmk

222

ˆ ˆ:σσ =−=

( ) ( )k

mxk

EEk

ikik

k

44

2

2

1

22222

ˆˆ

1

1ˆ:2

σµσσσσσ

−≈

−−

−=−= ∑

=

( ) 44 : mxE i −=µ

Kurtosis of random variable xiDefine

44:

σµλ =

( ) ( ) ( )k

mxk

EEk

ikik

k

42

2

1

22222

ˆ

1ˆ

1

1ˆ:2

σλσσσσσ

−≈

−−

−=−= ∑

=

227


[ ] ϕσσσ σσ =≤≤ 2ˆ

2k

2

kˆ-0Prob n

For high values of k, according to the Central Limit Theorem the estimations of mean and of variance are approximately gaussian random variables.

km2ˆkσ

We want to find a region around that will contain σ2 with a predefined probabilityφ as function of the number of iterations k.

2ˆkσ

Since are approximately gaussian random variables nσ is given by solving:

2ˆkσ

ϕζζπ

σ

σ

=

−∫

+

−

n

n

d2

2

1exp

2

1 nσ φ

1.000 0.6827

1.645 0.9000

1.960 0.9500

2.576 0.9900

Cumulative Probability within nσStandard Deviation of the Mean for a

Gaussian Random Variable

22k

22 1ˆ-

1 σλσσσλσσ k

nk

n−≤≤−−

22k

2 11

ˆ-11 σλσσλ

σσ

−−≤≤

+−−

kn

kn

228


[ ] ϕσσσ σσ =≤≤ 2ˆ

2k

2

kˆ-0Prob n

22k

22 1ˆ-

1 σλσσσλσσ k

nk

n−≤≤−−

22k

2 11

ˆ-11 σλσσλ

σσ

−−≤≤

+−−

kn

kn

22

ˆ

12

kσλσ

σ k

−=

22k

2 11ˆ

11 σλσσλ

σσ

−−≥≥

−+k

nk

n

−−≥≥

−+k

nk

n1

1

ˆ1

1

22

k

2

λσσ

λσ

σσ

kn

kn

11

:ˆ:1

1

k

−−

=≥≥=−+ λ

σσσσλ

σ

σσ

229


230


231


kn

kn kk 1ˆ

1

:&1ˆ

1

:

00−

−

=−

+

=λ

σσλ

σσ

σσ

Monte-Carlo Procedure

Choose the Confidence Level φ and find the corresponding nσ

using the normal (gaussian) distribution.

nσ φ

1.000 0.6827

1.645 0.9000

1.960 0.9500

2.576 0.9900

1

Run a few sample k0 > 20 and estimate λ according to2

( )

( )2

1

2

0

1

4

0

0

0

0

0

0

ˆ1

ˆ1

:ˆ

−

−=

∑

∑

=

=

k

iki

k

iki

k

mxk

mxkλ∑

==

0

010

1:ˆ

k

iik x

km

3 Compute and as function of kσ σ

4 Find k for which

[ ] ϕσσσ σσ =≤≤ 2ˆ

2k

2

kˆ-0Prob n

5 Run k-k0 simulations

232

SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue – 11)

Monte-Carlo Procedure

Choose the Confidence Level φ = 95% that gives the corresponding nσ=1.96.

nσ φ

1.000 0.6827

1.645 0.9000

1.960 0.9500

2.576 0.9900

1

The kurtosis λ = 32

3 Find k for which ϕσλσσ

σ

σ =

−≤≤

2kˆ

22k

2 1ˆ-0Prob

kn

4 Run k>800 simulations

Example:Assume a gaussian distribution λ = 3

95.02

96.1ˆ-0Prob

2kˆ

22k

2 =

≤≤

σ

σσσk

Assume also that we require also that with probability φ = 95 % 22k

2 1.0ˆ- σσσ ≤

1.02

96.1 =k

800≈k

233


Kurtosis of random variable xi

Kurtosis

Kurtosis (from the Greek word κυρτός, kyrtos or kurtos, meaning bulging) is a measure of the "peakedness" of the probability distribution of a real-valued random variable. Higher kurtosis means more of the variance is due to infrequent extreme deviations, as opposed to frequent modestly-sized deviations.

1905 Pearson defines Kurtosis, as a measure of departure from normality in a paper published in Biometrika. λ=3 for the normal distribution and the terms ‘leptokurtic’ (λ>3), mesokurtic (λ=3), platikurtic (λ<3) are introduced.

( ) ( ) [ ]224 /: mxEmxE ii −−=λ

( ) ( ) [ ]22

4

:mxE

mxE

i

i

−

−=λ

Karl Pearson (1857 –1936)

A leptokurtic distribution has a more acute "peak" around the mean (that is, a higher probability than a normally distributed variable of values near the mean) and "fat tails" (that is, a higher probability than a normally distributed variable of extreme values). A platykurtic distribution has a smaller "peak" around the mean (that is, a lower probability than a normally distributed variable of values near the mean) and "thin tails" (that is, a lower probability than a normally distributed variable of extreme values).

234Hyperbolic-Secant

25

x2

sech2

1 π


Distribution GraphicalRepresentation

FunctionalRepresentation

Kurtosisλ

ExcessKurtosis

λ-3

Normal ( )

σπσµ

2

2exp 2

2

−− x3 0

Laplace

−−

b

x

b

µexp

2

16 3

Uniformbxorxa

bxaab

>>

≤≤−0

1

1.8 -1.2

WignerRx

RxxRR

>

≤−

0

2 222π -1.02

235


Skewness of random variable xi

Skewness

( ) ( ) [ ] 2/32

3

:mxE

mxE

i

i

−

−=γ Karl Pearson (1857 –1936)

Negative skew: The left tail is longer; the mass of the distribution is concentrated on the right of the figure. The distribution is said to be left-skewed.

1

Positive skew: The right tail is longer; the mass of the distribution is concentrated on the left of the figure. The distribution is said to be right-skewed.

2

More data in the left tail thanit would be expected in a normal distribution

More data in the righttail thanit would be expected in a normal distribution

Karl Pearson suggested two simpler calculations as a measure of skewness:• (mean - mode) / standard deviation • 3 (mean - median) / standard deviation

236

SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable using a Recursive Filter (Unknown Statistics)

We found that using k measurements the estimated mean and variance are given in batch form by:

∑=

=k

iik x

kx

1

1:ˆ

A random variable, x, may take on any values in the range - ∞ to + ∞.Based on a sample of k values, xi, i = 1,2,…,k, we wish to estimate the sample mean, ,and the variance pk, by a Recursive Filter

kx

The k+1 measurement will give:

( )1

1

11 ˆ

1

1

1

1ˆ +

+

=+ +

+=

+= ∑ kk

k

iik xxk

kx

kx

( )kkkk xxk

xx ˆ1

1ˆˆ 11 −

++= ++

Therefore the Recursive Filter form for the k+1 measurement will be:

( )∑=

−−

=k

ikik xx

kp

1

2ˆ1

1:

( )∑+

=++ −=

1

1

211 ˆ

1 k

ikik xx

kp

237

SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable using a Recursive Filter (Unknown Statistics) (continue – 1)

We found that using k+1 measurements the estimated variance is given in batch form by:


kx

( )

+−−

++= ++ kkkkk p

k

kxx

kpp

1ˆ

1

1 211

( )

( )( )

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) 21

212

1

0

11

21

1

1

2

1

1

2

11

1

211

ˆ1

111ˆ

1

1

ˆˆˆ1

2ˆˆ

1

1

ˆˆ

1ˆ

1

kkkkk

kk

k

ikikkkk

pk

k

iki

k

i

kkki

k

ikik

xxk

pk

xxkk

k

xxxxxxkk

xxxxk

k

xxxx

kxx

kp

k

−+

+

−=−

+++

−+−−+

−

−+−=

+−−−=−=

++

+=

++

−

=

+

=

++

=++

∑∑

∑∑

( )kkkk xxk

xx ˆ1

1ˆˆ 11 −

++= ++

238

SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable using a Recursive Filter (Unknown Statistics) (continue – 2)


kx

( )

+−−

++= ++ kkkkk p

k

kxx

kpp

1ˆ

1

1 211

( )kkkk xxk

xx ˆ1

1ˆˆ 11 −

++= ++ ( ) ( ) ( )kkkk xxkxx ˆˆ1ˆ 11 −+=− ++

( ) ( )

−−++= ++ kkkkk p

kxxkpp

1ˆˆ1 2

11

239


Estimate the value of a constant x, given discrete measurements of x corrupted by anuncorrelated gaussian noise sequence with zero mean and variance r0.The scalar equations describing this situation are:

kk xx =+1

kkk vxz +=

System

Measurement ( )0,0~ rNvk

The Discrete Kalman Filter is given by:

( ) ( )+=−+ kk xx ˆˆ 1

( ) ( ) ( ) ( )[ ] ( )[ ]−−+−−+−=+ ++−

++++

+

111

01111 ˆˆˆ

1

kk

K

kkkk xzrppxx

k

0

1 kkk

I

kk wxx Γ+Φ=+

kk

I

kk vxHz +=

( ) ( )[ ] ( )[ ]

( )

( )+=ΓΓ+Φ+Φ=−−−−=− +++++ kT

I

Tkk

I

kT

kkkkk pQpxxxxEp

0

11111 ˆˆ

( ) ( )[ ] ( )[ ] ( ) ( )

( )

( )

( ) ( ) ( )( ) 0

011

1

0111111

11111

1

1

ˆˆ

rp

prpHrHpHHpp

xxxxEp

k

kpp

k

I

k

K

T

I

kk

I

kT

I

kkk

Tkkkkk

kk

k

+++=−

+−−−−=

−+−+=+

+=−

++

−

++++++

+++++

+

+

General Form

with Known Statistics Moments Using a Discrete Recursive FilterEstimation of the Mean and Variance of a Random Variable

240


Estimate the value of a constant x, given discrete measurements of x corrupted by anuncorrelated gaussian noise sequence with zero mean and variance r0.

We found that the Discrete Kalman Filter is given by:

( ) ( ) ( )[ ]+−++=+ +++ kkkkk xzKxx ˆˆˆ 111

( ) ( )( )

( )( )0

0

01

1r

pp

rp

prp

k

k

k

kk ++

+=+++=++

( )

0

0

01

1r

pp

p+

=+ ( ) ( )( )0

1

12

1r

pp

p ++

+=+ ( )k

r

pp

pk

0

0

0

1+=+

( )( ) 0

1 rp

pK

k

kk ++

+=+

( )( ) 0

1 rp

pK

k

kk ++

+=+( ) ( )( )

( )[ ]+−++

++=+ ++ kkkk xzk

r

pr

p

xx ˆ11

ˆˆ 1

0

0

0

0

1

0=k1=k

0

0

0

21

r

pp

+=

( )111

1

0

0

0

0

0

0

0

0

0

0

0

++=

++

+=

krp

rp

rk

rpp

krpp

with Known Statistics Moments Using a Discrete Recursive Filter (continue – 1)Estimation of the Mean and Variance of a Random Variable

241


Estimate the value of a constant x, given continuous measurements of x corrupted by anuncorrelated gaussian noise sequence with zero mean and variance r0.The scalar equations describing this situation are:

0=x

vxz +=

System

Measurement ( )rNv ,0~

The Continuous Kalman Filter is given by:

( ) ( ) ( ) ( ) ( )[ ] ( ) 00ˆ&ˆˆˆ

1

1

0

=−

+=

+

− xtxtzrHtptxAtx

kK

I

00

wxAx Γ+=

vxHzI

+=

( ) ( ) ( )[ ] ( ) ( )[ ] TtxtxtxtxEtp −−= ˆˆ:

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 12

1

1

000

−− −=−++= rtptptHrtHtptGQtGtAtptptAtp TT

I

TT

General Form

with Known Statistics Moments Using a Continuous Recursive FilterEstimation of the Mean and Variance of a Random Variable

( ) ( ) ( ) 012 0& ptprtptp ==−= −or: ∫∫ −=

tp

p

dtrp

pd

02

0

1 ( )t

r

pp

tp0

0

1+=

( )t

r

pr

p

rtpK0

0

1

1+== − ( ) ( )[ ]txz

tr

prp

tx ˆ1

ˆ0

0

−+

=

242


Monte Carlo approximation

Monte Carlo runs , generate a set of samples that approximate the filtering distribution . So, with P samples, expectations with respect to the filtering distribution are approximated by

( )xp

( ) ( ) ( )( )∑∫=

≈P

L

LxfP

dxxpxf1

1

and , in the usual way for Monte Carlo, can give all the moments etc. of the distribution up to some degree of approximation.

( ) ( )∑∫=

≈==P

L

LxP

dxxpxxE1

1

1µ

( ) ( ) ( ) ( )( )∑∫=

−≈−=−=P

L

nLnnn x

PdxxpxxE

1111

1 µµµµ

243


Types of Estimation

t t+τ

t

available measurement data

t



Filtering

t+ττ > 0

τ > 0

Use all the measurement datato the present time t to estimate.

Smoothing

Use all the measurement datato a future time t+τ to estimateat present time t..

Prediction

Use all the measurement datato the present time t to predictthe outcome at a future time t + τ.

244


Conditional Expectations and Their Smoothing Property

The Conditional Expectation is defined as: ( )∫+∞

∞−

= dxyxpxyxE yx || |

Similarly, for a function of x and y, g (x,y), the Conditional Expectation is defined as:

( ) ( ) ( )∫+∞

∞−

= dxyxpyxgyyxgE yx |,|, |

Smoothing property of the Expectation states that the Expected value of the ConditionalExpectation is equal to the Unconditional Expected Value

( ) ( )

( ) ( )

( )

( ) xEdxxpx

dxdyyxpx

dxdyypyxpx

dyypdxyxpxyxEE

x

yx

yyx

yyx

==

=

=

=

∫

∫ ∫

∫ ∫

∫ ∫

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

+∞

∞−

+∞

∞−

,

|

||

,

|

|

xEyxEE =|

This relation is also called the Law of Iterated Expectation, summarized as:

245


Gaussian Mixture Equations

A mixture is a p.d.f. given by a weighted sum of p.d.f.s with the weighths summing upto unity:

( ) ( )∑=

=n

jjjj Pxxpxp

1

,;N

A Gaussian Mixture is a p.d.f. consisting of a weighted sum of Gaussian densities

where: 11

=∑=

n

jjp

( ) jjj PxxA ,~: N=Denote by Aj the event that x is Gaussian distributed with mean and covariance Pjjx

with Aj , j=1,…,n, mutually exclusive and exhaustive: and S

1A 2A nA

jj pAP =:jiOAAandSAAA jin ≠∀/=∩=∪∪∪ 21

( ) ( ) ( ) ( )∑∑==

==n

jjj

n

jjjj AxpAPPxxpxp

11

|,;NTherefore:

246


Gaussian Mixture Equations (continue – 1)

A Gaussian Mixture is a p.d.f. consisting of a weighted sum of Gaussian densities

( ) ( ) ( ) ( )∑∑==

==n

jjj

n

jjjj AxpAPPxxpxp

11

|,;N

The mean of such a mixture is:

( ) ( ) ∑∑==

====n

jjj

n

jjjj xpPxxEpxpxEx

11

,;N

The covariance of the mixture is:

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )∑∑

∑∑

∑

∑

==

==

=

=

−−+−−+

−−+−−=

−+−−+−=

−−=−−

n

jj

Tjj

n

jjj

Tjj

n

jj

Tjjj

n

jjj

Tjj

n

jjj

Tjjjj

n

jjj

TT

pxxxxpAxxExx

pxxAxxEpAxxxxE

pAxxxxxxxxE

pAxxxxExxxxE

110

10

1

1

1

247


Gaussian Mixture Equations (continue – 2)

The covariance of the mixture is:

( ) ( ) ( ) ( ) ( ) ( ) PpPpxxxxpAxxxxExxxxEn

jjj

n

jj

Tjj

n

jjj

Tjj

T ~

111

+=−−+−−=−− ∑∑∑===

where:

( ) ( )∑=

−−=n

jj

Tjj pxxxxP

1

:~

Is the spread of the mean term.

Tn

jj

Tjj

n

jj

TT

x

n

jjj

x

n

jj

Tj

n

jj

Tjj

xxpxx

pxxxpxpxxpxxP

T

−=

+−−=

∑

∑∑∑∑

=

====

1

1

1111

:~

( ) ( ) Tn

jj

Tjj

n

jjj

T xxpxxpPxxxxE −+=−− ∑∑== 11

Note: Since we developed only first and second moments of the mixture, those relations will still be correct even if the random variables in the mixture are not Gaussian.

248

SOLO

Linear Gaussian Systems

A Linear Combination of Independent Gaussian random vectors is also a Gaussian random vector

mmm XaXaXaS +++= 2211:

( ) ( ) ( )( ) ( )

( ) ( ) ( )

( ) ( )

+++++++−=

+−

+−

+−=

ΦΦ⋅Φ==Φ ∫ ∫+∞

∞−

+∞

∞−

mmmm

mmmm

YYYm

YpYp

mYYmS

aaajaaa

ajaajaaja

YdYdYYpSjm

mmYY

mm

µµµωσσσω

µωσωµωσωµωσω

ωωωωω

2211222

22

22

12

12

22222

22

22

211

21

21

2

11,,

2

1exp

2

1exp

2

1exp

2

1exp

,,exp21

11

1

( ) ( )

−−= 2

2

2exp

2

1,;

i

ii

i

iiiX

XXp

i σµ

σπσµ ( ) ( ) ( )

+−==Φ ∫

+∞

∞−iiiiXiX jXdXpXj

iiµωσωωω 22

2

1expexp:

Moment-Generating

Function

Gaussian distribution

Define

Proof:

( ) ( )iXii

iX

iiYiii Xp

aa

Yp

aYpXaY

iii

11: =

=→=

( ) ( ) ( ) ( ) ( ) ( )

+−=Φ===Φ ∫∫

+∞

∞−

+∞

∞−iiiiiiX

asign

asign

iii

iXiiiiYiY ajaXaXda

a

XpXajYdYpYj

i

i

iiµωσωωωω 222

2

1expexpexp:

1

1


249

SOLO

Linear Gaussian Systems

A Linear Combination of Independent Gaussian random vectors is also a Gaussian random vector

mmm XaXaXaS +++= 2211:

Therefore the Linear Combination of Independent Gaussian Random Variables is a Gaussian Random Variable with

mmS

mmS

aaa

aaa

m

m

µµµµσσσσ

+++=

+++=

2211

2222

22

21

21

2

Therefore the Sm probability distribution is:

( ) ( )

−−=

2

2

2exp

2

1,;

m

m

m

mm

S

S

S

SSm

xSp

σµ

σπσµ

Proof (continue – 1):

( ) ( ) ( )

+++++++−=Φ mmmmS aaajaaa

mµµµωσσσωω 2211

2222

22

21

21

2

2

1exp

We found:


250


Linear Gaussian Markov Systems

kkkk

kkkkkkk

vxHz

wuGxx

+=Γ++Φ= −−−−−− 111111

wk-1 and vk, white noises, zero mean, Gaussian, independent


xxx =−= &:

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=

( ) ( ) ( ) ( ) ( ) ( ) lkT


0

&: δ=−=

( ) ( ) 0=lekeE Tvw

=≠

=lk

lklk 1

0,δ

( ) ( )Qwwpw ,0;N=

( ) ( )Rvvpv ,0;N=

( )( )

−= − wQw

Qwp T

nw1

2/12/ 2

1exp

2

1

π

( )( )

−= − vRv

Rvp T

pv1

2/12/ 2

1exp

2

1

π

A Linear Gaussian Markov Systems is defined as

( ) ( )0|0000 ,;0

Pxxxp ttx == = N ( )( )

( ) ( )

−−−= =

−== 00

10|0002/1

0|02/0 2

1exp

2

10

xxPxxP

xp tT

tntxπ

251


Linear Gaussian Markov Systems (continue – 2)

111111 −−−−−− Γ++Φ= kkkkkkk wuGxxPrediction phase (before zk measurement)

0

1:111111:1111:11| |||:ˆ −−−−−−−−−− Γ++Φ== kkkkkkkkkkkk ZwEuGZxEZxEx

or 111|111| ˆˆ −−−−−− +Φ= kkkkkkk uGxx

The expectation is

[ ] [ ] ( )[ ] ( )[ ] 1:1111|111111|111

1:11|1|1|

|ˆˆ

|ˆˆ:

−−−−−−−−−−−−−

−−−−

Γ+−ΦΓ+−Φ=

−−=

kT

kkkkkkkkkkkk

kT

kkkkkkkk

ZwxxwxxE

ZxExxExEP

( ) ( ) ( )

( ) Tk

Q

Tkkk

Tk

Tkkkkk

Tk

Tkkkkk

Tk

P

Tkkkkkkk

wwExxwE

wxxExxxxE

kk

11111

0

1|1111

1

0

11|11111|111|111

ˆ

ˆˆˆ

1|1

−−−−−−−−−−

−−−−−−−−−−−−−−

ΓΓ+Φ−Γ+

Γ−Φ+Φ−−Φ=−−

Tkk

Tkkkkkk QPP 1111|111| −−−−−−− ΓΓ+ΦΦ=

( )1|1|1:1 ,ˆ;| −−− = kkkkkkk PxxZxP NSince is a Linear Combination of Independent Gaussian Random Variables:

111111 −−−−−− Γ++Φ= kkkkkkk wuGxx

Table of Content

252

Random VariablesSOLO

Random Variable: A variable x determined by the outcome Ω of a random experiment.

( )Ω= xx

Random Process or Stochastic Process:

A function of time x determined by the outcome Ω of a random experiment.

( ) ( )Ω= ,txtx

1Ω

2Ω

3Ω

4Ω

x

t

This is a family or an ensemble of functions of time, in general different for each outcome Ω.

Mean or Ensemble Average of the Random Process: ( ) ( )[ ] ( ) ( )∫+∞

∞−

=Ω= ξξξ dptxEtx tx,:

Autocorrelation of the Random Process: ( ) ( ) ( )[ ] ( ) ( ) ( )∫ ∫+∞

∞−

+∞

∞−

=ΩΩ= ηξξξη ddptxtxEttR txtx 21 ,2121 ,,:,

Autocovariance of the Random Process: ( ) ( ) ( )[ ] ( ) ( )[ ] 221121 ,,:, txtxtxtxEttC −Ω−Ω=

( ) ( ) ( )[ ] ( ) ( ) ( ) ( ) ( )2121212121 ,,,, txtxttRtxtxtxtxEttC −=−ΩΩ=

253


Stationarity of a Random Process

1. Wide Sense Stationarity of a Random Process: • Mean Average of the Random Process is time invariant:

( ) ( )[ ] ( ) ( ) .,: constxdptxEtx tx ===Ω= ∫+∞

∞−

ξξξ

• Autocorrelation of the Random Process is of the form: ( ) ( ) ( )ττ

RttRttRtt 21:

2121 ,−=

=−=

( ) ( ) ( )[ ] ( ) ( ) ( ) ( )12,2121 ,,,:,21

ttRddptxtxEttR txtx === ∫ ∫+∞

∞−

+∞

∞−

ηξξξηωωsince:

We have: ( ) ( )ττ −= RR

Power Spectrum or Power Spectral Density of a Stationary Random Process:

( ) ( ) ( )∫+∞

∞−

−= ττωτω djRS exp:

2. Strict Sense Stationarity of a Random Process: All probability density functions are time invariant: ( ) ( ) ( ) .,, constptp xtx == ωωω

Ergodicity:

( ) ( ) ( )[ ]Ω==Ω=Ω ∫+

−∞→

,,2

1:, lim txExdttx

Ttx

ErgodicityT

TT

A Stationary Random Process for which Time Average = Assembly Average

254


Time Autocorrelation:

Ergodicity:

( ) ( ) ( ) ( ) ( )∫+

−∞→

Ω+Ω=Ω+Ω=T

TT

dttxtxT

txtxR ,,2

1:,, lim τττ

For a Ergodic Random Process define

Finite Signal Energy Assumption: ( ) ( ) ( ) ∞<Ω=Ω= ∫+

−∞→

T

TT

dttxT

txR ,2

1,0 22 lim

Define: ( ) ( ) ≤≤−Ω

=Ωotherwise

TtTtxtxT 0

,:, ( ) ( ) ( )∫

+∞

∞−

Ω+Ω= dttxtxT

R TTT ,,2

1: ττ

( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( )∫∫∫

∫∫∫

−−

−

−

+∞

−

−

−

−

∞−

Ω+Ω−Ω+Ω=Ω+Ω=

Ω+Ω+Ω+Ω++Ω=

T

T

TT

T

T

TT

T

T

TT

T

TT

T

T

TT

T

TTT

dttxtxT

dttxtxT

dttxtxT

dttxtxT

dttxtxT

dttxtxT

R

τ

τ

τ

τ

τττ

ττωττ

,,2

1,,

2

1,,

2

1

,,2

1,,

2

1,,

2

1

00

Let compute:

( ) ( ) ( ) ( ) ( )∫∫−∞→−∞→∞→

Ω+Ω−Ω+Ω=T

T

TTT

T

T

TTT

TT

dttxtxT

dttxtxT

Rτ

τττ ,,2

1,,

2

1limlimlim

( ) ( ) ( )ττ RdttxtxT

T

T

TT

T

=Ω+Ω∫−∞→

,,2

1lim

( ) ( ) ( ) ( )[ ] 0,,2

1,,

2

1 suplimlim →

Ω+Ω≤Ω+Ω≤≤−∞→−∞→

∫ τττττ

txtxT

dttxtxT TT

TtTT

T

T

TTT

therefore: ( ) ( )ττ RRTT

=→∞

lim

( ) ( ) ( )[ ]Ω==Ω=Ω ∫+

−∞→

,,2

1:, lim txExdttx

Ttx

ErgodicityT

TT

T− T+

( )txT

t

255


Ergodicity (continue - 1):

( ) ( ) ( ) ( ) ( )

( ) ( )[ ] ( ) ( )( )[ ]

( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( ) [ ]TTTT

TT

TT

TTT

XXT

dvvjvxdttjtxT

dtjtxdttjtxT

ddttjtxtjtxT

dttxtxdjT

djR

*

2

1exp,exp,

2

1

exp,exp,2

1

exp,exp,2

1

,,exp2

1exp

=−ΩΩ=

+−Ω+Ω=

+−Ω+Ω=

Ω+Ω−=−

∫∫

∫∫

∫ ∫

∫ ∫∫

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

+∞

∞−

+∞

∞−

+∞

∞−

ωω

ττωτω

ττωτω

τττωττωτLet compute:

where: and * means complex-conjugate.( ) ( )∫+∞

∞−

−Ω= dvvjvxX TT ωexp,:

Define:

( ) ( ) ( ) ( ) ( ) ( )[ ]∫ ∫∫+∞

∞−

+

−∞→

+∞

∞−∞→∞→

Ω+Ω−=

−=

= τττωττωτω ddttxtxE

TjdjRE

T

XXES

T

T

TTT

TT

TT

T

,,2

1expexp

2: limlimlim

*

Since the Random Process is Ergodic we can use the Wide Stationarity Assumption:

( ) ( )[ ] ( )ττ RtxtxE TT =Ω+Ω ,,

( ) ( ) ( ) ( ) ( )

( ) ( )∫

∫ ∫∫ ∫∞+

∞−

+∞

∞−

+

−∞→

+∞

∞−

+

−∞→∞→

−=

−=

−=

=

ττωτ

ττωττττωω

djR

ddtT

jRddtRT

jT

XXES

T

TT

T

TT

TT

T

exp

2

1exp

2

1exp

2:

1

*

limlimlim

256


Ergodicity (continue - 2):

We obtained the Wiener-Khinchine Theorem (Wiener 1930):

( ) ( ) ( )∫+∞

∞−→∞−=

= dtjR

T

XXES TT

T

τωτω exp2

:*

lim


Alexander YakovlevichKhinchine1894 - 1959

The Power Spectrum or Power Spectral Density of a Stationary Random Process S (ω) is the Fourier Transform of the Autocorrelation Function R (τ).

257


White Noise

A (not necessary stationary) Random Process whose Autocorrelation is zero for any two different times is called white noise in the wide sense.

( ) ( ) ( )[ ] ( ) ( )211

2

2121 ,,, ttttxtxEttR −=ΩΩ= δσ

( )1

2 tσ - instantaneous variance

Wide Sense Whiteness

Strict Sense Whiteness

A (not necessary stationary) Random Process in which the outcome for any two different times is independent is called white noise in the strict sense.

( ) ( ) ( ) ( )2121, ,,21

ttttp txtx −=Ω δ

A Stationary White Noise Random has the Autocorrelation:

( ) ( ) ( )[ ] ( )τδσττ 2,, =Ω+Ω= txtxER

Note

In general whiteness requires Strict Sense Whiteness. In practice we have only moments (typically up to second order) and thus only Wide Sense Whiteness.

258


White Noise

A Stationary White Noise Random has the Autocorrelation:

( ) ( ) ( )[ ] ( )τδσττ 2,, =Ω+Ω= txtxER

The Power Spectral Density is given by performing the Fourier Transform of the Autocorrelation:

( ) ( ) ( ) ( ) ( ) 22 expexp στωτδστωτω =−=−= ∫∫+∞

∞−

+∞

∞−

dtjdtjRS

( )ωS

ω2σ

We can see that the Power Spectrum Density contains all frequencies at the same amplitude. This is the reason that is called White Noise.

The Power of the Noise is defined as: ( ) ( ) 20 σωτ ==== ∫+∞

∞−

SdtRP

259


Table of Content

Markov Processes

A Markov Process is defined by:

Andrei AndreevichMarkov

1856 - 1922

( ) ( )( ) ( ) ( )( ) 111 ,|,,,|, tttxtxptxtxp >∀ΩΩ=≤ΩΩ ττ

i.e. the Random Process, the past up to any time t1 is fully defined by the process at t1.

Examples of Markov Processes:

1. Continuous Dynamic System( ) ( )( ) ( )wuxthtz

vuxtftx

,,,

,,,

==

2. Discrete Dynamic System

( ) ( )( ) ( )kkkkk

kkkkk

wuxthtz

vuxtftx

,,,

,,,

1

1

==

+

+

x - state space vector (n x 1)u - input vector (m x 1)v - white input noise vector (n x 1)

- measurement vector (p x 1)z

- white measurement noise vector (p x 1)w

260


Table of Content

Markov Processes


3. Continuous Linear Dynamic System( ) ( ) ( )( ) ( )txCtz

tvtxAtx

=+=

Using the Fourier Transform we obtain: ( ) ( )( )

( ) ( ) ( )ωωωωωω

VVAIjCZ HH

=−= −

1

Using the Inverse Fourier Transform we obtain:

( ) ( ) ( )∫+∞

∞−

= ξξξ dvtHtz ,

( ) ( ) ( ) ( ) ( ) ( ) ( )

( )

( )

( ) ( )( )( )

( ) ( ) ( )∫∫ ∫

∫ ∫∫

∞+

∞−

∞+

∞−

−

∞+

∞−

+∞

∞−

+∞

∞−

+∞

∞−

−=−=

−==

ξξξξξωξωωπ

ωωξξωξωπ

ωωωωπ

ξ

ω

dvtHdvdtj

dtjdjvdtjVtz

tH

egrattionoforderchange

V

exp2

1

expexp2

1exp

2

1

int

H

HH

261


Table of Content

Markov Processes


3. Continuous Linear Dynamic System( ) ( ) ( )( ) ( )txCtz

tvtxAtx

=+=

The Autocorrelation of the output is:

( ) ( ) ( )∫+∞

∞−

= ξξξ dvtHtz ,

( ) ( ) ( )[ ] ( ) ( ) ( ) ( )

( ) ( ) ( )[ ] ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )∫∫

∫ ∫∫ ∫

∫∫

∞+

∞−

−=∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

+∞

∞−

+∞

∞−

+=−+−=

−−+−=−+−=

−+−=+=

ζτζζξξτξ

ξξξξδξτξξξξτξξξ

ξξτξξξξττ

ξζdHSHdtHStH

ddtHStHddtHvvEtH

dtHvdvtHEtztzER

Tvv

tT

vv

Tvv

TT

TTTzz

1

111

212121211211

222111

( ) ( ) ( )[ ] ( )τδττ vvT

vv StvtvER =+=

( ) ( ) ( ) ( ) ( ) vvvvvvvv SdjSdjRS =−=−= ∫∫+∞

∞−

+∞

∞−

ττωτδττωτω expexp

( ) ( ) ( )( ) ( )

( ) ( ) ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( )ωωχχωχζζωζ

χχωζζωζχττζωζζωζτζ

ττωζτζζττωτω

χτζ

ττ

*expexp

expexpexpexp

expexp

HH vvT

vv

Tvv

Tvv

Tvv

RR

zzzz

SdjHSdjH

djdjHSHdjdjHSH

djdHSHdjRSzzzz

=

−=

−=−−−=

−−=−=

∫∫

∫ ∫∫ ∫

∫ ∫∫

∞+

∞−

∞+

∞−

∞+

∞−

∞+

∞−

=+∞+

∞−

∞+

∞−

+∞

∞−

+∞

∞−

−=+∞

∞−

( ) ( ) ( ) ( ) conjugatecomplexSS vvzz −== ∗ωωωω *HH

262


Table of Content

Markov Processes


4. Continuous Linear Dynamic System ( ) ( ) ( )∫+∞

∞−

= ξξξ dvthtz ,

( ) ( ) ( )[ ] ( )τδσττ 2

vvv tvtvER =+= ( ) 2

vvvS σω =

v (t) z (t)( )xj

KH

ωωω

/1+=

( )xj

KH

ωωω

/1+=

The Power Spectral Density of the output is:

( ) ( ) ( ) ( ) ( ) 2

22*

/1 x

vvvzz

KHSHS

ωωσωωωω

+==

( ) ( ) 2

22

/1 x

vvzz

KS

ωωσω

+=

ω

xω

22

vvK σ

2/22

vvK σ

The Autocorrelation of the output is:( ) ( ) ( )

( ) ( ) ( ) ( )∫∫

∫∞+

∞−

=∞+

∞−

+∞

∞−

−−

=+

=

=

dsss

K

jdj

K

djSR

x

vjs

x

v

zzzz

τωσ

πωτω

ωωσ

π

ωτωωπ

τ

ω

exp/12

1exp

/12

1

exp2

1

2

22

2

22

ωj

xω

R

( ) 0/1 2

22

=−∫

∞→R

s

x

vv dses

K τ

ωσ( ) 0

/1 2

22

=−∫

∞→R

s

x

vv dses

K τ

ωσ

xω−

σ

ωσ js +=

0<τ0>τ

( ) τωσωω xeK

R vvxzz

==2

22

τ

2/22

vvxK σω

( )τωσω

xvxK

−= exp2

22

( ) ( )

( ) ( )

>

+

−−=

−−

<

−

−=

−−

=

∫

∫

→

−→

0exp

Reexp2

1

0exp

Reexp2

1

222

22

222

222

22

222

τω

τσωτ

ωσω

π

τω

τσωτ

ωσω

π

ωω

ωω

x

vx

x

vx

x

vx

x

vx

s

sKsdss

s

K

j

s

sKsdss

s

K

j

x

x

263


Markov Processes


5. Continuous Linear Dynamic System with Time Variable Coefficients

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )21121&

:&:

tttQteteE

twEtwtetxEtxteT

ww

wx

−=

−=−=

δ

w (t) x (t)

( )tF

( )tG ∫x (t)

( ) ( ) ( ) ( ) ( ) ( )twtGtxtFtxtxtd

d +==

( ) ( ) ( ) ( ) ( )tetGtetFte wxx +=

( ) ( ) ( ) ( ) ( ) ( )∫Φ+Φ=t

t

dwGttxtttx0

,, 00 λλλλ

The solutions of the Linear System are:

where:

( ) ( ) ( ) ( ) ( ) ( ) ( )3132210000 ,,,&,&,, ttttttItttttFtttd

d Φ=ΦΦ=ΦΦ=Φ

( ) ( ) ( ) ( ) ( ) ( )∫Φ+Φ=t

t

wxx deGttettte0

,, 00 λλλλ

( ) ( ) ( ) ( ) ( ) twEtGtxEtFtxE +=

264

Random VariablesSOLO Markov Processes


5. Continuous Linear Dynamic System with Time Variable Coefficients (continue – 1)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )21121&

:&:

tttQteteE

twEtwtetxEtxteT

ww

wx

−=

−=−=

δ

w (t) x (t)

( )tF

( )tG ∫x (t)

( ) ( ) ( ) ( ) ( ) ( )∫Φ+Φ=t

t

dwGttxtttx0

,, 00 λλλλ ( ) ( ) ( ) ( ) ( ) ( )∫Φ+Φ=t

t

wxx deGttettte0

,, 00 λλλλ

( ) ( ) ( ) ( ) ( )ttRteteEtxVartV xT

xxx ,: ===( ) ( ) ( ) 2121 :, teteEttR Txxx =

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( )∫ ∫∫

∫

∫∫

ΦΦ+Φ

Φ+

ΦΦ+ΦΦ=

Φ+Φ

Φ+Φ=

−

1

0

2

0 211

1

0

2

000

2

0

1

0

222222111102101111

2222200102

,

0001

222220021111100121

1,,,,

,,,,

,,,,,

t

t

t

t

TT

Q

Tww

Tt

t

T

t

t

TTTT

ttV

Txx

Tt

t

t

t

x

ddtGeeEGtttdtxwEGt

dtGwtxEttttteteEtt

dwGttxttdwGttxttEttR

x

λλλλλλλλλλλλ

λλλλ

λλλλλλλλ

λλδλ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )( )

∫∫ ∫=

−

ΦΦ=ΦΦ

≤≤←==21

0

1

0

2

0 211

,min

2122221111

212102001

,,,,

,,0ttt

t

TTt

t

t

t

TT

Q

Tww

TT

dtGQGtddtGeeEGt

tttwtxEtxwE

λλλλλλλλλλλλλλ

λλλλ

λλδλ

265



5. Continuous Linear Dynamic System with Time Variable Coefficients (continue – 2)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )21121&

:&:

tttQteteE

twEtwtetxEtxteT

ww

wx

−=

−=−=

δ

w (t) x (t)

( )tF

( )tG ∫x (t)

( ) ( ) ( ) ( ) ( ) ( )∫Φ+Φ=t

t

dwGttxtttx0

,, 00 λλλλ ( ) ( ) ( ) ( ) ( ) ( )∫Φ+Φ=t

t

wxx deGttettte0

,, 00 λλλλ

( ) ( ) ( ) ( ) ( )ttRteteEtxVartV xT

xxx ,: ===( ) ( ) ( ) 2121 :, teteEttR Txxx =

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )( )

∫=

ΦΦ+ΦΦ==21

0

,min

0200012121 ,,,,,,ttt

t

TTTx

Tx dtGQGtttttVtttxtxEttR λλλλλλ

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )∫ ΦΦ+ΦΦ===t

t

TTTxx

Tx dtGQGtttttVttttRtxtxEtV

0

,,,,,, 0000 λλλλλλ

266



6. Discrete Linear Dynamic System with Variable Coefficients

( ) ( ) ( ) ( ) ( )kwkkxkkx Γ+Φ=+1( ) ( ) ( )

( ) ( ) ( )lkQlekeE

kwEkwke

wT

ww

w

−=

−=

δ

:

( ) ( ) ( ) ( ) ( ) ( )kXkekeE

kxEkxkeT

xx

x

=

−=: ( ) ( ) lkkekeE Twx ,0 ∀=

( ) ( ) ( ) ( ) ( ) kwEkkxEkkxE Γ+Φ=+1

( ) ( ) ( ) ( ) ( )kekkekke wxx Γ+Φ=+1

( ) ( ) ( ) ( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( ) ( ) ( )kekkekkkekkkekkekke wwx

kk

wxx 1111112,2

+Γ+Γ+Φ+Φ+Φ=+Γ+++Φ=++Φ

( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( )∑−+

=+Φ

Γ++Φ+Φ+Φ−+Φ=+1

,

1,11lk

knwx

klk

x nennlkkekklklke

where we defined ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )kmknnmIkkkklkklk ,,,&,11:, Φ=ΦΦ=ΦΦ+Φ−+Φ=+Φ

Hence ( ) ( ) ( ) ( ) ( ) ( )∑−+

=

Γ++Φ++Φ=+1

1,,lk

knwxx nennlkkeklklke

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∑−+

=

Γ++Φ++Φ=+1

1,,lk

kn

Txw

Txx

Txx keneEnnlkkekeEklkkelkeE

267



6. Discrete Linear Dynamic System with Variable Coefficients (continue – 1)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∑−+

=

Γ++Φ++Φ=+1

1,,lk

kn

Txw

Txx

Txx keneEnnlkkekeEklkkelkeE

( ) ( ) ( ) ( ) ( ) ( )∑−+

=

Γ++Φ++Φ=+1

1,,lk


( ) ( ) ( ) ( ) ( ) ( )∑−

−=

Γ+Φ+−−Φ=1

1,,k

lkmwxx memmklkelkkke

=−

→,2,1l

lkk

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( )∑−

−= −

+ΦΓ+−Φ−=1

1,,k

lkm

TT

mnQ

Tww

TTxw

Txw mkmmeneElkklkeneEkeneE

w

δ

[ ][ ]

=−−∈−+∈

,2,1

1,

1,

l

klkm

lkkn ( ) ( ) ( ) 0

0

=−=−

nmQ

lkeneE

w

Txw

δ( ) ( ) 0=keneE T

xw

( ) ( ) ( ) ( ) ( ) kekeEklkkelkeE Txx

Txx ,+Φ=+

268



6. Discrete Linear Dynamic System with Variable Coefficients (continue – 2)

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )∑−+

=

++ΦΓ++Φ=+1

1,,lk

kn

TTTwx

TTxx

Txx nlknnekeEklkkekeElkekeE

( ) ( ) ( ) ( ) ( ) ( )∑−+

=

Γ++Φ++Φ=+1

1,,lk


( ) ( ) ( ) ( ) ( ) ( )∑−

−=

Γ+Φ+−−Φ=1

1,,k

lkmwxx memmklkelkkke

=−

→,2,1l

lkk

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

∑−

−= −

Γ+Φ+−−Φ=1

1,,k

lkmnmQ

Tww

Tw

Tx

Twx

w

nemeEmmknelkeElkknekeE

δ

[ ][ ]

=−−∈−+∈

,2,1

1,

1,

l

klkm

lkkn ( ) ( ) ( ) 0

0

=−=−

mnQ

nelkeE

w

Twx

δ( ) ( ) 0=nekeE T

wx

( ) ( ) ( ) ( ) ( )klkkekeElkekeE TTxx

Txx ,+Φ=+

Table of Content

269

SOLO Matrices Trace of a Square Matrix The trace of a square matrix is defined as ( ) ( )T

nn

n

iiinn AtraceaAtrace ×

=× == ∑

1

:

q.e.d.

( ) ( )ABtraceBAtrace =1

Proof:

( ) ∑ ∑= =

=

n

i

n

jjiij baBAtrace

1 1

( ) ( )BAtracebaabABtracen

i

n

jjiij

n

j

n

iijji ==

= ∑ ∑∑ ∑

= == = 1 11 1

( ) ( ) ( ) ( ) ( ) ( ) ( )( )

( )ABtraceBAtraceBAtraceABtraceABtraceBAtrace TTTT111

=≠===2

Proof:

( ) ( ) ( )ABtraceBAtracebabaBAtracen

i

n

jjiij

n

i

n

jijij

T ==

≠

= ∑ ∑∑ ∑

= == = 1 11 1

( ) ( )Tn

j

n

iijij

T BAtraceabABtrace =

= ∑ ∑

= =1 1q.e.d.

270


nn

n


=× == ∑

1

:

3

Proof:

q.e.d.

( ) ( ) ( )∑=

− ==n

ii APAPtraceAtrace

1

1 λ

where P is the eigenvector matrix of A related to the eigenvalue matrix Λ of A by

=Λ=

n

PPPA

λ

λ

0

01

( ) ( ) ( ) ( )AtraceAPPtracePAPtrace == −− 11

1

=Λ=

n

PPPA

λ

λ

0

01

=Λ=→ −

n

PAP

λ

λ

0

01

1

( ) ( ) ∑=

− =Λ=→n

i

itracePAPtace1

1 λ

271


nn

n


=× == ∑

1

:

Proof:

q.e.d.

Definition

4( )AtraceA ee =det

( )AtraceA eeePeP

PePPePe

n

i

i

======∑

=ΛΛΛ−Λ− 1detdetdetdet

1detdetdetdetdet 11

λ

If aij are the coefficients of the matrix Anxn and z is a scalar function of aij, i.e.:

( ) njiazz ij ,,1, ==

then is the matrix nxn whose coefficients i,j areA

z

∂∂

njia

z

A

z

ijij

,,1,: =∂∂=

∂∂

(see Gelb “Applied Optimal Estimation”, pg.23)

272


nn

n


=× == ∑

1

:

Proof:

q.e.d.

5( ) ( ) ( )

A

AtraceI

A

Atrace T

n ∂∂==

∂∂ 1

( )

=≠

==∂∂=

∂

∂ ∑= ji

jia

aA

Atraceij

n

i

ii

ijij1

0

1

δ

6( ) ( ) ( ) ( ) nmmnTTT RBRCCBBC

A

BCAtrace

A

ABCtrace ×× ∈∈==∂

∂=∂

∂ 1

Proof:

( ) ( ) ( )[ ] ijT

ji

m

ppijp

ik

jl

n

l

m

p

n

kklpklp

ijij

BCBCbcabcaA

ABCtrace ===∂

∂=

∂

∂ ∑∑ ∑ ∑=

=

== = = 11 1 1q.e.d.

7 If A, B, C ∈ Rnxn,i.e. square matrices, then

( ) ( ) ( ) ( ) ( ) ( ) TTT CBBCA

BCAtrace

A

CABtrace

A

ABCtrace ==∂

∂=∂

∂=∂

∂ 11

273


nn

n


=× == ∑

1

:

Proof:

q.e.d.

8 ( ) ( ) ( ) ( ) ( )( ) ( )nmmn

TTT

RBRCBCA

ABCtrace

A

BCAtrace

A

ABCtrace ×× ∈∈=∂

∂=∂

∂=∂

∂ 721

9

( ) ( ) ( ) ( ) ( ) ( )BC

A

BCAtrace

A

CABtrace

A

ABCtrace TTT 811

=∂

∂=∂

∂=∂

∂

If A, B, C ∈ Rnxn,i.e. square matrices, then

10

( ) TAA

Atrace2

2

=∂

∂

( ) ( ) ( ) ijT

jiji

n

l

n

mmllm

ijijij

Aaaaaaa

Atrace

A

Atrace2

1 1

22

=+=

∂∂=

∂∂=

∂

∂ ∑ ∑= =

11

( ) ( ) 1−=∂

∂ kTk

AkA

Atrace

Proof:( ) ( ) ( ) ( ) ( ) 1111 −−−− =+++=

∂

⋅∂

=∂

∂ kT

k

kTkTkT

k

k

AkAAAA

AAAtrace

A

Atrace

q.e.d.

274


nn

n


=× == ∑

1

:

Proof:

q.e.d.

12

( ) TAA

eA

etrace =∂

∂

( ) ( ) ( ) TAn

k

n

k

kT

n

kkkT

n

n

k

k

n

n

k

k

n

A

eAk

Ak

k

k

Atrace

Ak

Atrace

AA

etrace ===

∂∂=

∂∂=

∂∂ ∑ ∑∑∑

= =→∞

→−−

→∞=

→∞=

→∞1 0

11

00 !

1lim

!lim

!lim

!lim

13

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( ) ( ) ( )TT

TTTTTTTTT

TTTTT

TTT

BACBAC

A

ACABtrace

A

BACAtrace

A

ABACtrace

A

CABAtrace

A

BACAtrace

A

CABAtrace

A

ACABtrace

A

BACAtrace

A

ABACtrace

+=

∂∂=

∂∂=

∂∂=

∂∂=

∂∂=

∂∂=

∂∂=

∂∂=

∂∂

111

21

11

( ) ( ) ( ) ( ) ( ) ( ) TTTTTTT

BACBACCABBACA

ABACtrace

A

ABACtrace

A

ABACtrace +=+==∂

∂+∂

∂=∂

∂ + 86

2

2

1

1Proof: q.e.d.

14

( ) ( ) ( )A

A

AAtrace

A

AAtrace TT

213

=∂

∂=∂

∂Table of Content

275

Functional AnalysisSOLO

Inner Product

If X is a complex linear space, for the Inner Product < , > between the elements (a complex number) is defined by:

Xzyx ∈∀ ,,

><>=< xyyx ,,1 Commutative law><+>>=<+< zxyxzyx ,,,2 Distributive law

Cyxyx ∈∀><>=< λλλ ,,300,&0, =⇔=><≥>< xxxxx4

Define: ( ) ( ) ( ) ( ) ( )( )

( )( )

( )

( )

=

==>< ∫

tg

tg

tg

tf

tf

tfdttgtftgtf

nn

T

11

,:,

Table of Content

276

SignalsSOLO

Signal Duration and Bandwidth

then

( ) ( )∫+∞

∞−

−= tdetsfS tfi π2 ( ) ( )∫+∞

∞−

= fdefSts tfi π2

t

t∆2

t

( ) 2ts

ff

f∆2

( ) 2fS

( ) ( )

( )

2/1

2

22

:

−

=∆

∫

∫∞+

∞−

+∞

∞−

tdts

tdtstt

t

( )

( )∫

∫∞+

∞−

+ ∞

∞−=tdts

tdtst

t2

2

:

Signal Duration Signal Median

( ) ( )

( )

2/1

2

2224

:

−

=∆

∫

∫∞+

∞−

+∞

∞−

fdfS

fdfSff

f

π ( )

( )∫

∫∞+

∞−

+ ∞

∞−=fdfS

fdfSf

f2

22

:

π

Signal Bandwidth Frequency Median

Fourier

277

Signals

( ) ( )∫+∞

∞−

= fdefSts tfi π2

SOLO

Signal Duration and Bandwidth (continue – 1)

( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )∫∫ ∫

∫ ∫∫ ∫∫∞+

∞−

∞+

∞−

∞+

∞−

−

∞+

∞−

∞+

∞−

−∞+

∞−

∞+

∞−

∞+

∞−

=

=

=

=

dffSfSdfdesfS

dfdefSsdfdefSsdss

tfi

tfitfi

ττ

τττττττ

π

ππ

2

22

( ) ( )∫+∞

∞−

= fdefSts tfi π2 ( ) ( ) ( )∫+∞

∞−

== fdefSfitd

tsdts tfi ππ 22'

( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )∫∫ ∫

∫ ∫∫ ∫∫∞+

∞−

∞+

∞−

∞+

∞−

−

+∞

∞−

+∞

∞−

−+∞

∞−

+∞

∞−

−+∞

∞−

=

−=

−=

−=

dffSfSfdfdesfSfi

dfdesfSfidfdefSfsidss

tfi

tfitfi

222

22

2'2

'2'2''

πττπ

ττπττπτττ

π

ππ

( ) ( )∫∫+∞

∞−

+∞

∞−

= dffSds 22 ττ

Parseval Theorem

From

From

( ) ( )∫∫+∞

∞−

+∞

∞−

= dffSfdtts2222

4' π

278

Signals

( )

( )

( ) ( )

( )

( ) ( )

( )

( ) ( )

( )

( ) ( )

( )∫

∫

∫

∫ ∫

∫

∫ ∫

∫

∫

∫

∫∞+

∞−

+∞

∞−∞+

∞−

+∞

∞−

+∞

∞−

−

∞+

∞−

+∞

∞−

+∞

∞−

−

∞+

∞−

+∞

∞−∞+

∞−

+∞

∞− =====dffS

fdfdfSd

fSi

dffS

fdtdetstfS

dffS

tdfdefStst

dffS

tdtstst

tdts

tdtst

t

fifi

22

2

2

2

22

2

2:

πππ

SOLO

Signal Duration and Bandwidth

( ) ( )∫+∞

∞−

−= tdetsfS tfi π2 ( ) ( )∫+∞

∞−

= fdefSts tfi π2Fourier

( ) ( )∫+∞

∞−

−−= tdetstifd

fSd tfi ππ 22( ) ( )∫

+∞

∞−

= fdefSfitd

tsd tfi ππ 22

( )

( )

( ) ( )

( )

( ) ( )

( )

( ) ( )

( )

( ) ( )

( )∫

∫

∫

∫ ∫

∫

∫ ∫

∫

∫

∫

∫∞+

∞−

+∞

∞−∞+

∞−

+∞

∞−

+∞

∞−∞+

∞−

+∞

∞−

+∞

∞−∞+

∞−

+∞

∞−∞+

∞−

+∞

∞−

−=

====tdts

tdtd

tsdtsi

tdts

tdfdefSfts

tdts

fdtdetsfSf

tdts

fdfSfSf

fdfS

fdfSf

f

fifi

22

2

2

2

22

2 2222

:

ππ ππππ

279

Signals

( ) ( ) ( ) ( ) ( )∫∫∫∫∫+∞

∞−

+∞

∞−

+∞

∞−

+∞

∞−

+∞

∞−

=≤

dffSfdttstdttsdttstdtts

222222

2

2 4'4

1 π

( ) ( )∫∫+∞

∞−

+∞

∞−

= dffSdts22 τ

SOLO


0&0 == ftChange time and frequency scale to get

From Schwarz Inequality: ( ) ( ) ( ) ( )∫∫∫+∞

∞−

+∞

∞−

+∞

∞−

≤ dttgdttfdttgtf22

Choose ( ) ( ) ( ) ( ) ( )tstd

tsdtgtsttf ':& ===

( ) ( ) ( ) ( )∫∫∫+∞

∞−

+∞

∞−

+∞

∞−

≤ dttsdttstdttstst22

''we obtain

( ) ( )∫+∞

∞−

dttstst 'Integrate by parts( )

=+=

→

==

sv

dtstsdu

dtsdv

stu '

'

( ) ( ) ( ) ( ) ( )∫∫∫+∞

∞−

+∞

∞−

∞+

∞−

+∞

∞−

−−= dttststdttsstdttstst '' 2

0

2

( ) ( ) ( )∫∫

+∞

∞−

+∞

∞−

−= dttsdttstst 2

2

1'

( ) ( )∫∫+∞

∞−

+ ∞

∞−

= dffSfdtts2222

4' π

( )

( )

( )

( )

( )

( )

( )

( )∫

∫

∫

∫

∫

∫

∫

∫∞+

∞−

+∞

∞−∞+

∞−

+∞

∞−∞+

∞−

+∞

∞−∞+

∞−

+∞

∞− =≤dffS

dffSf

dtts

dttst

dtts

dffSf

dtts

dttst

2

222

2

2

2

222

2

244

4

1ππ

assume ( ) 0lim =→∞

tstt

280

SignalsSOLO


( )

( )

( )

( )

( )

( )

22

2

222

2

24

4

1

ft

dffS

dffSf

dtts

dttst

∆

∞+

∞−

+∞

∞−

∆

∞+

∞−

+∞

∞−

≤

∫

∫

∫

∫ π

Finally we obtain ( ) ( )ft ∆∆≤2

1

0&0 == ftChange time and frequency scale to get

Since Schwarz Inequality: becomes an equalityif and only if g (t) = k f (t), then for:

( ) ( ) ( ) ( )∫∫∫+∞

∞−

+∞

∞−

+∞

∞−

≤ dttgdttfdttgtf22

( ) ( ) ( ) ( )tftsteAttd

sdtgeAts tt ααα αα 222:

22

−=−=−==⇒= −−

we have ( ) ( )ft ∆∆=2

1Table of Content

2 estimators

Science

Transcript of 2 estimators