2Campus Bio-medico di Roma. 18th World Congress of the International Federation of. Automatic Control, 2011. Conte, Cusimano, Germani. Virtual ...
Problem Formulation The new filter Evaluation
Optimal polynomial filtering for planar tracking via virtual measurement process F. Conte1
V. Cusimano2
1 Dipartimento
A. Germani1
di Ingegneria Elettrica e dell’Informazione Università dell’Aquila
2 Campus
Bio-medico di Roma
18th World Congress of the International Federation of Automatic Control, 2011
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Outline
1
Problem Formulation The Kinematic model
2
The new filter The Virtual Measurement Map Polynomial extension
3
Evaluation Comparison with the EKF Conclusions and future work
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Outline
1
Problem Formulation The Kinematic model
2
The new filter The Virtual Measurement Map Polynomial extension
3
Evaluation Comparison with the EKF Conclusions and future work
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Assumptions for planar moving object
We consider a kinematics model in which: The state is given by the position, velocity and acceleration in the two dimension plane: x = [x1 , x2 , x˙ 1 , x˙ 2 , x¨1 , x¨2 ]T It is assumed the acceleration dynamics forced by a Gaussian white term (Jerk, J ) with variance σS2
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
System equation Continuous system ˙ x(t) = Ac x(t) + BJ (t)
(1)
x(k + 1) = Ax(k ) + f (k )
(2)
Discrete system
with Z A=
∆
e
Ac η
Z dη ; fk =
0
∆
eAc η dηBJ (t)
0
where initial condition x(0) is assumed to be a zero mean Gaussian vector J (t) is constant in the time interval [k ∆, (k + 1) ∆] Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Measurements process At each time instant k , the measurements are given by the noisy values of the radius ρm (k ) and the angle position θm (k ) of the target. q ρm (k ) = ρ(k ) + nρ (k ) = x12 (k ) + x22 (k ) + nρ (k ) −1 x2 (k ) θm (k ) = θ(k ) + nθ (k ) = tan + nθ (k ) x1 (k ) where nρ (k ) and nθ (k ) denote the measurement errors. assumed to be independent zero mean white sequence with σρ2 and σθ2 variances
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Some problem
Note that: Since the measurement process is nonlinear, the state estimation requires a nonlinear system algorithm, that as well known, is, in general, an infinite dimensional problem. Only suboptimal algorithms can be used for engineering applications, for example the EKF, UKF and others.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Some problem
Note that: Since the measurement process is nonlinear, the state estimation requires a nonlinear system algorithm, that as well known, is, in general, an infinite dimensional problem. Only suboptimal algorithms can be used for engineering applications, for example the EKF, UKF and others.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Rationale The use of a suitable transformation of the measurements allows to obtain a linear output model. The main results are that: 1 The nonlinearity of the measure map is transferred into a modification of the noise sequence distribution in a nongaussian white sequence. Note that, this result is the property required for Kalman filtering which, although non more optimal, remains to be the optimal linear filtering algorithm. 2 Measurements process is in a form amenable for polynomial filtering without the need of the measure map linearization.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Kinematic model
Rationale The use of a suitable transformation of the measurements allows to obtain a linear output model. The main results are that: 1 The nonlinearity of the measure map is transferred into a modification of the noise sequence distribution in a nongaussian white sequence. Note that, this result is the property required for Kalman filtering which, although non more optimal, remains to be the optimal linear filtering algorithm. 2 Measurements process is in a form amenable for polynomial filtering without the need of the measure map linearization.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Outline
1
Problem Formulation The Kinematic model
2
The new filter The Virtual Measurement Map Polynomial extension
3
Evaluation Comparison with the EKF Conclusions and future work
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Radius position value Taking account that x1 (k )cos(θ(k )) = ρ(k ) cos2 (θ(k )) x2 (k )sin(θ(k )) = ρ(k ) sin2 (θ(k ))
(3)
the equation of radius position can be written as ρm (k ) − nρ (k ) = ρ(k ) = =cos(θm (k ) − nθ (k ))x1 (k ) + sin(θm (k ) − nθ (k ))x2 (k )
(4)
=C1 (θm (k ), nθ (k ))x(k ), where C1 (θ, n) =
cos(θm − nθ ) sin(θm − nθ ) 0 0 0 0
Conte, Cusimano, Germani
Virtual measurement process
.
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Angle position value From equation of θm (k ), it follows: θ(k ) = θm (k ) − nθ (k ),
x2 (k ) sin(θ(k )) = , cos(θ(k )) x1 (k )
(5)
from which 0 =cos(θm (k ) − nθ (k ))x2 (k ) − sin(θm (k ) − nθ (k ))x1 (k )
(6)
=C2 (θm (k ), nθ (k ))x(k ), where C2 (θ, n) =
−sin(θm − nθ ) cos(θm − nθ ) 0 0 0 0
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Virtual output Equations (4) and (6) realize in a linear form the nonlinear output measurements in a nongaussian setting. The virtual output is the sequence ρm (k ) yv (k ) = = C(θm (k ), nθ (k ))x(k ) + Gnρ (k ) 0
(7)
where C(θm (k ), nθ (k )) =
C1 (θm (k ), nθ (k )) C2 (θm (k ), nθ (k ))
Conte, Cusimano, Germani
,G =
Virtual measurement process
1 0
.
(8)
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Remark
Note that the idea of virtual output measurement map allow to model in a simple way the present of constraints on the state evolution that can be very usefull in control application.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Outline
1
Problem Formulation The Kinematic model
2
The new filter The Virtual Measurement Map Polynomial extension
3
Evaluation Comparison with the EKF Conclusions and future work
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Kronecker powers
The measurement map obtained is nongaussian and a polynomial filtering approach can be used. In particular, such a nonlinear estimate can be obtained through a filtering process computed on a system whose output vector carries the Kronecker powers of the original output vector up to a certain order ν.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Extended virtual output The extended virtual output vector is defined as (1) Yv (k ) (2) Yv (k ) ∈ Rq , Yv (k ) = .. .
(9)
(ν)
Yv (k ) where ν is the order of the filter, q = 2 + 22 + . . . + 2ν is the extended output vector dimension and [i] [i] (i) (i) Yv (k ) = R (θm (k ))T yv (k ) − R(θm (k ))T G[i] ψnρ .
Conte, Cusimano, Germani
Virtual measurement process
(10)
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Extended state The extended state vector X (k ) is defined as x(k ) x [2] (k ) X (k ) = ∈ RN , .. .
(11)
x [ν] (k ) The corresponding state equation has the form X (k + 1) = A(k )X (k ) + U(k ) + F(k ) where U(k ) is a deterministic term F(k ) is a zero-mean multiplicative state noise white sequence Conte, Cusimano, Germani
Virtual measurement process
(12)
Problem Formulation The new filter Evaluation
The Virtual Measurement Map Polynomial extension
Extended system The extended system is X (k + 1) = A(k )X (k ) + U(k ) + F(k ) Yv (k ) = C(θm (k ))X (k ) + N (k ),
(13)
where C(θm (k )) is the output matrix {N (k )} is a zero-mean white noise sequence The system modelled by (13) satisfies the requirements for applying the Kalman filtering. Therefore a ν-order polynomial estimate of the extended state vector can be obtained.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Outline
1
Problem Formulation The Kinematic model
2
The new filter The Virtual Measurement Map Polynomial extension
3
Evaluation Comparison with the EKF Conclusions and future work
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Assumption The observer is located at the axis origin The target moves for 100 s at a nearly constant velocity in the 2-D Target initial state: x(0) =
12Km 8Km −120m/s 15m/s 0m/s2 0m/s2
The output noise covariance matrix is 2 σρ 0 σρ = 0.35 m Q(k ) = , 2 σθ = je − 3 rad, j = 1, 2, · · · , 30 0 σθ Initial state for the filter is calculated from the first measurements. Conte, Cusimano, Germani
Virtual measurement process
T
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Relative position error The standard relative position error (RPEi (k )) is defined for each sample measurement noise realization i at time k as q ˆ1(i) (k ))2 + (e ˆ2(i) (k ))2 (e · 100%, RPEi (k ) = q (i) (i) 2 2 (x1 (k )) + (x2 (k )) ˆl(i) (k ) = xl(i) (k ) − xˆl(i) (k ), l = 1, 2, xˆl(i) (k ) being the with e estimated state vector. A run is considered to be convergent only if the RPEi (k ) < 15% at the end of simulation time.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Simulation results
VOKF EKF Runs
1e-3 0.075 0.075 500
5e-3 0.261 0.343 500
σθ 1e-2 0.452 3.492 397
2e-2 0.728 22.509 136
3e-2 1.127 47.604 81
Table: RPE numerical results
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Typical trajectory 8.5
8.5
8.0
8.0 7.5
7.5
Ground truth EKF VOKF
Ground truth EKF VOKF
7.0 6.5
Km
Km
7.0
6.5
6.0 6.0 5.5 5.5
5.0
5.0
4.5 7.0
4.5
8.0
9.0
10.0
11.0
12.0
13.0
14.0
15.0
Km
(a)
4.0 7.0
8.0
9.0
10.0
11.0
12.0
Km
(b)
Figure: Estimation result for a typical trajectory in CCS with: σθ = 6e − 3 rad (a) and σθ = 1e − 2 rad (b)
Conte, Cusimano, Germani
Virtual measurement process
13.0
14.0
15.0
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Outline
1
Problem Formulation The Kinematic model
2
The new filter The Virtual Measurement Map Polynomial extension
3
Evaluation Comparison with the EKF Conclusions and future work
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Summary
New idea regarding the post-processing of the measurement data in order to transform the nonlinear measurement map in a linear one at expensive of the loss of the original Gaussianity of the measurement noises. The use of the polynomial Kalman filters could significantly improve the performance of the standard Kalman filter by taking into account moments of higher order of the transformed noises.
Conte, Cusimano, Germani
Virtual measurement process
Problem Formulation The new filter Evaluation
Comparison with the EKF Conclusions and future work
Thanks
Thank you for your attention
Conte, Cusimano, Germani
Virtual measurement process
