View
638
Download
4
Category
Tags:
Preview:
Citation preview
1
Estimators
SOLO HERMELIN
Updated: 22.02.09 17.06.14
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
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
−=−=−= −−−−
−
Estimators for Static Systems
15
v
H zx
2
0
22
0*
111 −−− −=−=WWW
xHzxHzJ
SOLO
Optimal Weighted Least-Square Estimate (continue – 2)
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
Estimators for Static Systems
16
v
H zx
2
0
22
0*
111 −−− −=−=WWW
xHzxHzJ
SOLO
Optimal Weighted Least-Square Estimate (continue – 3)
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
−−−−−− =−+=−
Estimators for Static Systems
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
Estimators for Static Systems
19
SOLO
Recursive Weighted Least Square Estimate (RWLS) (continue -1)
( ) ( ) 1
0
1
00:−−=− HWHP T( ) ( ) 0
1
00 zWHPx T −−=−
( ) ( ) [ ] [ ]
( ) ( )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
−−−
−−
++−+−−−−=
++=+
Estimators for Static Systems
20
v
H zx
SOLO
Recursive Weighted Least Square Estimate (RWLS) (continue -2)
( ) ( ) ( )( ) ( ) ( )[ ] ( ) ( )
( )( )
( ) ( )[ ]( )
( )( )
( )
( ) ( ) ( ) ( ) 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
00 zWHPx T −−=−
( ) ( ) HWHPP T 111 −−− +−=+
( ) ( ) ( ) ( )( )−−++−=+ − xHzWHPxx T 1
Recursive Weighted Least Square Estimate (RWLS)
z
( )−x
( )+x
Delay
( ) HWHP T 11 −− =+
H
( ) 1−+ WHP T
Estimator
Estimators for Static Systems
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
Recursive Weighted Least Square Estimate (RWLS) (continue -3)
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
Estimators for Static Systems
22
( ) 0
1
00
1 : HWHP T −− =−
SOLO
Recursive Weighted Least Square Estimate (RWLS) (continue -4)
Second Way (continue – 1)
We want to prove that
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 −− =−
Estimators for Static Systems
23
( ) 0
1
00
1 : HWHP T −− =− ( ) ( ) 0
1
00: zWHPx T −−=−
SOLO
Recursive Weighted Least Square Estimate (RWLS) (continue -5)
Second Way (continue – 2)
We want to prove that
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.
Estimators for Static Systems
24
( )[ ] ( ) ( )[ ] ( ) ( )xHzWxHzxxPxxJ TT −−+−−−−−= −− 11
1
SOLO
Recursive Weighted Least Square Estimate (RWLS) (continue -6)
Second Way (continue – 5)
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
Estimators for Static Systems
25
SOLO
Recursive Weighted Least Square Estimate (RWLS) (continue -7)
( ) ( ) 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
Estimators for Static Systems
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
Estimators for Static Systems
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
Estimators for Static Systems
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
Estimators for Static Systems
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
Estimators for Static Systems
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/π
Estimators for Static Systems
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
Estimators for Static Systems
32
SOLO
Bayesian Maximum Likelihood Estimate (Maximum Aposterior Estimate) (continue – 2)
( ) ( ) ( )( ) ( ) ( )( ) ( )( ) ( )[ ] ( )( )−−+−−−−−−−−−+−−−−− 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|
π
Estimators for Static Systems
33
SOLO
Bayesian Maximum Likelihood Estimate (Maximum Aposterior Estimate) (continue – 3)
then
where: ( ) ( )[ ] 111:−−− +−=+ HRHPP T
( )( ) ( )
( ) ( ) ( )[ ] ( ) ( ) ( ) ( )[ ]
−+−−−+−+−−−−⋅
+= −−− xHzRHPxxPxHzRHPxx
Pzxp TTT
nzx111
2/12/| 2
1exp
2
1|
π
( )zxp zxx
|max | ( ) ( ) ( ) ( )( )−−++−==+ − xHzRHPxxx T 1*:
Table of Content
Estimators for Static Systems
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
The Cramér-Rao Lower Bound on the Variance of the Estimator (continue – 2)
[ ]( ) [ ] [ ] ( )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
The Cramér-Rao Lower Bound on the Variance of the Estimator (continue – 3)
[ ]( ) [ ]( )
[ ] [ ]∫∫
∂
∂
∂
∂+≥−
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
The Cramér-Rao Lower Bound on the Variance of the Estimator (continue – 4)
[ ] [ ] ( ) [ ]( )
[ ] [ ]( )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
The Cramér-Rao Lower Bound on the Variance of the Estimator (continue – 5)
( )
[ ]( )
( )
[ ] ( )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 Cramér-Rao Lower Bound on the Variance of the Estimator (continue – 5)
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
Kalman Filter Discrete Case (continue – 2)
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
Kalman Filter Discrete Case (continue – 3)
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
Kalman Filter Discrete Case (continue – 4)
( ) ( ) ( ) ( ) 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
Kalman Filter Discrete Case (continue – 5)
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
Kalman Filter Discrete Case (continue – 6)
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
Kalman Filter Discrete Case (continue – 7)
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
Kalman Filter Discrete Case (continue – 8)
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
Kalman Filter Discrete Case (continue – 9)
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
Kalman Filter Discrete Case (continue – 10)
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
Kalman Filter Discrete Case (continue – 9)
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
( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
kkkk
kkkkkkk
vxHz
wuGxx
+=Γ++Φ= −−−−−− 111111
SOLO
Kalman Filter Discrete Case - Innovation Assume a discrete dynamic system
( ) ( ) ( ) ( ) ( ) ( ) lkT
www kQlekeEkwEkwke ,
0
&: δ=−=
( ) ( ) ( ) ( ) ( ) ( ) lkT
vvv kRlekeEkvEkvke ,
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
Recursive Bayesian EstimationSOLO
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 −
=χ
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 19)Innovation (continue – 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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
Derivation of Chi and Chi-square Distributions (continue – 2)
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
SOLO Review of Probability
Derivation of Chi and Chi-square Distributions (continue – 3)
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
SOLO Review of Probability
Derivation of Chi and Chi-square Distributions (continue – 4)
Note the skewedness of the chi-square distribution: the above two-sided regions arenot symmetric about the corresponding means
nE n =2χ
Tail probabilities of the chi-square and normal densities.
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 19)Innovation (continue – 2)
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
Recursive Bayesian EstimationSOLO
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
Recursive Bayesian EstimationSOLO
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 22)Evaluation of Kalman Filter Consistency (continue – 2)
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 ε
Tail probabilities of the chi-square and normal densities.
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 23)Evaluation of Kalman Filter Consistency (continue – 3)
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 24)Evaluation of Kalman Filter Consistency (continue – 4)
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 25)Evaluation of Kalman Filter Consistency (continue – 5)
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
χ
χχε
Tail probabilities of the chi-square and normal densities.
.0 975 .0 025
74130
must have a chi-square distribution with N nx degrees of freedom.
xN ε
77
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 26)Evaluation of Kalman Filter Consistency (continue – 6)
Monte-Carlo Simulation Based Tests (continue – 2)
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
χ
χχε
Tail probabilities of the chi-square and normal densities.
.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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 27)Evaluation of Kalman Filter Consistency (continue – 7)
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
Monte-Carlo Simulation Based Tests (continue – 3)
79
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 28)Evaluation of Kalman Filter Consistency (continue – 8)
Examples Bar-Shalom, Y, Li, X-R, “Estimation and Tracking: Principles, Techniques and Software”, Artech House, 1993, pg.242
Monte-Carlo Simulation Based Tests (continue – 4)
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 29)Evaluation of Kalman Filter Consistency (continue – 9)
Examples Bar-Shalom, Y, Li, X-R, “Estimation and Tracking: Principles, Techniques and Software”, Artech House, 1993, pg.244
Monte-Carlo Simulation Based Tests (continue – 5)
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 30)Evaluation of Kalman Filter Consistency (continue – 10)
Examples Bar-Shalom, Y, Li, X-R, “Estimation and Tracking: Principles, Techniques and Software”, Artech House, 1993, pg.245
Monte-Carlo Simulation Based Tests (continue – 6)
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
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems (continue – 31)Evaluation of Kalman Filter Consistency (continue – 11)
Examples Bar-Shalom, Y, Li, X-R, “Estimation and Tracking: Principles, Techniques and Software”, Artech House, 1993, pg.246
Monte-Carlo Simulation Based Tests (continue – 7)
Example Mismatched Filter (continue -1)
A Mismatched Filter is tested: Real System Process Noise q = 9 Filter Model Process Noise qF=1
( ) ( ) ( ) qkxkkx +−Φ= 1
(3) A N=50 runs Monte-Carlo with the 95% probability region
(4) A N=50 runs Monte-Carlo with the 95% probability region
( ) ( ) ( ) ( )∑=
−=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
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
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
( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
( ) ( ) ( ) ( ) ( ) ( ) lkT
www kQlekeEkwEkwke ,
0
&: δ=−=
( ) ( ) lklekeE Tvw ,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)
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 - )
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
( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
( ) ( ) ( ) ( ) ( ) ( ) lkT
www kQlekeEkwEkwke ,
0
&: δ=−=
( ) ( ) lklekeE Tvw ,0 ∀=
=≠
=lk
lklk 1
0,δ
The Unscent Algorithm using ( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
Determines ( ) ( ) ( ) ( ) ( ) ( )kPkekeEkzEkzke zT
zzz =−= &:
88
Unscented Kalman FilterSOLO
( ) ( )[ ]
( )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
Unscented Kalman FilterSOLO
Propagating Means and Covariances Through Nonlinear Transformations
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
Unscented Kalman FilterSOLO
Propagating Means and Covariances Through Nonlinear Transformations
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
Unscented Kalman FilterSOLO
Propagating Means and Covariances Through Nonlinear Transformations
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
Unscented Kalman FilterSOLO
Propagating Means and Covariances Through Nonlinear Transformations
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
Propagating Means and Covariances Through Nonlinear Transformations
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
Unscented Kalman FilterSOLO
Propagating Means and Covariances Through Nonlinear Transformations
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
Uscented Kalman FilterSOLO
96
Uscented Kalman FilterSOLO
( ) ( )∑∑ −−==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)
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
Simon J. Julier Jeffrey K. Uhlman
100
Estimators
( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( ) ( )kkvkkv
kvkxkHkz
kwkkukGkxkkx
ξ+Ψ=++=
Γ++Φ=+
1
1
SOLO
Kalman Filter Discrete Case & Colored Measurement Noise
Assume a discrete dynamic system
( ) ( ) ( ) ( ) ( ) ( ) lkT
www kQlekeEkwEkwke ,
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
( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )111211*1
1
++++++++=+Γ++Φ=+
kkukGkHkxkHk
kwkkukGkxkkx
εζ
SOLO
Kalman Filter Discrete Case & Colored Measurement Noise
The new discrete dynamic system:
( ) ( ) ( ) ( ) ( ) ( ) lkT
www kQlekeEkwEkwke ,
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
Kalman Filter Discrete Case & Colored Measurement Noise
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
Kalman Filter Discrete Case & Colored Measurement Noise
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
kukGkHkkxkHkkDkukGkkxkkkx
==+−−++Φ=+ ζ
( ) ( ) ( ) ( ) ( )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
kukGkHkkxkHkkDkukGkkxkkkx
==+−−++Φ=+ ζ
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)
The Autocorrelation of the error is:
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
Optimal State Estimation in Linear Stationary Systems (continue – 3)
The Autocorrelation of the error is:
Using the Bilateral Laplace Transform we finally obtained:
where
( ) ( ) ( )( ) ( )
( ) ( ) ( ) ( ) ( )
( ) ( ) ( )( )
( ) ( ) ( )ns
rrrrrrrr
rrrrrrrrrr
rrrr
rrrr
,
expexp
expexpexp
=
=−=−=
−==−−=−=
∫∫
∫∫∫∞+
∞−
=∞+
∞−
+ ∞
∞−
−=+ ∞
∞−
−=+ ∞
∞− r
sSdsRdsRsS
sSdsRdsRdsRsS
RRTT
RR
T
ττττττ
υυυττττττ
τ
τυττ
109
EstimatorsSOLO
Optimal State Estimation in Linear Stationary Systems (continue – 4)
( )( ) ( )
( )( ) ( )
( )( )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
Optimal State Estimation in Linear Stationary Systems (continue – 5)
( ) ( ) ( ) ( )[ ] ( )
( ) ( )[ ] ( ) ( ) ( ) ( ) 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
Optimal State Estimation in Linear Stationary Systems (continue – 6)
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
Optimal State Estimation in Linear Stationary Systems (continue – 7)
Example 8.5-4 Sage, “Optimum System Control”, Prentice Hall, 1968, pp.211-213
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
Optimal State Estimation in Linear Stationary Systems (continue – 8)
Example 8.5-4 Sage, “Optimum System Control”, Prentice Hall, 1968, pp.211-213
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
Optimal State Estimation in Linear Stationary Systems (continue – 9)
Example 8.5-4 Sage, “Optimum System Control”, Prentice Hall, 1968, pp.211-213
Solution (continue - 2):
( ) ( ) ( )( ) ( ) ( )
( ) ( ) ( ) ( ) 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
Optimal State Estimation in Linear Stationary Systems (continue – 10)
Example 8.5-4 Sage, “Optimum System Control”, Prentice Hall, 1968, pp.211-213
Solution (continue - 3):
( ) ( ) ( )( ) ( ) ( )
( ) ( ) ( ) ( ) 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
Optimal State Estimation in Linear Stationary Systems (continue – 11)
Example 8.5-4 Sage, “Optimum System Control”, Prentice Hall, 1968, pp.211-213
Solution (continue - 4):
( ) ( ) ( )( ) ( ) ( )
( ) ( ) ( ) ( ) 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
Kalman Filter Continuous Time Case (continue – 2)
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )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
Kalman Filter Continuous Time Case (continue – 3)
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 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
Norbert Wiener1894 - 1964
Eberhard Frederich Ferdinand Hopf
1902 - 1983
( ) ( ) ( ) ( ) ( ) λτλττλ <<= ∫ ttdzzEtAztxEt
t
TT0
0
,ˆ
121
EstimatorsSOLO
Kalman Filter Continuous Time Case (continue – 4)
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
Kalman Filter Continuous Time Case (continue – 5)
Solution of Wiener-Hopf Equation(continue – 1)
( ) ( ) ( ) ( ) ( ) λτλττλ <<= ∫ ttdzzEtAztxEt
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
Kalman Filter Continuous Time Case (continue – 6)
Solution of Wiener-Hopf Equation(continue – 2)
( ) ( ) ( ) ( ) ( ) λτλττλ <<= ∫ ttdzzEtAztxEt
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
Multi-sensor Estimate (continue – 2)
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
Multi-sensor Estimate (continue – 3)
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
Multi-sensor Estimate (continue – 4)
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
SOLO RADAR Range-Doppler
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
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 2)
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
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 3)
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
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 4)
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
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 5)
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
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 6)
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/δ
=ΓΓ
Guideline for Choice of Process Noise Intensity
For this model q should be of the order of maximum acceleration magnitude aM.
A practical range is 0.5 aM ≤ q ≤ aM.
141
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 7)
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
SOLO RADAR Range-Doppler
Target Acceleration Models (continue – 8)
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/
δ
=ΓΓ
Guideline for Choice of Process Noise Intensity
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
Target Acceleration Models (continue – 9)
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
Target Acceleration Models (continue – 10)
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
Target Acceleration Models (continue – 11)
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
Target Acceleration Models (continue – 13)
148
SOLO
Constant Speed Turning Model
RADAR Range-Doppler
Target Acceleration Models (continue – 14)
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
Target Acceleration Models (continue – 15)
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
Target Acceleration Models (continue – 16)
( ) ( ) ( ) ( )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
Target Acceleration Models (continue – 17)
( ) ( )[ ]( ) ( )
( ) ( )
+−=
+=
−++=−
−−
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
Target Acceleration Models (continue – 18)
( )( ) ( )[ ]
( ) ( )( ) ( )
−
−=Φ −
−−
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
Target Acceleration Models (continue – 19)
( ) ( ) 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
Target Acceleration Models (continue – 20)
( ) 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
Fixed Wing Air Vehicle Acceleration Model (continue – 2)
RADAR Range-Doppler
Target Acceleration Models (continue – 21)
( )[ ]( ) 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/
: ττττ
τττ
Fixed Wing Air Vehicle Acceleration Model (continue – 3)
157
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 23)
( )
( ) ( )
( )
( )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
Fixed Wing Air Vehicle Acceleration Model (continue – 4)
( ) ( ) ( )
−
=−
l
pl
f
CAEA W
TWL
LL
( ) ( ) ( ) ( ) ( ) WL
l
p
f
TWL
TLLLL ClCAEAAEAE
W
=
−
−
2
22
2
00
00
00
σ
σ
σ
158
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 24)
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
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 25)
Fixed Wing Air Vehicle Acceleration Model (continue – 6)
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
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 26)
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
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 27)
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
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 28)
Ballistic Missile Acceleration Model (continue – 2)
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
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 29)
Ballistic Missile Acceleration Model (continue – 3)
( )( )
( ) ( ) 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
SOLO RADAR Range-DopplerTarget Acceleration Models (continue – 30)
Ballistic Missile Acceleration Model (continue – 4)
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)
Ballistic Missile Acceleration Model (continue – 5)
MV
Bx
By
BzWz
Wy
Wx
αβ
αβ
Bp
Wp
Bq
WqBrWr
166
SOLOTarget Acceleration Models (continue – 32)
Ballistic Missile Acceleration Model (continue – 6)
MV
Bx
By
BzWz
Wy
Wx
αβ
αβ
Bp
Wp
Bq
WqBrWr
167
SOLOTarget Acceleration Models (continue – 33)
Ballistic Missile Acceleration Model (continue – 7)
MV
Bx
By
BzWz
Wy
Wx
αβ
αβ
Bp
Wp
Bq
WqBrWr
168
SOLOTarget Acceleration Models (continue – 34)
Ballistic Missile Acceleration Model (continue – 8)
MV
Bx
By
BzWz
Wy
Wx
αβ
αβ
Bp
Wp
Bq
WqBrWr
169
SOLOTarget Acceleration Models (continue – 35)
Ballistic Missile Acceleration Model (continue – 9)
MV
Bx
By
BzWz
Wy
Wx
αβ
αβ
Bp
Wp
Bq
WqBrWr
170
SOLOTarget Acceleration Models (continue – 36)
Ballistic Missile Acceleration Model (continue – 10)
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
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
EstimatorsSOLOKalman Filter for Filtering Position and Velocity Measurements (continue – 2)
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
EstimatorsSOLOKalman Filter for Filtering Position and Velocity Measurements (continue – 2)
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
EstimatorsSOLOKalman Filter for Filtering Position and Velocity Measurements (continue – 3)
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
σ
σ
σ
=
−=
+−=
α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 2)
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
α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 3)
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σ
α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 4)
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 ββ
βββλ
−=
+−=
α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 5)
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 σα
βαβ−
−=
&
α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 6)
183
Estimators
( )( )λλλλλα 8488
1 22 ++−+−=
SOLO
We found
( ) ( )λλλλλλλλλβ 844
844
1 222 +−+=+−+=
α, β gains, as function of λ in semi-log and log-log scales
α - β (2-D) Filter with Piecewise Constant White Noise Acceleration Model (continue – 7)
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
Guideline for Choice of Process Noise Intensity
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
:=
α – β - γ (3-D) Filter with Piecewise Constant Wiener Process Acceleration Model (continue – 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:
α – β - γ (3-D) Filter with Piecewise Constant Wiener Process Acceleration Model (continue – 3)
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
SOLO EstimatorsContinuous Filter-Smoother Algorithms
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
SOLO EstimatorsContinuous Filter-Smoother Algorithms
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
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 2) :
( )( )
( )( )
+
−−−
=
−− 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
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 +=
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
SOLO EstimatorsContinuous Filter-Smoother Algorithms
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 −=
zRHFxHRH TTT 11 −− +−−= λλ
( ) ( )[ ]
( ) ( )[ ]( ) ( ) ( ) ( )txtStt
Sxtxx
Jt
Sxtxx
Jt
FF
fT
ff
t
fT
T
t
T
f
−=⇒
−=∂∂=
−−=∂∂−=
λλλ
λ 0000
0
Second Way, Assumption 2:
Forward
200
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 1) :
Differentiate and use previous equations
( )( )
( )( )
+
−−−
=
−− 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
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 1) :
( )( )
( )( )
+
−−−
=
−− zRH
wG
t
tx
FHRH
GQGF
t
txTTT
T
11 λλ
( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )[ ] ( )txHRHtSGQGtStSFFtStS
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
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 +=
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
where = Estimate of w(t)( ) ( ) ( )tGQtwtw Tλ−=
= Smoothed Estimate of x(t)( ) ( ) ( )tPtxtx FF λ−=
203
SOLO EstimatorsContinuous Filter-Smoother Algorithms
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 −=
zRHFxHRH TTT 11 −− +−−= λλ
( )[ ] ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )ttPtxtx
tPtSxtx
tPtSxtxBB
ffffff
λλλ
λλ+=⇒
==−
==−−−
−
1
0001
000
Third Way, Assumption 3:
Backward
204
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 1) :
Differentiate and use previous equations
( )( )
( )( )
+
−−−
=
−− 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
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 1) :
( )( )
( )( )
+
−−−
=
−− 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
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 +=
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 −+−=−++−= −− λλλ
where = Estimate of w(t)( ) ( ) ( )tGQtwtw Tλ−=
= Smoothed Estimate of x(t)( ) ( ) ( )tPtxtx FF λ−=
207
SOLO EstimatorsContinuous Filter-Smoother Algorithms
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 −=
zRHFxHRH TTT 11 −− +−−= λλ
( ) ( )[ ]
( ) ( )[ ]( ) ( ) ( ) ( )txtStt
Sxtxx
Jt
Sxtxx
Jt
BB
fT
ff
t
fT
T
t
T
f
+=⇒
−=∂∂=
−−=∂∂−=
λλλ
λ 0000
0
Fourth Way, Assumption 4:
Backward
208
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 1) :
Differentiate and use previous equations
( )( )
( )( )
+
−−−
=
−− zRH
wG
t
tx
FHRH
GQGF
t
txTTT
T
11 λλ
( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )[ ] ( )
( ) ( ) ( ) ( )[ ] ( )tzRHtxtStFtxHRH
twGtxtStGQGtxFtStxtSt
txtStxtStt
TBB
TT
BBT
BBB
BBB
11 −− ++−−=
++−⋅++=
++=
λλλ
λλ
( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )[ ] ( )txHRHtSGQGtStSFFtStS
twGtStzRHtGQGtStFtT
BT
BBT
BB
BT
BT
BBT
B
1
1
−
−
−+−−−=
+−−+
λλλ
( ) ( ) ( ) ( )txtStt BB +=λλFourth Way, Assumption 4
( ) ( )[ ]( ) ( )[ ]
−=
−−=
fT
fffT
TT
Sxtxt
Sxtxt
λλ 0000
or
209
SOLO EstimatorsContinuous Filter-Smoother Algorithms
Solution to the Problem (continue – 1) :
( )( )
( )( )
+
−−−
=
−− 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
+−+=⇒+−=
−+−=
++−=
− λλ
λλ
( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )[ ] ( )txHRHtSGQGtStSFFtStS
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
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 +=
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 +−+=−+−= − λλ
where = Estimate of w(t)( ) ( ) ( )tGQtwtw Tλ−=
= Smoothed Estimate of x(t)( ) ( ) ( )tPtxtx FF λ−=
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:bryson@sun-valley.stanford.edu
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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
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:ˆ
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ˆ
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 2)
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
mxEk
mEk
iik == ∑
=1
1ˆ
( ) 2
1
22 ˆ1
1:ˆ σσ =
−
−= ∑
=
k
ikik mx
kEE
223
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 3)
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 4)
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 4)
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 5)
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 6)
[ ] ϕσσσ σσ =≤≤ 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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 7)
[ ] ϕσσσ σσ =≤≤ 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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 8)
230
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 9)
231
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 10)
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 12)
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 π
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 13)
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
SOLO Review of ProbabilityEstimation of the Mean and Variance of a Random Variable (continue - 14)
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:
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
( )
+−−
++= ++ 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)
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
( )
+−−
++= ++ 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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
Types of Estimation
t t+τ
t
available measurement data
t
available measurement data
available measurement data
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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
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
SOLO Review of Probability
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
Review of Probability
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:
Review of Probability
250
Recursive Bayesian EstimationSOLO
Linear Gaussian Markov Systems
kkkk
kkkkkkk
vxHz
wuGxx
+=Γ++Φ= −−−−−− 111111
wk-1 and vk, white noises, zero mean, Gaussian, independent
( ) ( ) ( ) ( ) ( ) ( )kPkekeEkxEkxke xT
xxx =−= &:
( ) ( ) ( ) ( ) ( ) ( ) lkT
www kQlekeEkwEkwke ,
0
&: δ=−=
( ) ( ) ( ) ( ) ( ) ( ) lkT
vvv kRlekeEkvEkvke ,
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
Recursive Bayesian EstimationSOLO
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
Random VariablesSOLO
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
Random VariablesSOLO
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
Random VariablesSOLO
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
Random VariablesSOLO
Ergodicity (continue - 2):
We obtained the Wiener-Khinchine Theorem (Wiener 1930):
( ) ( ) ( )∫+∞
∞−→∞−=
= dtjR
T
XXES TT
T
τωτω exp2
:*
lim
Norbert Wiener1894 - 1964
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
Random VariablesSOLO
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
Random VariablesSOLO
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
Random VariablesSOLO
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
Random VariablesSOLO
Table of Content
Markov Processes
Examples of 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
Random VariablesSOLO
Table of Content
Markov Processes
Examples of 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
Random VariablesSOLO
Table of Content
Markov Processes
Examples of 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
Random VariablesSOLO
Markov Processes
Examples of 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
Examples of 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
Random VariablesSOLO Markov Processes
Examples of Markov Processes:
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
Random VariablesSOLO Markov Processes
Examples of Markov Processes:
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
Random VariablesSOLO Markov Processes
Examples of Markov Processes:
6. Discrete Linear Dynamic System with Variable Coefficients (continue – 1)
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ∑−+
=
Γ++Φ++Φ=+1
1,,lk
kn
Txw
Txx
Txx keneEnnlkkekeEklkkelkeE
( ) ( ) ( ) ( ) ( ) ( )∑−+
=
Γ++Φ++Φ=+1
1,,lk
knwxx nennlkkeklklke
( ) ( ) ( ) ( ) ( ) ( )∑−
−=
Γ+Φ+−−Φ=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
Random VariablesSOLO Markov Processes
Examples of Markov Processes:
6. Discrete Linear Dynamic System with Variable Coefficients (continue – 2)
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )∑−+
=
++ΦΓ++Φ=+1
1,,lk
kn
TTTwx
TTxx
Txx nlknnekeEklkkekeElkekeE
( ) ( ) ( ) ( ) ( ) ( )∑−+
=
Γ++Φ++Φ=+1
1,,lk
knwxx nennlkkeklklke
( ) ( ) ( ) ( ) ( ) ( )∑−
−=
Γ+Φ+−−Φ=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
SOLO Matrices Trace of a Square Matrix The trace of a square matrix is defined as ( ) ( )T
nn
n
iiinn AtraceaAtrace ×
=× == ∑
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
SOLO Matrices Trace of a Square Matrix The trace of a square matrix is defined as ( ) ( )T
nn
n
iiinn AtraceaAtrace ×
=× == ∑
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
SOLO Matrices Trace of a Square Matrix The trace of a square matrix is defined as ( ) ( )T
nn
n
iiinn AtraceaAtrace ×
=× == ∑
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
SOLO Matrices Trace of a Square Matrix The trace of a square matrix is defined as ( ) ( )T
nn
n
iiinn AtraceaAtrace ×
=× == ∑
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
SOLO Matrices Trace of a Square Matrix The trace of a square matrix is defined as ( ) ( )T
nn
n
iiinn AtraceaAtrace ×
=× == ∑
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
Signal Duration and Bandwidth (continue – 1)
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
Signal Duration and Bandwidth (continue – 2)
( )
( )
( )
( )
( )
( )
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
Recommended