Modelling 3D rigid-body object motion and structure estimation with HRR/GMTI measurements. S. Wu and L. Hong. Abstract: When traditional single-point target ...
Modelling 3D rigid-body object motion and structure estimation with HRR/GMTI measurements S. Wu and L. Hong Abstract: When traditional single-point target tracking algorithms are directly applied to multiplepoint rigid targets, filter divergence and data association problems occur. By introducing the concepts of global motion, local motion and structure parameters, and employing motion-induced invariants on local motion and structure parameters, we have developed several kinematic and measurement models to simultaneously estimate the motion and structure information of a 3D rigid-body target from high range resolution (HRR) and ground moving target indicator (GMTI) measurements. To test the ability of the proposed modelling methods in handling false alarms, occlusions and HRR measurements without correspondence information, several data association approaches are also proposed, that is permutation, combination-permutation, dummy measurements, and dynamically changing the interested number of scattering centres. Simulation results have shown that the proposed modelling methods can successfully estimate both the motion and structure information of an object by using HRR and GMTI measurements with a proper data association method.
1
Introduction
We have recently proposed two novel approaches for 2D rigid body target modelling, motion and structure parameters estimation by using ground moving target indicator (GMTI) and high resolution range (HRR) sensors [1]. These approaches effectively explore the concepts of local and global motions of a rigid body. The kinematics of the global motion is described by a constant acceleration model, and the local motion is modelled by the pivoting centre or pseudo centre based approaches. By using an extended Kalman filter (EKF) with a probabilistic data association (PDA) method, the proposed approaches not only correctly track a rigid body target in a complicated scenario but also simultaneously report its identification/ structure information. However, the target is simplified as a 2D rigid-body and its motion is also restricted in a 2D plane. In this paper, we extend the related work to 3D space. Conceptually, in a 3D scenario, we still need to do motion decomposition, using rigid constraints in local motion kinematic modelling, and jointly estimate motion and structure parameters. However, due to the complexity of 3D motion and a large number of degrees of freedom, joint motion and structure estimation architecture does not work well. To overcome this drawback, we propose a new algorithm simultaneously estimating the motion and structure information of a 3D rigid-body object by using GMTI and multiscan HRR measurements. The algorithm compiles a motion filter and a structure filter together based on the concept of # The Institution of Engineering and Technology 2007 doi:10.1049/iet-cta:20060034 Paper first received 23rd January and in revised form 26th September 2006 The authors are with the Department of Electrical Engineering, Wright State University, Dayton, OH 45435, USA S. Wu is now with Sarnoff Corporation, 201 Washington Road, Princeton, NJ 08536, USA E-mail: swu@sarnoff.com IET Control Theory Appl., 2007, 1, (4), pp. 1023 –1032
motion decomposition: a motion filter is proposed to estimate global and local motion parameters by using one scan GMTI and HRR measurements; a structure filter is used to estimate structure information by using multiscan HRR measurements. Further, several measurement data association techniques are also proposed to solve the problems such as HRR measurements without correspondence information, occlusion and false alarms. The data association techniques include permutation, combinationpermutation, dummy measurements, and dynamically changing the interested number of scattering centres (SCs). Simulation results have shown that the proposed algorithms can successfully estimate both the motion and structure information of a 3D object by using HRR and GMTI measurements with a proper data association method. Although various kinematical modelling [2], measurement modelling [3], and measurement data association [4] methods have been developed for tracking a point target in the past three decades, there is little literature dealing with the simultaneous tracking and ID problem. Stuff et al. [5, 6] initiated an attempt on applying invariant constraints to tracking and identifying moving targets. Jacobs and O’Sullivan [7, 8] introduced a joint tracking and recognition technique using an HRR model and a likelihood-based approach. Miller et al. [9] proposed a method for generating the conditional mean estimation of target positions, orientation and types in recognition, and the tracking of an unknown number of targets by using the measurements from both narrowband sensor array (for providing azimuth and elevation angles) and optical or radar imagers (for obtaining the target type and orientation). The jump-diffusion technique was adopted by Sworder and Boyd [10] in their tracking and recognition work. Blasch and Hong developed a belief function based algorithm for joint target tracking and ID using GMTI and HRR measurements [11, 12]. Dezert [13] proposed a 2-step approach to track an extended target in a cluttered environment. The first step is to find the best match between the set of 1023
measurements and the predicted pattern, while the second stage is to generate an estimation based on the assumption that the selected association is correct. By doing so, although the complexity of managing multiple hypotheses data association problem is avoided, it takes the risk by updating the filter with a wrong match, particularly in stressing scenarios where the measurement-object association is highly uncertain. To overcome this shortcoming, Salmond and Gordon [14] presented a Bayesian multiple hypothesis solution by using the particle filter technique in which only the feasible association hypotheses are considered to update the filter. More recently, Pulford and Salmond [15] defined a near-far object tracking problem and presented a Gaussian mixture multi-hypothesis filter for tracking a 1D object with known number of scattering sources moving in 3D. Vermaak et al. [16] proposed a sequential Monte Carlo framework for tracking an extended object in which they augment the state with the unknown association hypothesis, and sample candidate values from an efficiently designed proposal distribution. Moreover, our research team has developed a few methods on 2D rigid-body target tracking by applying invariant constraints in recently. Gu and Hong [17] proposed an algorithm to track 2D rigid body targets with invariant constraints. Hong et al. [18] proposed an invariant-based interacting multiple template algorithm for simultaneous tracking and ID using GMTI/HRR. Wu et al. [1] modelled the kinematics and measurements of a 2D rigid object in two approaches, by feeding GMTI and HRR measurements into an EKF with the PDA algorithm, the motion and structure parameters of an object can be simultaneously estimated. In all the works of [1, 17, 18], a target is simplified as a 2D rigid-body and its motion is also restricted in a 2D space. This work attempts to estimate the motion and structure information of a 3D object moving in 3D space. 2
Problem statement
½ px0 ; py0 ; pz0 T ¼ A½ pu ; pv ; pw T
As shown in Fig. 1, a 3D rigid object with M SCs, denoted as pi ði ¼ 1; . . . ; MÞ, is moving along a curve in 3D space, while it is rotating along the rotation axis ab. Assume (i) the number of SCs M is fixed, (ii) a GMTI sensor can measure the centre range (dck), range-rate (d_ ck ), azimuth (hck) and elevation ðeck Þ angles, and (iii) an HRR sensor can provide range and range-rate profiles ðdik ; d_ ik i ¼ 1; . . . ; MÞ from sensor to each SC at time tk .
HR
G R/
M
y radar wave front plane
TI
o
zc
xc
hc
ec
x
z
e1 e3 ð1 Ca Þ þ e2 Sa
a
ω
y'
w
pi
o'
yc
x'
z' b
v u
Fig. 1 Sketch of a 3D motion and structure estimation problem, the reference systems, and sensor-target geometry
ð1Þ
where the AT is called the direction cosine matrix, which can have several representations such as the Euler axis/ angle, Euler angles, Gibbs vector, and Euler symmetric parameters (also called quaternions) [19]. The pros and cons of each representation can be found on p. 412 of Wertz [19]. The Euler axis/angle representation is used in this work. Assume the rotation axis, e ¼ fe1 ; e2 ; e3 gT is a unit vector in BFCS and the rotation angle a follows the right-hand rule, the cosine direction matrix of the Euler axis/angle representation is expressed as 2 Ca þ e21 ð1 Ca Þ e1 e2 ð1 Ca Þ e3 Sa 6 Ak ¼ 4 e1 e2 ð1 Ca Þ þ e3 Sa Ca þ e22 ð1 Ca Þ e1 e3 ð1 Ca Þ e2 Sa
dc di
1024
Our tasks are to estimate the geometric structure and motion parameters of the object from the HRR and GMTI measurement sequences. The first assumption is only true for the target with a known number of SCs. However, for the case that the tracker does not know the exact number of SCs, one might assume a conservative number to roughly estimate the structure of the target. In addition, because of occlusion, the number of observed SCs may dynamically change during tracking in the real world. In this case, by using the rigidity constraint and rotation information, we can either use dummy measurements or form synthetically occluded pseudo measurements to update the filter. For the second assumption, instead of measuring centroid range, range-rate, and angle information, the GMTI sensor may obtain some combination of the individual SCs. Nevertheless, as long as the combined centre remains still on/in a target, we can adjust the GMTI measurement noise intensities to handle this. Moreover, since the proposed motion and structure filters are separated (see Section 3 for details), less accurate estimation of motion parameters should not affect the structure estimation much. To model the problem, we first define three orthogonal reference systems. As displayed in Fig. 1, the first one is oxyz, which is the space fixed coordinate system (SFCS) or the inertial coordinate system. The second one is o 0 uvw, which is a target body fixed coordinate system (BFCS), and its origin point, o 0 , locates at the pivoting centre of the target. The third one is o 0 x 0 y 0 z 0 , which is the target local coordinate system (TLCS), and its origin point is also fixed at o 0 but its three axes are always parallel to the three axes of the SFCS. With the definitions of the three reference systems, any motion of a rigid target can be regarded as the translation of the pivoting centre, o 0 , in the SFCS, and the rotation of the target in the TLCS. Further, the relationship between the coordinates of a point in BFCS and TLCS is linked by
e2 e3 ð1 Ca Þ þ e1 Sa 3
7 ð2Þ e2 e3 ð1 Ca Þ e1 Sa 5 2 Ca þ e3 ð1 Ca Þ p with the constraint ðe21 þ e22 þ e23 Þ ¼ 1, where Ca W cos a; Sa W D sin a: Further, we may classify the motion and structure estimation problem from GMTI and HRR measurements as three sub-problems depending on the applications. The first one estimates global and local motions by knowing the structure; the second one estimates the structure with IET Control Theory Appl., Vol. 1, No. 4, July 2007
the prior information of global and local motions, and the third one simultaneously estimates the global, local motions and structure information. The system and measurement modelling details of these approaches are addressed in the next section.
ðmÞ and nðmÞ xk N ð0; Q Þ with
2
" Q
3
ðmÞ
¼
O5;9
System and measurement modelling
Our previous work [1] demonstrates that a rigid body motion is composed of two kinds of motions: one is the motion of the mass centre (or pseudo centre), which is defined as global motion, and another is the rotational motion relative to the pseudo centre which is defined as local motion. Global motion is the same as point object motion used by the tracking community and local motion is a new concept. For example, as shown in Fig. 1, global motion can be described by the location of the centre and its velocity and acceleration, and local motion can be expressed by the direction cosine matrix. Moreover, for identifying an object, we also need to estimate its structural information, which is defined as the position of SCs in the BFCS. Here are some comments on global and local motions: (i) global motion reflects entire object motion and carries no ID information, while local motion is encoded with object structural information – how a group of prominent points or SCs move together; (ii) local motion modulates on global motion; (iii) an independent variable for global motion is time; and (iv) independent variables for local motion may or may not be time, such as aspect angles etc. The difficulty is how to separate these two kinds of motions when given only the composite motion measurements, ranges and range-rates. 3.1
Estimate motion assuming structure is known
The state vector of the motion filter is defined by _ c ; y_ c ; z_c ; x€ c ; y€ c ; z€ c ; e1 ; e2 ; e3 ; a; a˙ Tk ð3Þ xðmÞ k ¼ ½xc ; yc ; zc ; x where ½xc ; yc ; zc k , ½_xc ; y_ c ; z_ c k and ½€xc ; y€ c ; z€ c k are the elements of position, velocity and acceleration of the global centre in the SFCS at time tk , respectively; ½e1 ; e2 ; e3 k is the rotation axis in the BFCS, and [a, a˙]k is the rotation angle and its velocity at tk . By using an uncoupled constant acceleration model to describe the global motion and a fixed axis with constant angular velocity to describe the local motion, the kinematic equation of the motion filter can be written as ðmÞ ðmÞ xðmÞ xk þ nðmÞ xk kþ1 ¼ F
ð4Þ
where " F
ðmÞ
¼
F ðgÞ
O9;5
O5;9
F ðlÞ
# ð5Þ
QðgÞ
3 T4 T3 T2 I I I 6 4 3 2 3 2 37 # 6 3 7 6 7 O9;5 ðgÞ 2 6T 2 ¼ s ; Q I3 T I3 TI3 7 qg 6 7 ðlÞ Q 6 2 7 4 2 5 T I3 TI3 I3 2 ð7Þ
" ðlÞ
Q ¼
s2qe I 3
O3;2
O2;3
s2qa Qcv
2 6 I3 F ðgÞ ¼ 6 4O
3;3
TI3 I3
O3;3 O3;3
3 2 3 T2 I 3 O3;1 O3;1 I3 7 6O 2 7 ðlÞ 1 T 7 5 T I 3 5; F ¼ 4 1;3 1 O1;3 0 I3
3 3
T4 6 4 ; Qcv ¼ 6 4T3
#
T 2 7 7 5 2 T
2
ð8Þ
Further, in (5) to (8) and hereafter, Om,n stands for an m n matrix of zeros; Im is an m m identity matrix; Im,n is an m n matrix of ones; T is the sampling interval in second; sqg , sqe and sqa are the noise intensities of the global motion, axis and angle, respectively. As indicated by Fig. 1, the GMTI sensor can measure the range, range-rate, azimuth and elevation angles of the pivoting centre, while the HRR sensor provides the range and range-rate measurements of each SC. These quantities can be modelled as ðmÞ ðmÞ zðmÞ 1k ¼ h1 ðxk Þ ¼ dck þ nz1
ð9Þ
k
ðmÞ ðmÞ _ zðmÞ 2k ¼ h2 ðxk Þ ¼ d ck þ nz2
ð10Þ
k
ðmÞ zðmÞ 3k ¼ h3 ðxk Þ 8 ! > zck > 1 > > tan þ nðmÞ z3k > > x > c k > > ! > > > zc k p > 1 > > þ nðmÞ > z3k < 2 þ tan xck ! ¼ > zc k > > > p þ tan1 þ nðmÞ > z3k > x > ck > > ! > > > zc k > 3p > 1 > þ nðmÞ > z3k : 2 þ tan xck
for xck . 0 and zck , 0 for xck , 0 and zck , 0 for xck , 0 and zck . 0 for xck . 0 and zck . 0 ð11Þ
zðmÞ 4k
¼
h4 ðxðmÞ k Þ
0 1 8 > > > B yck C ðmÞ > > for yck , 0 tan1 @qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi A þ nz4 > > k 2 2 > xck þ zck < 0 1 ¼ > > > y > ck B C ðmÞ > > tan1 @qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi A þ nz4 for yck . 0 > > k : 2 2 x c k þ zc k
ð12Þ
ðmÞ zðmÞ ð4þiÞk ¼ h4þi ðxk Þ
¼ dck þ ðl ck Ak Þpik þ nðmÞ zð4þiÞ
with
2
i ¼ 1; . . . ; M
ð13Þ
k
ðmÞ _ _ _ zðmÞ ð4þMþiÞk ¼ h4þMþi ðxk Þ ¼ d ck þ ðl ck Ak þ l ck Ak Þpik
zðmÞ ð5þ2MÞk
þ ðl ck Ak Þ_pik þ nðmÞ zð4þMþiÞk i ¼ 1; . . . ; M qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ðmÞ 2 2 2 ¼ h5þM ðxðmÞ k Þ ¼ e1k þ e2k þ e3k þ nzð5þ2MÞ
ð14Þ ð15Þ
k
ð6Þ IET Control Theory Appl., Vol. 1, No. 4, July 2007
1025
where i indexes the SCs and M is the assumed number of them, Ak is defined in (2), A_ k ¼ d Ak =dt, other variables are defined by qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi dck ¼ x2ck þ y2ck þ z2ck ð16Þ xck x_ ck þ yck y_ ck þ yck y_ ck qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi x2ck þ y2ck þ z2ck
ð17Þ
pik ¼ ½ui ; vi ; wi Tk
ð18Þ
p_ ik ¼ ½_ui ; v_ i ; w_ i Tk " # xck yck zck l ck ¼ ; ; dck dck dck
ð19Þ
d_ ck ¼
ðsÞ and nðsÞ xk N ð0; Q Þ with 2
QðsÞ
ðsÞ zðsÞ ik ¼ dck þ ðl ck Ak Þpik þ nzi
ð30Þ
k
_ _ _ pik zðkÞ ðMþiÞk ¼ d ck þ ðl ck Ak þ l ck Ak Þpik þ ðl ck Ak Þ_ þ nðsÞ zðiþMÞ
i ¼ 1; . . . ; M
ð31Þ
k
Moreover h ðmÞ ðmÞ ðmÞ ðmÞ ðmÞ W nðmÞ nðmÞ zk z1 ; nz2 ; nz3 ; nz4 ; nz5 ; . . . ; nzð4þMÞ ; ðmÞ ðmÞ nðmÞ zð5þMÞ ; . . . ; nzð4þ2MÞ ; nzð5þ2MÞ
iT
ð22Þ
is the GMTI, HRR and pseudo measurement noise vector, ðmÞ which is assumed to be nðmÞ zk N ð0; R Þ with 2 3 Rgmti O4;2M O4;1 6 O2M;1 7 ð23Þ RðmÞ ¼ 4 O2M;4 Rhrr 5 O1;4 O1;2M s2pseudo ð24Þ Rgmti ¼ diagðs2d ; s2d_ ; s2h ; s2e Þ " # s2R I M;M þ s2r I M OM;M Rhrr ¼ ð25Þ OM;M s2R_ I M;M þ s2r_ I M where sd , sd_ , sh , and se are measurement standard deviations (STDs) of range, range-rate, azimuth and elevation angles from GMTI sensor; sR and sr are common and individual STDs of HRR range measurements [1]; sR_ and sr_ are common and individual STDs of HRR range-rate measurements. Having defined the system and measurement equation pair in (4) and (9 – 15), we can use the standard EKF algorithm to estimate motion state vector and its covariances from measurement history. 3.2 Estimate structure assuming the motion is known The state vector of the structure filter is defined as
w_ 1 ; . . . ; u_ M ; v_ M ; w_ M Tk
By using the uncoupled constant velocity model, the kinematics equation of the system can be expressed by ð27Þ
where F
ðsÞ
¼
I 3M O3M;3M
O3M;3M T I 3M
ðsÞ ðsÞ ðsÞ zðsÞ k ¼ H k xk þ nzk
ð32Þ
where h iT ðsÞ ðsÞ ðsÞ W z ; . . . ; z ; z ; . . . ; z zðsÞ ðMþ1Þ M 1 k ð2MÞ
ð33Þ
k
h iT ðsÞ ðsÞ ðsÞ ðsÞ nðsÞ zk W nz1 ; . . . ; nzM ; nzðMþ1Þ . . . ; nzð2MÞ k " # ðsÞ H a O3M;3M H ðsÞ k W H ðsÞ H ðsÞ a b k
ð34Þ ð35Þ
H ðsÞ with H ðsÞ a ¼ diagðak ; . . . ; ak Þ, b ¼ diagðbk ; . . . ; bk Þ, _ ak ¼ l ck Ak , and bk ¼ l ck Ak þ l ck A_ k . Moreover, according to the rigidity constraint that the distance between any two SCs is a constant during the kinematics evolution, we have ðui uj Þ2 þ ðvi vj Þ2 þ ðwi wj Þ2 ¼ rij W const 81 i , j M
ð36Þ
Take the first-order derivative with respect to time for both sides of (36) and consider ui u_ i þ vi v_ i þ wi w_ i ¼ 0
81 i M
ð37Þ
ui u_ j þ uj u_ i þ vi v_ j þ vj v_ i þ wi w_ j þ wj w_ i ¼ 0 81 i , j M ð26Þ
ðsÞ ðsÞ ðsÞ xðsÞ kþ1 ¼ F xk þ nxk
where pik ; p_ ik ; l ck and l_ck are defined in (18) to (21), respectively; Ak is defined in (2) and again A_ k ¼ dAk =dt. Actually, by knowing the motion parameters, the two measurement equations in (30) and (31) are linear. Their matrix form can be written as
we get the M(M 2 1)/2 constraint equations, that is
_ 1 ; v_ 1 ; xðsÞ k ¼ ½u1 ; v1 ; w1 ; . . . ; uM ; vM ; wM ; u
1026
ð29Þ
On the measurement side, since the motion parameters are known, a measurement range and range-rate have the same form as that in (13) and (14) but are only dependent on ðui ; vi ; wi Þ and ð_ui ; v_ i ; w_ i Þ (treat the elements of motion state vector as constants). As a result, the single scan measurement equations at tk can be simplified as
ð20Þ
" # x_ ck dck xck d_ ck y_ ck dck yck d_ ck z_ ck dck zck d_ ck _l c ¼ ; ; ð21Þ k dc2k dc2k dc2k
3 T3 I 2 3M 7 7 5 2 T I 3M
T4 I 6 4 3M ¼ s2qs 6 4 T3 I 2 3M
ð28Þ
ð38Þ Therefore, the constraints in (38) provide extra M(M 2 1)/2 pseudo measurements Note that since the rank of F ðsÞ is 3M and the rank of the observability matrix of pair (F ðsÞ ; H ðsÞ k ) is M (see Appendix A for proof details), the system in (27) is unobservable with the measurement equations (30, 31). To overcome this shortage, we propose a multiscan measurement method as follows, and show that when the minimum number of scans is equal to or greater than three, the system is observable (see Appendix B for details). IET Control Theory Appl., Vol. 1, No. 4, July 2007
Define an N-scan measurement block ðN ¼ 1; 2; . . .Þ h iT ðsÞ ðsÞ ðsÞ ðsÞ ð39Þ zðsÞ k W z1 ; . . . ; zM ; zðMþ1Þ ; . . . ; zð2MÞ
HRR/GMTI
z k -2 ˆ (m) xˆ(m) k-3|k-3 motion xk-2|k-2 filter at k-2
k
h iT ðsÞ ðsÞ ðsÞ ðsÞ zðsÞ kþ1 W z1 ; . . . ; zM ; zðMþ1Þ ; . . . ; zð2MÞ
z k -1 motion filter at k-1
z k +1
zk
ˆ (m) xˆk(m) −1|k-1 motion xk|k filter at k
motion filter at k+1
z k+2
z k +3
xˆ (m) xˆ(m) ˆ (m) k + 3|k + 3 k+1|k+1 motion x k+2|k+2 motion filter at k+2
filter at k+3
ð40Þ
kþ1
xˆ(s) k-3|k-3
.. . h iT ðsÞ ðsÞ ðsÞ ðsÞ zðsÞ kþN 1 W z1 ; . . . ; zM ; zðMþ1Þ ; . . . ; zð2MÞ
and its corresponding uncertainty vectors h iT ðsÞ ðsÞ ðsÞ ðsÞ nðkÞ zk W nz1 ; . . . ; nzM ; nzðMþ1Þ ; . . . ; nz2M ðsÞ ðsÞ ðsÞ ðsÞ nðsÞ zkþ1 W nz1 ; . . . ; nzM ; nzðMþ1Þ ; . . . ; nz2M
iT
ð42Þ ð43Þ
.. .
ðsÞ
ðsÞ ðsÞ zðsÞ kþ1 ¼ H kþ1 xk þ nzkþ1
ð44Þ
ðsÞ
ð46Þ
ð47Þ
ðsÞ
where the explicit forms of H kþj and nðsÞ zkþj are given in Appendix C. By using the correlations between nðsÞ zkþi and ðsÞ nzkþj ði; j ¼ 1; . . . ; N 1Þ, we can evaluate the covariance matrix of multiscan measurements. The details are presented in Appendix C. 3.3 Simultaneously estimating motion and structure information To simultaneously estimate the motion and structure information of an object, we designed an architecture to run the two filters in parallel by using a multiscan technique. As shown in Fig. 2, the proposed algorithm first estimates the current motion state by an EKF with GMTI and HRR measurements. Then with the nearest previous N-scan motion estimation results and the latest structure information, a Kalman filter (KF) is used to estimate the current structure by employing multiscan HRR measurements and a pseudo measurement for rigidity constraints. 4
Measurement data association methods
Several data association methods are proposed to handle the unknown correspondences in HRR measurements with false alarms (FAs) and occlusions. In the following discussion, we assume the target of interest has Q SCs, and the actual number of measurement points in the measurement space at time tk is mk . The details are briefly reported as follows. † Permutation technique. If mk ¼ Q but without knowing HRR correspondence information, we can exhaust all the IET Control Theory Appl., Vol. 1, No. 4, July 2007
z hrr k +3
Step 1: Form the synthetic measurement sequences. It can be done by (i) choosing Q points from mk measurement points by taking all the combinations, and (ii) doing the permutation inside each combination. Step 2: Select the k-best candidates. By evaluating the likelihood value of all the hypothesis measurements in the measurement space, the k-best candidates can be selected. Then the PDA method is employed to do the data association.
ð45Þ
.. . ðsÞ ðsÞ zðsÞ kþN 1 ¼ H kþN 1 xk þ nzkþN 1
z hrr k+2
possible Q! sequences and select the k-best to update the filter by the PDA method [20]. † Combination– permutation technique. If we have FAs, table is, mk . Q. In this case, two steps are used to do the data association.
where zðsÞ ikþj is the range measurement for ith SC at time tkþj , ðsÞ and zðMþiÞkþj is the range-rate measurement for ith SC at time tkþj , with i ¼ 1; . . . ; M and j ¼ 0; . . . ; N 1. According to (27) and (32), we have ðsÞ
z hrr k +1
xˆ (s) k + 3|k + 3
Fig. 2 Data flow of simultaneously estimating motion and structure information by using multiscan HRR/GMTI measurements
kþN 1
ðsÞ ðsÞ zðsÞ k ¼ H k xk þ nzk
z hrr k
structure filter at k+3
Block size N= 3
kþ1
h iT ðsÞ ðsÞ ðsÞ ðsÞ nðsÞ zkþN 1 W nz1 ; . . . ; nzM ; nzðMþ1Þ ; . . . ; nz2M
z hrr k -1
xˆ (s) k |k
HRR
k
h
z hrr k -2
ð41Þ
kþN 1
structure filter at k
† Dummy measurement. When an occlusion occurs, that is, mk . Q, we can add additional mk Q dummy measurements to form synthetic measurements, and use the permutation technique to update the filter. † Dynamically changing the number of interest SCs. This is an alternative way to handle the occlusion scenario. In our proposed architecture in Section 3.3, since the motion filter is independent of the number of SCs and the state vector of the structure filter is constant, we can dynamically change the number of SCs to update the structure filter and the structure information of the occluded SCs can still be estimated by motion parameters with rigidity constraints. 5
Simulations
An object with five SCs is simulated to test the proposed algorithm. The coordinates of the five SCs in the BFCS are p1(21.9, 3.5, 1.67) m, p2(1.8, 3.6, 1.68) m, p3(21.7, 23.1, 1.5) m, p4(1.6,23.3, 1.45) m, and p5 (0, 26.0, 1.95) m. As shown in Fig. 3, the global motion trajectory of the object is on the plane of y ¼ 2333.3 m. Its beginning point is located at (3533.3, 2333.3, 3333.3) m in the SFCS, and the radius of the circle is 200 m with constant angular velocity V ¼ p/180 rad/s. Meanwhile, the object is rotating about the axis e ¼1/3 [1, 1, 1]T in the BFCS with angular velocity v ¼ p/90 rad/s. Moreover, the GMTI and HRR sensors are located at (0, 0, 0) in the SFCS, and the sampling interval T ¼ 1 s. A synthetic measurement data generation algorithm is used to simulate the GMTI and HRR sensors. The measurements are formed by adding Gaussian noise to the ground truth of GMTI and HRR range, range-rate, and angle measurements. The observation noise standard deviations (STDs) for GMTI are sd ¼ 2 m, sd_ ¼ 0:2 m/s, sh ¼ 0:001 rad, se ¼ 0:001 rad. The STDs for HRR noise are sR ¼ 0:1 m, sR_ ¼ 0:1 m/s, sr ¼ 0:02 m, sr_ ¼ 0:02 m/s. 1027
GRR/GMTI
† SIII: Simultaneously estimate motion and structure parameters without the correspondence information and without FAs. It is used to test the ability of the algorithm to handle measurement data associations. † SIV: Simultaneously estimate motion and structure parameters without the correspondence information but with FAs. It is used to test the ability of the algorithm to handle measurement data associations with FAs.
y o
x
z
Rc
w
Ω R0
Fig. 3 Drawing up of the simulation scenario
The following four scenarios are considered in our simulations. † SI: Estimate motion with known structure information or estimate structure parameters with a priori motion information under the assumption that HRR range and range-rate correspondences between two consecutive scans are known. It is designed to test the correctness of implementation of the system and measurement equations. † SII: Simultaneously estimate motion and structure parameters with known HRR range and range-rate correspondences. This scenario is designed to test the implementation correctness of the algorithm presented in Fig. 2.
estimated true
3500
-332 y (m)
3400
x c (m)
For all the scenarios, the system noise STDs are chosen as sqg ¼ 1 m/s2 , sqe ¼ 0:01, sqa ¼ 0:001 rad/s2 for motion filter, and sqs ¼ 0:01 m/s2 for the structure filter. The detection probability is set as Pd ¼ 0:98 in the PDA algorithm for scenarios SIII and SIV . The filters are initialised by adding system noise on the ground truth of the state vectors. Finally, in scenario SIV , the range FA measurements at each sampling time are generated by the following steps: (i) calculate the actual number of FA measurements by n ¼ Nf j, where Nf is a design parameter, and j is a uniform random number in ½0; 1; (ii) form a range interval by finding their maximum and minimum from synthetic measurements, and extending the interval 10% larger; (iii) generate n uniformly distributed range FA measurements from the extended range intervals. The same method is also used to construct the range-rate FA measurements. As an example, nine true and estimated (with an average of 50 Monte Carlo runs) global motion elements of SII are plotted in Fig. 4, and its local motion and the third SC’s structure information are displayed in Fig. 5. These plots
-333
c
3300
-334
3200 3100 50
100
150
200
250
300
350
-3100 dx /dt (m/s)
z (m)
-3300
150
200
250
300
350
-3400
50
100
150
200
250
300
350
50
100
150
200
250
300
350
50
100
150
200
250
300
350
2 0
c
c
100
4
-3200
-3500
-2 -4
50
100
150
200
250
300
350
4 2
dz /dt (m/s)
1 0.5
0
c
0
c
dy /dt (m/s)
50
-0.5
-2 -4
-1 50
100
150
200
250
300
350
d2y c/dt2 (m/s 2)
0
c
d2x /dt2 (m/s 2)
0.4 0.2
-0.2 50
100
150
200
250
300
350
0.2 0 -0.2
0.2 0
c
d2z /dt2 (m/s 2)
k
-0.2 50
100
150
200
250
300
350
k
Fig. 4 Evolutions of true and estimated (with an average of 50 Monte Carlo runs) global motion elements for scenario SII 1028
IET Control Theory Appl., Vol. 1, No. 4, July 2007
show that the filter is stable after converging to the states around the ground truth. To compare the performance of the filter in all scenarios, we calculate the averaged root mean square (ARMS) errors with N Monte Carlo runs, that is vffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi u K N u X 1X t1 ARMS err ðxi Þ ¼ ð48Þ ½^x ðkÞ x ijðkÞ2 N j¼1 K k¼1 ij where N is the total number of Monte Carlo runs; K is the total number of samplings; x^ ij ðkÞ and x ij ðkÞ are the estimated and the ith true state element of the jth Monte Carlo run at time index k, respectively. The ARMS errors of motion and structure information of the four scenarios are listed in Tables 1, 2, and 3 from 50 Monte Carlo runs. These results show that both motion and structure parameters of a rigid-body can be estimated with GMTI and HRR measurements by a proper data association method even in a cluttered environment with FAs. A comment on the numerical fluctuations in the simulations is in order. Due to a moderate number of Monte Carlo runs and a high order of nonlinearity, the ARMS numbers in the simulations exhibited some degree of fluctuation. 6
Summary and conclusion
In summary, we investigated the problem of estimating motion and structure information of a 3D rigid object by
Table 1: ARMS error table of a motion filter with 50 Monte Carlo runs for different scenarios III
IV (Nf = 2)
IV (Nf = 8)
xc
0.4179
0.5963
0.5698
0.6715
0.7080
0.5563
0.6207
0.6246
0.6388
0.7221
zc
0.4070
0.4698
0.6779
0.5941
0.5892
x_ c
0.2623
0.3178
0.3568
0.3579
0.3744
y_ c
0.3331
0.3864
0.4327
0.4089
0.4802
z_ c
0.2596
0.3183
0.3470
0.3527
0.3636
x€ c
0.0834
0.1021
0.1144
0.1170
0.1224
y€ c
0.1061
0.1244
0.1422
0.1316
0.1583
z€ c
0.0838
0.1057
0.1125
0.1133
0.1182
nx
0.0124
0.1787
0.2415
0.2326
0.2351
ny
0.0100
0.1433
0.1744
0.1790
0.1824
nz
0.0163
0.0689
0.0868
0.0798
0.1159
u u˙
0.0018
0.6813
0.8677
0.8804
1.2557
0.0002
0.0040
0.0053
0.0054
0.0073
Unit: m for location, m/s for velocity, m/s2 for acceleration, deg for rotation angle, and deg/s for angular velocity
using HRR and GMTI measurements under the framework of a KF algorithm. The state of a 3D rigid object is divided into global motion, local motion, and structure information based on motion decomposition. Global motion describes 2
estimated true
0.7 e1
II
yc
0.8
1.5 w
0.6
3
1
0.5
0.5 0
50
100
150
200
250
300
0
350 u3,v3,w3 (m)
0.4
I
0.8 0.7
-0.5 -1 u
3
e2
-1.5 0.6 -2 0.5 -2.5 0.4
0
50
100
150
200
250
300
350
v3
-3 -3.5
0.8
0
50
100
150
e3
0.7
200
250
300
350
200
250
300
350
b
0.6 0.5 -3
0.4
0
50
100
150
200
250
300
5
350
x 10
dw3/dt
0 -5 -10 -15
0
50
100
150
200
250
300
350
dα /dt (rad./s)
-0.03
du3/dt, dv3/dt,dw3/dt (m/s)
α (rad.)
0
du3/dt
-5
dv3/dt
-10
-15
-0.035
-0.04 0
50
100
150
200
250
300
350
-20
0
50
100
150
k
k
a
b
Fig. 5 Evolutions of true and estimated (with an average of 50 Monte Carlo runs) local motion elements (a), and the position (b) and velocity (c) of the third scattering centre for scenario SII IET Control Theory Appl., Vol. 1, No. 4, July 2007
1029
Table 2: ARMS error table of the derivatives of SC positions in the BFCS with 50 Monte Carlo runs for different scenarios I
II
III
IV (Nf ¼ 2)
(Nf ¼ 8)
u1
0.0026
0.0161
0.0216
0.0236
0.0219
v1
0.0027
0.0310
0.0286
0.0250
0.0269
w1
0.0020
0.0386
0.0085
0.0070
0.0090
u2
0.0132
0.0397
0.0342
0.0304
0.0343
v2
0.0104
0.0573
0.0570
0.0558
0.0557
w2
0.0121
0.0272
0.0187
0.0187
0.0176
u3
0.0071
0.0408
0.0333
0.0338
0.0274
v3
0.0054
0.0431
0.0398
0.0478
0.0400
w3
0.0053
0.0721
0.0577
0.0635
0.0576
u4
0.0102
0.0255
0.0441
0.0415
0.0285
v4
0.0076
0.0179
0.0211
0.0193
0.0232
w4
0.0090
0.0630
0.0577
0.0528
0.0490
u5
0.0132
0.1048
0.0775
0.0729
0.0659
v5
0.0132
0.1166
0.1170
0.1193
0.1043
w5
0.0124
0.0592
0.0534
0.0581
0.0472
Unit: m
the position, velocity and acceleration of a pivoting centre; local motion is described by the rotation axis, angle and angular velocity; and structure is described by the positions of SCs in the BFCS. The uncorrelated constant acceleration model is used to describe the kinematics of motions and GMTI with HRR measurements are employed to update the motion states. For structure modelling, both its system and measurement are modelled by linear equations. Moreover, since the structure subsystem is unobservable by using only one scan HRR observations, a multiscan measurement method is proposed to estimate the structure information. In addition, to test the ability of the proposed algorithms in handling false alarms, occlusions and HRR measurements without correspondence information, several data association methods are also Table 3: ARMS error table of the derivatives of SC positions in the BFCS with 50 Monte Carlo runs for different scenarios I
II
III
IV (Nf ¼ 2)
(Nf ¼ 8)
u_ 1
0.0067
0.0184
0.0109
0.0116
0.0107
v_ 1
0.0021
0.0049
0.0037
0.0033
0.0029
_1 w
0.0046
0.0124
0.0082
0.0083
0.0086
u_ 2
0.0071
0.0195
0.0129
0.0124
0.0118
v_ 2
0.0049
0.0120
0.0069
0.0069
0.0069
_2 w
0.0029
0.0077
0.0036
0.0037
0.0040
u_ 3
0.0048
0.0109
0.0046
0.0052
0.0049
v_ 3
0.0015
0.0040
0.0022
0.0021
0.0018
_3 w
0.0026
0.0056
0.0026
0.0026
0.0030
u_ 4
0.0051
0.0132
0.0083
0.0074
0.0070
v_ 4
0.0044
0.0116
0.0059
0.0064
0.0063
_4 w
0.0042
0.0117
0.0083
0.0081
0.0081
u_ 5
0.0096
0.0248
0.0137
0.0136
0.0129
v_ 5
0.0017
0.0046
0.0030
0.0029
0.0031
_5 w
0.0062
0.0153
0.0075
0.0085
0.0091
Unit: m/s 1030
proposed, which are permutation, combinationpermutation, dummy measurements, and dynamically changing the interested number of SCs. Simulation results show that both the motion and structure information of a 3D object can be estimated by using multiscan HRR and GMTI measurements.
7
Acknowledgment
S. Wu would like to acknowledge the financial support from the Employee Educational Benefit Program of Wright State University and the Dayton Area Graduate Studies Institute (DAGSI) for his engineering PhD program of study. The authors also would like to thank the anonymous reviewers for their helpful comments. This work was done while S. Wu was with Wright State University.
8
References
1 Wu, S., Hong, L., and Layne, J.: ‘2D rigid-body target modelling for tracking and identification with GMTI/HRR measurements’, IEE Proc., Control Theory Appl., 2004, 151, (4), pp. 429–438 2 Li, X.R., and Jilkov, V.P.: ‘Survey of maneuvering target tracking. Part I: Dynamic models’, IEEE Trans. Aerosp. Electron. Syst., 2003, 39, (4), pp. 1333–1364 3 Li, X.R., and Jilkov, V.P.: ‘Survey of maneuvering target tracking. Part III: Measurement models’. Proc. SPIE Conf. on Signal and Data Processing of Small Targets, July 2001, vol. 4473, pp. 423 –446 4 Bar-Shalom, Y., and Li, X.R.: ‘Estimation and tracking: principles, techniques, and software’ (Artech House, Norwood, MA, 1993) 5 Stuff, M.: ‘Derivation and estimation of Euclidean invariants of far-field range data’. PhD dissertation, University of Michigan, 2002 6 Stuff, M., Sanchez, P., and Biancalana, M.: ‘Extraction of three-dimensional motion and geometric invariants from range dependent signals’, Multidimen. Syst. Sig. Process., 2003, 14, pp. 161–181 7 Jacobs, S., and O’Sullivan, J.A.: ‘A high resolution radar model for joint tracking and recognition’. Proc. IEEE National Radar Conf., May 1997, pp. 99– 104 8 O’Sullivan, J.A., Jacobs, S.P., Miller, M.I., and Snyder, D.L.: ‘A likelihood-based approach to joint target tracking and identification’. Proc. 27th Asilomar Conf. on Signals, Systems, and Computers, November 1993, vol. 1, pp. 290–294 9 Miller, M.J., Srivastava, A., and Grener, U.: ‘Conditional-mean estimation via jump-diffusion processes in multiple target tracking/ recognition’, IEEE Trans. Signal Process., 1995, 43, (11), pp. 2678–2690 10 Sworder, D.D., and Boyd, J.E.: ‘Jump-diffusion processes in tracking/ recognition’, IEEE Trans. Signal Process., 1998, 46, (1), pp. 235 –239 11 Blasch, E., and Hong, L.: ‘Sensor fusion cognition using belief filtering for tracking and identification’. Proc. SPIE Aerosense, Orlando, FL, 1999, vol. 3719, pp. 250–259 12 Blasch, E., and Hong, L.: ‘Simultaneous feature-based identification and track fusion’. Proc. IEEE Int. Conf. on Decision and Control, Tampa, FL, 1998, pp. 239 –244 13 Dezert, J.: ‘Tracking manoeuvring and bending extended target in cluttered environment’. Proc. SPIE: Signal and Data Processing of Small Targets, 1998, vol. 3373, pp. 283– 294 14 Salmond, D.J., and Gordon, N.J.: ‘Group and extended object tracking’. Proc. SPIE: Signal and Data Processing of Small Targets, Denver, July 1999, vol. 3809, pp. 16/1– 16/4 15 Pulford, G., and Salmond, D.J.: ‘A Gaussian mixture filter for near-far object tracking’. Proc. 7th Int. Conf. on Information Fusion, Philadelphia, July 2005, Philadelphia, pp. 337– 344 16 Vermaak, J., Ikoma, N., and Godsill, S.J.: ‘Sequential Monte Carlo framework for extended object tracking’, IEE Proc., Radar Sonar Navig., 2005, 152, (5), pp. 353–363 17 Gu, B., and Hong, L.: ‘Tracking 2-D rigid targets with invariant constraint’, Information Science, 2001, 138, pp. 79–97 18 Hong, L., Wu, S., and Layne, J.: ‘Invariant-based probabilistic target tracking and identification with GMTI/HRR measurements’, IEE Proc., Radar Sonar Navig, 2004, 151, (5), pp. 280 –290 19 Wertz, J.R.: ‘Spacecraft attitude determination and control’ (D. Reidel Publishing Co., 1978) 20 Blackman, S., and Popoli, R.: ‘Design and analysis of modern tracking system’ (Artech house, Norwood, MA, 1999), chap. 6 IET Control Theory Appl., Vol. 1, No. 4, July 2007
9 Appendix A: Rank of the observability matrix in system and measurement pair (27) and (32)
11 Appendix C: Measurement covariance matrix in multiscan measurement architecture
For a rigid-body object with M SCs, its system transition and the measurement matrices of the pair (27) and (32) are defined in (28) and (35), respectively. The dimension of F(s) is 6M 6M, and H(s) k is 2M 6M. The observability matrix of the pair fF(s), H(s) k g can be calculated by h iT ðsÞ ðsÞ T ðsÞ T ðsÞ 6M1 T Qo W ½H ðsÞ ð49Þ k ½H k F ½H k ðF Þ
To derive the covariance matrix of the multiscan measurements, consider a general linear system whose plant equation can be expressed as
By considering (28) and (35), we have " # O H ðsÞ ðsÞ ðsÞ n a H k ðF Þ ¼ n1 H ðsÞ H ðsÞ a T b and Qo can be simplified as " T T ½H ðsÞ ½H ðsÞ a b Qo ¼ ðsÞ T O ½H a
O
T ½H ðsÞ b
O
T ½H ðsÞ a T #T
Efvi vTj g ¼ Qi dij ; ð51Þ
6M1 T ½H ðsÞ a T
10 Appendix B: Minimum number of scans in multiscan measurement architecture In Appendix A, we proved that the system (27) is unobservable by using only one scan measurement. In this appendix, we show that the minimum number of scans required in the multiscan method is three such that the system is observable. According to the multiscan architecture described in Section 3.3, the equivalent measurement equations of an N-scan scenario can be written as h iT ðsÞ T ðsÞ T T ½zðsÞ ½z ½z k kþ1 kþN 1 ¼
h
T þ ½nðsÞ zk
zk ¼ H k xk þ wk
T ðsÞ T ½nðsÞ zkþ1 ½nzkþN 1
ð56Þ
where zk is an m dimensional measurement vector, and H k is an m n dimensional measurement matrix. Further, assume that both vk and wk are zero mean, white Gaussian processes with covariance properties
Hence, rank (Qo) ¼ rank(H(s) k ). Further, since the three elements in ak and bk cannot be zeros at the same time, we have rank(H(s) k ) ¼ 2M. Therefore, rank(Qo) ¼ 2M. On the other hand, since rank(F(s)) ¼ 6M, which is greater than rank(Qo), the system and measurement pair of (27) and (32) is not observable.
Hk xðsÞ k
ð55Þ
where xk is an n dimensional state vector at time tk , and F k;k1 is the state transition matrix from tk1 to tk . The measurement equation can be written as
ð50Þ
T ½H ðsÞ a
T ½H ðsÞ b
T ½H ðsÞ a
xk ¼ F k;k1 xk1 þ vk1
iT
Efwi wTj g ¼ Ri dij ;
Efwi vTj g ¼ 0 ð57Þ
where dij ¼ 1 for i ¼ j, dij ¼ 0 otherwise. Traditional Kalman filter based estimation algorithms use one-scan measurements to estimate the plant state. Hence, the measurement covariance matrix is Rk . However, in the multiscan measurement scenario, because all the measurements at tk and later on are linked by state xk and its uncertainty, the covariances between zkþi ði . 0Þ and zkþj ð j . 0Þ are non-zero. Therefore, the task in this appendix is to evaluate the non-zero covariance matrices between zkþi ði . 0Þ and zkþj ð j . 0Þ. Although the universal forms for any number of scans can be obtained, we give the evaluations from a two-scan measurement scenario to a five-scan measurement scenario. For convenience in expressing our derivations, we define a system transition matrix from tk to tkþn by F kþn;k WF kþn;kþn1 F kþn1;kþn2 F kþ1;k
ðn 1Þ ð58Þ
(i) Two-scan measurement scenario. For the first scan, we have
ð52Þ zk ¼ H k xk þ wk
where (from Appendix C) iT h ðsÞ T ðsÞ T ðsÞ N 1 T ½H ðsÞ Hk W ½H ðsÞ k k F ½H k ðF Þ
2N ðM6MÞ
ð53Þ
ð59Þ
Further, the second scan measurement equation can be written as a function of xk and its relative noise terms by zkþ1 ¼ H kþ1 xkþ1 þ wkþ1
ð60Þ
ðsÞ
From (49) and considering that F is a diagonal block matrix, we have the observability matrix of the pair fF ðsÞ ; Hg as h QðmultiÞ ¼ ½Hk T ½Hk F ðsÞ T ½Hk ðF ðsÞ Þ6M1 T T12N ðM 2 6MÞ o ð54Þ
zkþ1 ¼ H kþ1 F kþ1;k xk þ H kþ1vk þ wkþ1
2MN 6M
for N , 3 for N 3
we conclude that the minimum number of scans is three such that the system is observable. IET Control Theory Appl., Vol. 1, No. 4, July 2007
ð61Þ
Based on (57), (59) and (61), we can evaluate the covariances covfzk ; zk g ¼ Efwk wTk g ¼ Rk
Due to the fact that rankðQðmultiÞ Þ ¼ rankðHk Þ ¼ o
Inserting xkþ1 ¼ F kþ1;k xk þ vk into (C.6), gives
covfzk ; zkþ1 g ¼ Efwk ½H kþ1 vk þ wkþ1 T g ¼ O
ð62Þ ð63Þ
covfzkþ1 ; zkþ1 g ¼ Ef½H kþ1 vk þ wkþ1 ½H kþ1 vk þ wkþ1 T g ¼ H kþ1 Qk H Tkþ1 þ Rkþ1
ð64Þ 1031
where
where O is an M M matrix of zeros. Therefore
R R2 W k O
O H kþ1 Qk H Tkþ1 þ Rkþ1
covfzkþ1 ; zkþ3 g ¼ H kþ1 Qk F Tkþ3;kþ1 H Tkþ3
ð65Þ
ð74Þ
covfzkþ2 ; zkþ3 g ¼ H kþ2 ðF kþ2;kþ1 Qk F Tkþ3;kþ1 þ Qkþ1 F Tkþ3;kþ2 ÞH Tkþ3
(ii) Three-scan measurement scenario. covfzkþ3 ; zkþ3 g ¼
ð75Þ
H kþ3 ðF kþ3;kþ1 Qk F Tkþ3;kþ1 þ F kþ3;kþ2 Qkþ1 F Tkþ3;kþ2
zkþ2 ¼ H kþ2 xkþ2 þ wkþ2 ¼ H kþ2 ðF kþ2;k xk þ F kþ2;kþ1 vk þ vkþ1 Þ þ wkþ2
ð66Þ
þ Qkþ2 ÞH Tkþ3 þ Rkþ3
ð76Þ
In the derivation of (C.12), we used xkþ2 ¼ F kþ2;kþ1 xkþ1 þ vkþ1 xkþ1 ¼ F kþ1;k xk þ vk
ð67Þ ð68Þ
Beside the covariances of zk and zkþ1 mentioned in (65), we have the following terms covfzk ; zkþ2 g ¼ O
2 6 6 6 R5 W6 6 6 4 O
ð69Þ
covfzkþ1 ; zkþ2 g ¼ H kþ1 Qk F Tkþ2;kþ1 H Tkþ2
ð70Þ
covfzkþ2 ; zkþ2 g ¼ H kþ2 ðF kþ2;kþ1 Qk F Tkþ2;kþ1 þ Qkþ1 Þ H Tkþ2 þ Rkþ2
ð71Þ
covT fzkþ1 ; zkþ4 g
covT fzkþ2 ; zkþ4 g 3 O covfzkþ1 ; zkþ4 g 7 7 7 covfzkþ2 ; zkþ4 g 7 7 7 covfzkþ3 ; zkþ4 g 5 covT fzkþ3 ; zkþ4 g covfzkþ4 ; zkþ4 g
3
O
½R2
R 3 W4
covfzkþ1 ; zkþ2 g 5 O
ð72Þ
covfzkþ1 ; zkþ4 g ¼ H kþ1 Qk F Tkþ4;kþ1 H Tkþ4
(iii) Four-scan and five-scan measurement equations. By using the same strategy used in the two-scan and three-scan cases, we have the four-scan and five-scan measurement covariances as follows 2
3
O ½R3
ð78Þ
covfzkþ2 ; zkþ4 g ¼ H kþ2 ðF kþ2;kþ1 Qk F Tkþ4;kþ1
covT fzkþ1 ; zkþ2 g covfzkþ2 ; zkþ2 g
þ Qkþ1 F Tkþ4;kþ2 ÞH Tkþ4
6 6 R4 W 6 4
ð77Þ
where
Therefore 2
½R4
covfzkþ1 ;zkþ3 g 7 7 7 covfzkþ2 ;zkþ3 g 5
O covT fzkþ1 ;zkþ3 g covT fzkþ2 ;zkþ3 g covfzkþ3 ;zkþ3 g
ð79Þ
covfzkþ3 ; zkþ4 g ¼ H kþ3 ðF kþ3;kþ1 Qk F Tkþ4;kþ1 þ F kþ3;kþ2 Qkþ1 F Tkþ4;kþ2 þ Qkþ2 F Tkþ4;kþ3 ÞH Tkþ4
ð80Þ
covfzkþ4 ; zkþ4 g ¼ H kþ4 ðF kþ4;kþ1 Qk F Tkþ4;kþ1 þ F kþ4;kþ2 Qkþ1 F Tkþ4;kþ2 þ F kþ4;kþ3 Qkþ2 F Tkþ4;kþ3 þ Qkþ3 Þ H Tkþ4 þ Rkþ4
ð81Þ
ð73Þ
1032
IET Control Theory Appl., Vol. 1, No. 4, July 2007
