# Ballistic Target Tracking with Use of Cinetheodolites

Authors: Piotr Kaniewski; Piotr Smagowski; Stanislaw Konatowski
Journal: International Journal of Aerospace Engineering
 (2019)

---

## Abstract

The paper addresses a problem of ballistic object tracking with the use of the cinetheodolite electro-optical tracking system. Electro-optical systems are applied for acquiring the trajectory data of missiles, satellites, and rockets used for delivery of satellites to their prevised orbits. Despite the importance of such systems and their applications, in the open literature there are no publications describing tracking algorithms processing data from cinetheodolites. The paper describes a model-based algorithm of estimation of position and parameters of target motion for such a system, developed by the authors. The model of the system, nonlinear both in its description of the target dynamics and the measurement equations, is presented in detail. The proposed algorithm of estimation is also described, and chosen simulation results are included in the paper. Furthermore, a comparison of the proposed estimation algorithm with other possible, but simpler algorithms is presented.


---

## Body

## 1. Introduction

The main motivation for the work presented in this paper was a necessity to obtain possibly accurate and continuous data on position and other parameters of motion of ballistic objects in practical tests of weaponry conducted by the Military Institute of Armament Technology (MIAT), Zielonka, Poland. Among the areas of expertise of the Institute, tests of anti-aircraft missiles are of great importance. One of the methods to assess the capabilities of the missile is the live firing with trajectory recording. It allows estimating the position and velocity of the missile and the target and eventually the mutual geometrical relationship between them, particularly the miss distance.To estimate the position of a missile or a target, either an accurate on-board positioning system or an external tracking system is necessary. To assess the miss distance, an aerial target imitator fulfilling demanding requirements, between the others in terms of velocity, must be used. At MIAT, a special aerial target, ICP-89, based on the S-5 unguided missile, was developed. Its feasibility of deploying, high initial velocity, and market availability make it adequate for rocket missile tests. ICP-89 has proved its usefulness in numerous experiments with weapons so far.In practice, an accurate on-board positioning system with an appropriate downlink is unlikely to be installed on a combat missile (particularly a small one) or an aerial target such as ICP-89. Therefore, to assess the performance of the missile during live tests, electro-optical tracking systems are widely used. A cinetheodolite-based electro-optical tracking system (EOTS), acquiring precise angular data to assess flight paths of disparate objects, is currently used at MIAT. It is a core system to determine the performance of the tested missiles.In the open literature, there are no publications describing tracking algorithms for processing EOTS data; therefore, custom solutions were elaborated at MIAT. So far, straightforward geometrical computations have been used for position calculations of the missile and the aerial target. Such a method, however, has several significant drawbacks. Incomplete measurement data, occasionally occurring during some experiments, require supplementary techniques to attain only approximate and reduced-accuracy results. Moreover, the applied algorithm is not model-based and does not involve any data filtering; thus, its accuracy is not optimal. In this paper, a new algorithm of estimation of the position and parameters of ICP-89 target motion, based on the extended Kalman filter (EKF) [1–4], is proposed. It improves the accuracy of the estimated position, provides additional parameters of the target motion, such as its velocity and angular orientation, which are not directly available from the previously applied algorithm, and ensures availability of highly accurate, continuous data even in case of temporary outages of the measurements.The layout of the further part of this paper is as follows. The EOTS measurement system is presented in Section2. The proposed algorithms of estimation of a ballistic target position and parameters of motion together with a complete state-space model of the tracking system are given in Section 3. The paper includes also chosen results of simulations, demonstrating accuracy of the proposed algorithms (in Section 4) as well as discussion of the obtained results and conclusions given in Section 5.


## 2. EOTS Measurement System

The EOTS measurement system used for fire tests consists of at least two cinetheodolite stations, located at positions measured before the experiments, and providing for azimuthα and elevation λ angles of the tracked object. A photography of such a station is shown in Figure 1.Figure 1
EOTS cinetheodolite on the operating post.Positions of the stations and tracked object as well as all the parameters of motion are expressed in a local horizontal frame of reference which must be defined at the beginning of the experiments. The geometrical relationships in the EOTS system with two cinetheodolite stationsS1 and S2 are explained in Figure 2 [5].Figure 2
Geometrical relationships in the EOTS measurement system [5].The frame of reference is established as follows. Firstly, the positions ofS1 and S2 stations are measured with the use of an accurate geodetic-class GNSS receiver. These coordinates are obtained in a geodetic frame of reference [4]. At the firing stand, about one second before the ICP-89 is launched, its tracer is ignited, and it burns for the whole period of flight of the imitator. Therefore, the ICP-89 is clearly visible from the cinetheodolite stations and the EOTS can establish the angles of its observation. Based on the measured azimuth and elevation angles, its position expressed in the geodetic frame is also calculated. The local frame of reference is assumed to begin at point O, corresponding to the calculated ICP-89 position, and its local coordinates are set to zero. The orientation of the X-axis is assumed to correspond to the direction of shooting, which is approximated by a projection of the longitudinal axis of the launcher on the horizontal plane. The Z-axis is the local vertical, and the Y-axis completes a right-handed Cartesian frame of reference.After such a setting of the local frame of reference, the local coordinates ofS1 and S2 stations are calculated from the already known geodetic coordinates of O, S1, and S2. Local coordinates of these points are as follows: O=0,0,0, S1=x1,y1,z1, and S2=x2,y2,z2. For simplicity of further calculations, S1 and S2 are assumed to be located on the same horizontal plane as O; thus, their coordinates are S1=x1,y1,0 and S2=x2,y2,0. This assumption is not very restrictive, and the equations presented throughout the paper can be easily extended to a more general case of nonzero altitudes of S1 and S2.The cinetheodolite stations are synchronized by a common registration-triggering signal and perform acquisition of the images of the observed object as well as the angles of its observation from separate locations. The angles provided by two stations include azimuthsα1 and α2 along with the elevations λ1 and λ2, which are synchronized and time-stamped. The angle measurements are calculated from readouts obtained from encoders installed in the stations. The encoders register only changes of respective angles from an arbitrary point of reset. To obtain meaningful angular data, a known reference point must be recorded.A pair of measurements of azimuthαand elevation λ of the tracked object, obtained in two cinetheodolite stations of known locations, is sufficient for determination of the object positions and reconstruction of its trajectory. From the geometry of the system shown in Figure 2, the following formulae can be determined:
(1)d1sinα2=d2sinα1=dsinπ−α1−α2=dsinα1+α2,(2)d1=x−x12+y−y12,(3)d2=x−x22+y−y22,(4)d=x1−x22+y1−y22,(5)z=d1tanλ1,(6)z=d2tanλ2,where d is the distance between both EOTS stations (base of the system) and d1 and d2 are horizontal distances from S1 and S2 stations to the tracked object.Based on the above equations, the following set of four equations with three unknownsx,y,z can be formulated:
(7)x−x22+y−y22sinα1=x1−x22+y1−y22sinα1+α2,x−x12+y−y12sinα2=x1−x22+y1−y22sinα1+α2,z=x−x12+y−y12tanλ1,z=x−x22+y−y22tanλ2.This is an overdetermined set of equations, which can be approximately solved forx,y,z, if the sum of azimuth angles α1+α2≠0. The requirement can be easily fulfilled by appropriately locating the cinetheodolite stations behind the ICP-89 launcher, so that the tracked object would not pass over the baseline S1S2 of the system.Equation (7) can be simplified, if we notice that in the assumed local-level frame of reference, the target moves only in the OXZ plane. Thus, to represent its position we need only two coordinates x,z and the third coordinate y is always 0. Such an assumption was used for elaboration of the models and algorithms presented in the paper, and it resulted in the following simplified version of Equation (7):
(8)x−x22+y22sinα1=x1−x22+y1−y22sinα1+α2,x−x12+y12sinα2=x1−x22+y1−y22sinα1+α2,z=x−x12+y12tanλ1,z=x−x22+y22tanλ2.In practice, disturbing factors and an inaccurate setting of the reference frame may result in a nonzero value ofy, which can be considered a cross-track error. However, it does not significantly affect the estimation results for the along-track position x and altitude z. Thus, the results and conclusions presented in the paper would remain valid also for a more general case of modelling motion in all three axes of OXYZ.


## 3. Trajectory Estimation of Target Imitator

Estimation of the ICP-89 position can be realized with the use of various algorithms, which can be generally divided into two groups, i.e., non-model-based algorithms and model-based ones. The simplest are single-point solution algorithms, such as ordinary-least-squares (OLS) and weighted-least-squares (WLS) methods, which iteratively estimate the target position [6–9], individually for each new set of measurements. The model-based estimation methods make use of the previous estimates and project them ahead via model equations up to the time when the new measurements have been acquired. Then, the projected estimates are fused with the new measurements to provide for a more accurate solution. An example of such an algorithm is the EKF used by the authors for the ICP-89 trajectory estimation and described in this paper.


### 3.1. OLS Algorithm

The primary measurements realized in the EOTS measurement system are azimuth and elevation angles, which are obtained simultaneously, with a period of about 33 msec. For every time instance, they can be grouped into the primary measurement vectorz′ as follows:
(9)z′=α1α2λ1λ2T.As the relationship between the original measurement vectorz′ and the unknown target position x,z in OXZ is nontrivial, we propose here to construct a new measurement vector z as follows:
(10)z=tz′=sinα1sinα1+α2sinα2sinα1+α2tanλ1tanλ2,where t∗ is a transformation function of the primary (original) measurement vector z′ to the new form represented by the measurement vector z. The transformed vector z and the unknown state vector of the target position x=xzT are related by a nonlinear, but relatively simple formula z=hx+v, which can be deduced from Equation (8):
(11)sinα1sinα1+α2sinα2sinα1+α2tanλ1tanλ2⏟z=x−x22+y22x1−x22+y1−y22x−x12+y12x1−x22+y1−y22zx−x12+y12zx−x22+y22⏟hx+v.The above equation, apart from the geometrical relationships, includes also a vector of measurement errorsv, modelling limited accuracy of the measurement vector z. Equation (11) can be approximately solved with respect to the unknown position vector x using the OLS algorithm [6–9] as follows:
(1)
Assume an initial estimated target position vectorx^=x^z^T. This can be almost any reasonable guess, but the nearer it is to the true target position, the fewer iterations of OLS are necessary to achieve its required accuracy. The first guess can be based on knowledge of a typical target position, where the first EOTS images and angular data become available during firing tests. The subsequent guesses can be based on the previous target positions, estimated for the previous measurement vector z
(2)
Calculate the expected measurement vectorz^=hx^
(3)
Calculate the Jacobian matrixH=∂hx/∂xx=x^ of the nonlinear transformation z=hx, at the latest estimated target position x^. This yields the following linearized measurement equation:
(12)Δz^=z^−z=hx^−hx−v≈Hx^−x−v=HΔx^−v.
(4)
Solve Equation (12) and calculate a vector of estimation errors Δx^. For a full-rank matrix H, Δx^ is calculated with the use of the Moore-Penrose matrix pseudoinverse:
(13)Δx^=HTH−1HTΔz^.
(5)
Improve the previous estimate ofx^ by subtracting the calculated correcting term Δx^:
(14)x^←x^−Δx^.
(6)
Repeat steps 2 to 5 untilΔx^ falls below a predefined threshold Δx^≤Δx^trThe above OLS algorithm is run for every newly acquired measurement vectorz. The Jacobian matrix H in this algorithm is obtained as a matrix of partial derivatives of elements of the measurement vector z with respect to the elements of x, and they can be calculated based on the relationships given in Equation (11) as follows:
(15)H=∂hx∂xx=x^=∂z1∂x∂z1∂z∂z2∂x∂z2∂z∂z3∂x∂z3∂z∂z4∂x∂z4∂zx=x^,(16)∂z1∂xx=x^=∂∂xx−x22+y22x1−x22+y1−y22x=x^=x^−x2x^−x22+y22x1−x22+y1−y22,(17)∂z2∂xx=x^=∂∂xx−x12+y22x1−x22+y1−y22x=x^=x^−x1x^−x12+y12x1−x22+y1−y22,(18)∂z3∂xx=x^=∂∂xzx−x12+y12x=x^=−z^x^−x1x^−x12+y123,(19)∂z4∂xx=x^=∂∂xzx−x22+y22x=x^=−z^x^−x2x^−x22+y223,(20)∂z1∂zx=x^=∂z2∂zx=x^=0,(21)∂z3∂zx=x^=∂∂zzx−x12+y12x=x^=1x^−x12+y12,(22)∂z4∂zx=x^=∂∂zzx−x22+y22x=x^=1x^−x22+y22.Finally, the Jacobian matrixH is as follows:
(23)H=x^−x2x^−x22+y22x1−x22+y1−y220x^−x1x^−x12+y12x1−x22+y1−y220−z^x^−x1x^−x12+y1231x^−x12+y12−z^x^−x2x^−x22+y2231x^−x22+y22.


### 3.2. WLS Algorithm

The WLS algorithm is another single-point solution method [6–9]. It contains the same steps as the OLS, but it additionally weights the measurements depending on their expected accuracy, which improves the accuracy of estimation if the accuracies of measurements are different. If the standard deviations of angle measurement errors are known and given as σα1,σα2,σλ1,σλ2, and there is no cross-correlation of these errors, the following measurement errors covariance matrix R′ can be written down for the primary measurement vector z′:
(24)R′=Ev′v′T=σα120000σα220000σλ120000σλ22.As the measurement vectorz is obtained through the transformation z=tz′ of the primary measurement vector z′, the measurement errors of azimuth and elevation angles v′ also undergo a transformation, which can be obtained through a linearization of the function t∗ and realized as a multiplication of the v′ vector through the Jacobian matrix T:
(25)v=Tv′=∂tz′∂z′v′.Therefore, the covariance matrixR of the artificially constructed measurement vector z is also calculated through a transformation of the covariance matrix of the primary measurement errors R′ as follows:
(26)R=EvvT=ETv′Tv′T=ETv′v′TTT=T·Ev′v′T·TT=TR′TT.The Jacobian matrixT is obtained as a matrix of partial derivatives of elements of the measurement vector z with respect to the elements of the primary measurement vector z′, which are calculated as follows:
(27)T=∂tz′∂z′=∂z1∂α1∂z2∂α1∂z3∂α1∂z4∂α1∂z1∂α2∂z2∂α2∂z3∂α2∂z4∂α2∂z1∂λ1∂z2∂λ1∂z3∂λ1∂z4∂λ1∂z1∂λ2∂z2∂λ2∂z3∂λ2∂z4∂λ2,(28)∂z1∂α1=∂∂α1sinα1sinα1+α2=sinα2sin2α1+α2,(29)∂z1∂α2=∂∂α2sinα1sinα1+α2=−sinα1cosα1+α2sin2α1+α2,(30)∂z2∂α1=∂∂α1sinα2sinα1+α2=−sinα2cosα1+α2sin2α1+α2,(31)∂z2∂α2=∂∂α2sinα2sinα1+α2=sinα1sin2α1+α2,(32)∂z3∂λ1=∂∂λ1tanλ1=1cos2λ1,(33)∂z4∂λ2=∂∂λ2tanλ2=1cos2λ2.All the other partial derivatives in the Jacobian matrixT are zeroes; thus, finally this matrix is
(34)T=sinα2sin2α1+α2−sinα1cosα1+α2sin2α1+α200−sinα2cosα1+α2sin2α1+α2sinα1sin2α1+α200001cos2λ100001cos2λ2,and it should be recalculated for every new set of the measured azimuth and elevation angles. The WLS algorithm can be summarized as follows:
(1)
Assume an initial estimated target position vectorx^=x^z^T in a similar manner as in the OLS algorithm
(2)
Calculate the expected measurement vectorz^=hx^
(3)
Calculate the Jacobian matrixH (Equation (23))
(4)
Calculate the Jacobian matrixT (Equation (34))
(5)
Calculate the covariance matrixR (Equation (26))
(6)
Calculate the estimation errors vectorΔx^, weighting Δz^ in accordance with the assumed accuracy of measurements given by the matrix R:
(35)Δz^=z^−z,(36)Δx^=HTR−1H−1HTR−1Δz^.
(7)
Improve the previous estimate ofx^ by subtracting the calculated correcting term Δx^:
(37)x^←x^−Δx^.
(8)
Repeat steps 2 to 7 untilΔx^ falls below a predefined threshold Δx^≤Δx^tr


### 3.3. EKF Algorithm

The OLS and WLS algorithms are simple, but they do not use information from the previous steps of data processing nor do they comprise any filtering. In contrast, the EKF presented in this section realizes model-based data filtering [1, 3]. Apart from the measurement model, based on the same geometrical relationships as in OLS or WLS, the EKF requires also modelling of the dynamics of ICP-89 and uses this model for prediction of its positions and other parameters of motion. This way, it preserves past information gained in processing of the older measurement data, and thanks to the use of more information, it is expected to be more accurate than any single-point algorithm. Moreover, in case of outages or incomplete measurement data, the EKF still provides predicted or filtered estimates of reduced accuracy, this way filling the gaps in the missing measurement history, whereas the OLS and WLS do not output any solution in such a case.


#### 3.3.1. Dynamics Model

The use of EKF requires formulation of the dynamics model of the ICP-89 motion in the following form [1]:
(38)ẋt=fxt+Gtut,where f is a nonlinear function, describing changes of the target position and its parameters of flight, x is the state vector, containing variables describing the flight of the ICP-89, G is the matrix of process disturbances, and u is the vector of process disturbances.The elaboration of the dynamics model for the aerial target imitator ICP-89 requires consideration of its physical properties and its way of operation as well as properties of the atmosphere. The photography of the imitator at the initial phase of flight during firing tests is shown in Figure3.Figure 3
ICP-89 aerial target imitator approximately 20 msec after being launched.During tests, the imitator is launched from the firing stand due to the thrust force generated by its solid propellant. The propellant burns for approximately 0.7 seconds, and for the rest of the flight, the target moves without propulsion or control and follows a free-flight motion pattern. Therefore, the dynamics model of the considered system should be based on equations describing motion of a ballistic object (rocket) [10, 11]. A full description of such movement consists of six degrees of freedom (6-DOF) equations of motion (EOM), comprising linear and rotational components in the OXYZ frame of reference [11]. From the previous considerations, however, we may conclude that the target motion can be restricted to three degrees of freedom (3-DOF) movement in the vertical plane OXZ. ICP-89 in 2D space is illustrated in Figure 4.Figure 4
ICP-89 imitator outline in 2D space with essential variables describing its movement.The 3-DOF EOM are derived from Newton’s second law of dynamics and assume that the motion of the target of a massm is caused by external forces acting on it, which include gravity (weight) Fg=mg and aerodynamical forces: thrust FT, drag Fd, and lift Fl. The resultant model is nonlinear and can be very sophisticated; therefore, in practice it is often significantly simplified based on engineering experience. A frequently applied simplified set of 3-DOF EOM is as follows [5, 10, 11]:
(39)ẋ=vcosγ,(40)ż=vsinγ,(41)v̇=gFgFTcosα+γ−Fd−Fgsinγ,(42)γ̇=gFgvFTsinα+γ+Fl−Fgcosγ,(43)Fġ=−CfFT,where x is the horizontal coordinate in OXZ, z is the vertical coordinate in OXZ, v is the target velocity, γ is the pitch angle, α is the angle of attack, Fg,FT,Fd,Fl are external forces (weight, thrust, drag, and lift), Cf is the fuel consumption coefficient, and g is the gravitational acceleration.The drag and lift forces depend on the squared target velocity with respect to the airv, air density ρ, equivalent surface of the target S, and dimensionless drag Cd and lift Cl coefficients:
(44)Fd=12CdSρv2,(45)Fl=12ClSρv2.If we consider that, the first valid measurements of azimuth and elevation angles from the EOTS system become available when the target already moves without propulsion, i.e.,FT=0; the above model given by Equations (39)–(45) can be further simplified. As ICP-89 in its free-flight phase of motion does not consume fuel and does not change its weight, Equation (43) can be eliminated, and the following dynamics model can be formulated:
(46)ẋ=vcosγ,(47)ż=vsinγ,(48)v̇=gmg−Fd−mgsinγ=−12mCdSρv2−gsinγ,(49)γ̇=gmgvFl−mgcosγ=12mClSρv−gvcosγ.Equations (46)–(49) describe an undisturbed target motion in the OXZ plane. In practice, this ideal motion would be disturbed by unknown factors which can be modelled as stochastic processes. The dominant random effects are connected with the moving atmosphere.If we augment the undisturbed model of the target motion, taking into account the influence of wind, the changes of the target position are given as follows [10]:
(50)ẋ=vcosγ+wx,(51)ż=vsinγ+wz,where wx is the horizontal velocity of the wind and wz is the vertical velocity of the wind.The changeable character of wind influences not only kinematic equations of position (Equations (50)–(51)) but also the equations for velocity and pitch angle as follows [7]:
(52)v̇=−12mCdSρv2−gsinγ−ẇxcosγ+ẇzsinγ,(53)γ̇=12mClSρv−gvcosγ+1vẇxsinγ−ẇzcosγ.There exist various sophisticated stochastic models of wind, which generally assume existence of strong spatial and temporal correlation of wind velocity [12]. However, the usefulness of such models is clearly visible only in estimation of long-term effects of the atmospheric mass movements, e.g., in prediction of power generated by wind farms [13]. Their application in the proposed system would significantly complicate the equations, without guarantee of significant improvements of the EKF accuracy. For this reason, authors of some publications neglect the influence of atmospheric mass movement or assume a constant wind model for the whole period of flight [14].Instead, we propose using a simple stochastic model of wind, which takes into account the random temporal and spatial variability of wind velocity. Our model, however, is only an approximation as it does not consider a correlation which normally exists between the horizontal and vertical velocity of wind fluctuations. The proposed model assumes that the horizontal and vertical components of wind are two independent Wiener processes of unknown initial values, modelled as follows [1]:
(54)ẇx=uwx,(55)ẇz=uwz,where uwx and uwz are the independent Gaussian white noises with power spectral densities Swx and Swz.Based on the above assumptions and considerations, we finally obtain the following continuous dynamic model of the target motion, which has the standard form required by the theory and suitable for immediate use in the EKF algorithm:(56)ẋżv̇γ̇ẇxẇz⏟ẋt=vcosγ+wxvsinγ+wz−12mCdSρv2−gsinγ12mClSρv−gvcosγ00⏟+fxt00−cosγ1vsinγ1000−sinγ−1vcosγ01⏟Gtuwxuwz⏟ut.It is worth noticing that in contrast to the OLS and WLS, the variables in the state vectorx include not only target coordinates x,z in the OXZ plane but also other variables, such as velocity v, orientation (pitch angle) γ, and wind velocity components, which influence future states of the object, and therefore, their knowledge is necessary for appropriate prediction of the future target positions and parameters of motion. Thus, the state vector estimated by the EKF contains 6 variables and it is as follows:
(57)x=x,z,v,γ,wx,wzT.The above continuous model given by Equation (56) can be used directly for propagation ahead of the state vector x. Such an operation requires use of one of the methods of numerical integration [15, 16] and is usually realized as a part of data processing in the EKF algorithm [1]. The EKF, however, calculates also error covariance matrices, and these operations require linearization of the nonlinear dynamics model. Moreover, the Kalman filter is a discrete-time algorithm and its equations are based on a discrete version of the dynamics model. For the above reasons, it is necessary to perform linearization and discretization (sampling) of the dynamics model.The linearization is realized through calculation of the Jacobian matrixF=∂fx/∂xx=x^ of the nonlinear function f, at the latest estimated state vector x^ [1]. This yields the following, so-called, fundamental matrix of the system:
(58)F=∂fx∂xx=x^=00cosγ^−v^sinγ^1000sinγ^v^cosγ^0100−CdSρv^m−gcosγ^0000ClSρ2m+gcosγ^v^2gsinγ^v^00000000000000.The model sampling consists in calculation of two matrices: the state transition matrixΦk+1,k and the covariance matrix of discrete process disturbances Qk. The transition matrix describes the state propagation for one sampling period Δt ahead, from a time step k to the next step of calculation k+1. There exist several methods of its calculation based on the known fundamental matrix F [1]. In the presented algorithm, it was calculated as follows:
(59)Φk+1,k=eFΔt≈I+FΔt+FΔt2!2.The covariance matrix of discrete process disturbancesQk represents the uncertainty introduced to the previous knowledge of the state vector at a time step k by cumulative influence of the vector of continuous process disturbances u in the period Δt, between the time steps k and k+1. Various methods of calculation of this matrix, based on the covariance matrix of continuous process disturbances Qc=EuuT, exist [1]. In the presented algorithm, it was calculated as follows:
(60)Qc=EuuT=Swx00Swz,(61)Qc1=GQcGT,(62)Qk≈Qc1Δt+FQc1+Qc1FTΔt22+F2Qc1+2FQc1FT+Qc1FT2Δt36+F3Qc1+3F2Qc1FT+3FQc1FT2+Qc1FT3Δt424.


#### 3.3.2. Measurement Model

The measurement model used in the EKF is given by Equation (11) and is the same as the one used in OLS and WLS. Some EKF equations require additional knowledge of the Jacobian matrix H=∂hx/∂xx=x^ of the nonlinear function h∗. This matrix can be calculated similarly to the Jacobian matrix used in the OLS, given by Equations (15)–(23). However, it is necessary to take into account that the state vector in the EKF consists of six states, whereas in the OLS of only two states, thus the number of derivatives and the size of the H matrix will be different:
(63)H=∂hx∂xx=x^=∂z1∂x∂z1∂z∂z1∂v∂z1∂γ∂z1∂wx∂z1∂wz∂z2∂x∂z2∂z∂z2∂v∂z2∂γ∂z2∂wx∂z2∂wz∂z3∂x∂z3∂z∂z3∂v∂z3∂γ∂z3∂wx∂z3∂wz∂z4∂x∂z4∂z∂z4∂v∂z4∂γ∂z4∂wx∂z4∂wzx=x^.Finally, it is clear from Equation (11) that all the derivatives with respect to v,γ,wx,wz in Equation (63) are zeroes, and on the basis of already calculated derivatives over x and z given by Equations (16)–(22), the H matrix in the EKF is stated as follows:
(64)H=x^−x2x^−x22+y22x1−x22+y1−y2200000x^−x1x^−x12+y12x1−x22+y1−y2200000−z^x^−x1x^−x12+y1231x^−x12+y120000−z^x^−x2x^−x22+y2231x^−x22+y220000.The covariance matrix of measurement errorsR in the EKF is calculated the same way as in the WLS algorithm, with the use of Equations (26) and (34).


#### 3.3.3. Algorithm of Filtration

Estimation of the state vectorx, changing with time in accordance with the above formulated dynamics model (Equation (56)), based on the measurement vector z, related to the state vector via the measurement model (Equation (11)), can be realized by a nonlinear suboptimal filter [3, 5]. An example of such a filter is the EKF, implemented for that purpose by the authors of this paper. The EKF algorithm can be summarized as follows:
(1)
Assume an initial estimated state vectorx^0∣0. The target position can be initialized in a similar manner as in the OLS and WLS. The rest of the states can be initially assumed to be zeroes
(2)
Assume an initial value of the covariance matrix of filtration errorsP0∣0. Typically, it is initialized as a diagonal matrix with large diagonal values, modelling uncertainty of the initial knowledge of the state vector
(3)
Calculate the predicted state vectorx^k+1∣k at a time step t=k+1Δt, through the numerical integration of the deterministic part of the dynamics model ẋt=fxt, subject to the initial condition xt=x^k∣k at a time step t=kΔt:
(65)x^k+1∣k=∫kΔtk+1Δtfxtdt.
(4)
Calculate the Jacobian matrixF (Equation (58)), for x=x^k∣k
(5)
Calculate the state transition matrixΦk+1,k of the discrete version of the dynamics model (Equation (59)).
(6)
Calculate the covariance matrix of discrete process disturbancesQk (Equations (60)–(62)). The Qk matrix must be recalculated at each step due to changing values of the Jacobian matrix F
(7)
Calculate the covariance matrix of prediction errors:(66)Pk+1∣k=Φk+1,kPk∣kΦTk+1,k+Qk.
(8)
Calculate the Jacobian matrixHk+1 for x=x^k+1∣k (from Equation (64)).
(9)
Calculate the Jacobian matrixTk+1 for the last acquired measurement vector zk+1 (from Equation (34)).
(10)
Calculate the covariance matrixRk+1 (from Equation (26)).
(11)
Calculate the Kalman gain matrixKk+1:
(67)Kk+1=Pk+1∣kHTk+1Hk+1Pk+1∣kHTk+1+Rk+1−1.
(12)
Calculate the corrected state vectorx^k+1∣k+1 at a time step t=k+1Δt, by fusing the predicted state vector x^k+1∣k and the current measurement vector zk+1:
(68)x^k+1∣k+1=x^k+1∣k+Kk+1zk+1−hx^k+1∣k.
(13)
Calculate the covariance matrix of filtration errors:(69)Pk+1∣k+1=I−Kk+1Hk+1Pk+1∣k.
(14)
Recursively repeat steps 3rd to 13thThe above set of steps realized during filtration can be interpreted as the initialization (steps 1st and 2nd), the time update or prediction (steps 3rd to 7th), and the measurement update or filtration (steps 8th to 13th). The initialization is executed only once at the beginning of the filter operation, whereas the steps of prediction and filtration are executed recursively for the whole required period of the EKF operation.


## 3.1. OLS Algorithm

The primary measurements realized in the EOTS measurement system are azimuth and elevation angles, which are obtained simultaneously, with a period of about 33 msec. For every time instance, they can be grouped into the primary measurement vectorz′ as follows:
(9)z′=α1α2λ1λ2T.As the relationship between the original measurement vectorz′ and the unknown target position x,z in OXZ is nontrivial, we propose here to construct a new measurement vector z as follows:
(10)z=tz′=sinα1sinα1+α2sinα2sinα1+α2tanλ1tanλ2,where t∗ is a transformation function of the primary (original) measurement vector z′ to the new form represented by the measurement vector z. The transformed vector z and the unknown state vector of the target position x=xzT are related by a nonlinear, but relatively simple formula z=hx+v, which can be deduced from Equation (8):
(11)sinα1sinα1+α2sinα2sinα1+α2tanλ1tanλ2⏟z=x−x22+y22x1−x22+y1−y22x−x12+y12x1−x22+y1−y22zx−x12+y12zx−x22+y22⏟hx+v.The above equation, apart from the geometrical relationships, includes also a vector of measurement errorsv, modelling limited accuracy of the measurement vector z. Equation (11) can be approximately solved with respect to the unknown position vector x using the OLS algorithm [6–9] as follows:
(1)
Assume an initial estimated target position vectorx^=x^z^T. This can be almost any reasonable guess, but the nearer it is to the true target position, the fewer iterations of OLS are necessary to achieve its required accuracy. The first guess can be based on knowledge of a typical target position, where the first EOTS images and angular data become available during firing tests. The subsequent guesses can be based on the previous target positions, estimated for the previous measurement vector z
(2)
Calculate the expected measurement vectorz^=hx^
(3)
Calculate the Jacobian matrixH=∂hx/∂xx=x^ of the nonlinear transformation z=hx, at the latest estimated target position x^. This yields the following linearized measurement equation:
(12)Δz^=z^−z=hx^−hx−v≈Hx^−x−v=HΔx^−v.
(4)
Solve Equation (12) and calculate a vector of estimation errors Δx^. For a full-rank matrix H, Δx^ is calculated with the use of the Moore-Penrose matrix pseudoinverse:
(13)Δx^=HTH−1HTΔz^.
(5)
Improve the previous estimate ofx^ by subtracting the calculated correcting term Δx^:
(14)x^←x^−Δx^.
(6)
Repeat steps 2 to 5 untilΔx^ falls below a predefined threshold Δx^≤Δx^trThe above OLS algorithm is run for every newly acquired measurement vectorz. The Jacobian matrix H in this algorithm is obtained as a matrix of partial derivatives of elements of the measurement vector z with respect to the elements of x, and they can be calculated based on the relationships given in Equation (11) as follows:
(15)H=∂hx∂xx=x^=∂z1∂x∂z1∂z∂z2∂x∂z2∂z∂z3∂x∂z3∂z∂z4∂x∂z4∂zx=x^,(16)∂z1∂xx=x^=∂∂xx−x22+y22x1−x22+y1−y22x=x^=x^−x2x^−x22+y22x1−x22+y1−y22,(17)∂z2∂xx=x^=∂∂xx−x12+y22x1−x22+y1−y22x=x^=x^−x1x^−x12+y12x1−x22+y1−y22,(18)∂z3∂xx=x^=∂∂xzx−x12+y12x=x^=−z^x^−x1x^−x12+y123,(19)∂z4∂xx=x^=∂∂xzx−x22+y22x=x^=−z^x^−x2x^−x22+y223,(20)∂z1∂zx=x^=∂z2∂zx=x^=0,(21)∂z3∂zx=x^=∂∂zzx−x12+y12x=x^=1x^−x12+y12,(22)∂z4∂zx=x^=∂∂zzx−x22+y22x=x^=1x^−x22+y22.Finally, the Jacobian matrixH is as follows:
(23)H=x^−x2x^−x22+y22x1−x22+y1−y220x^−x1x^−x12+y12x1−x22+y1−y220−z^x^−x1x^−x12+y1231x^−x12+y12−z^x^−x2x^−x22+y2231x^−x22+y22.


## 3.2. WLS Algorithm

The WLS algorithm is another single-point solution method [6–9]. It contains the same steps as the OLS, but it additionally weights the measurements depending on their expected accuracy, which improves the accuracy of estimation if the accuracies of measurements are different. If the standard deviations of angle measurement errors are known and given as σα1,σα2,σλ1,σλ2, and there is no cross-correlation of these errors, the following measurement errors covariance matrix R′ can be written down for the primary measurement vector z′:
(24)R′=Ev′v′T=σα120000σα220000σλ120000σλ22.As the measurement vectorz is obtained through the transformation z=tz′ of the primary measurement vector z′, the measurement errors of azimuth and elevation angles v′ also undergo a transformation, which can be obtained through a linearization of the function t∗ and realized as a multiplication of the v′ vector through the Jacobian matrix T:
(25)v=Tv′=∂tz′∂z′v′.Therefore, the covariance matrixR of the artificially constructed measurement vector z is also calculated through a transformation of the covariance matrix of the primary measurement errors R′ as follows:
(26)R=EvvT=ETv′Tv′T=ETv′v′TTT=T·Ev′v′T·TT=TR′TT.The Jacobian matrixT is obtained as a matrix of partial derivatives of elements of the measurement vector z with respect to the elements of the primary measurement vector z′, which are calculated as follows:
(27)T=∂tz′∂z′=∂z1∂α1∂z2∂α1∂z3∂α1∂z4∂α1∂z1∂α2∂z2∂α2∂z3∂α2∂z4∂α2∂z1∂λ1∂z2∂λ1∂z3∂λ1∂z4∂λ1∂z1∂λ2∂z2∂λ2∂z3∂λ2∂z4∂λ2,(28)∂z1∂α1=∂∂α1sinα1sinα1+α2=sinα2sin2α1+α2,(29)∂z1∂α2=∂∂α2sinα1sinα1+α2=−sinα1cosα1+α2sin2α1+α2,(30)∂z2∂α1=∂∂α1sinα2sinα1+α2=−sinα2cosα1+α2sin2α1+α2,(31)∂z2∂α2=∂∂α2sinα2sinα1+α2=sinα1sin2α1+α2,(32)∂z3∂λ1=∂∂λ1tanλ1=1cos2λ1,(33)∂z4∂λ2=∂∂λ2tanλ2=1cos2λ2.All the other partial derivatives in the Jacobian matrixT are zeroes; thus, finally this matrix is
(34)T=sinα2sin2α1+α2−sinα1cosα1+α2sin2α1+α200−sinα2cosα1+α2sin2α1+α2sinα1sin2α1+α200001cos2λ100001cos2λ2,and it should be recalculated for every new set of the measured azimuth and elevation angles. The WLS algorithm can be summarized as follows:
(1)
Assume an initial estimated target position vectorx^=x^z^T in a similar manner as in the OLS algorithm
(2)
Calculate the expected measurement vectorz^=hx^
(3)
Calculate the Jacobian matrixH (Equation (23))
(4)
Calculate the Jacobian matrixT (Equation (34))
(5)
Calculate the covariance matrixR (Equation (26))
(6)
Calculate the estimation errors vectorΔx^, weighting Δz^ in accordance with the assumed accuracy of measurements given by the matrix R:
(35)Δz^=z^−z,(36)Δx^=HTR−1H−1HTR−1Δz^.
(7)
Improve the previous estimate ofx^ by subtracting the calculated correcting term Δx^:
(37)x^←x^−Δx^.
(8)
Repeat steps 2 to 7 untilΔx^ falls below a predefined threshold Δx^≤Δx^tr


## 3.3. EKF Algorithm

The OLS and WLS algorithms are simple, but they do not use information from the previous steps of data processing nor do they comprise any filtering. In contrast, the EKF presented in this section realizes model-based data filtering [1, 3]. Apart from the measurement model, based on the same geometrical relationships as in OLS or WLS, the EKF requires also modelling of the dynamics of ICP-89 and uses this model for prediction of its positions and other parameters of motion. This way, it preserves past information gained in processing of the older measurement data, and thanks to the use of more information, it is expected to be more accurate than any single-point algorithm. Moreover, in case of outages or incomplete measurement data, the EKF still provides predicted or filtered estimates of reduced accuracy, this way filling the gaps in the missing measurement history, whereas the OLS and WLS do not output any solution in such a case.


### 3.3.1. Dynamics Model

The use of EKF requires formulation of the dynamics model of the ICP-89 motion in the following form [1]:
(38)ẋt=fxt+Gtut,where f is a nonlinear function, describing changes of the target position and its parameters of flight, x is the state vector, containing variables describing the flight of the ICP-89, G is the matrix of process disturbances, and u is the vector of process disturbances.The elaboration of the dynamics model for the aerial target imitator ICP-89 requires consideration of its physical properties and its way of operation as well as properties of the atmosphere. The photography of the imitator at the initial phase of flight during firing tests is shown in Figure3.Figure 3
ICP-89 aerial target imitator approximately 20 msec after being launched.During tests, the imitator is launched from the firing stand due to the thrust force generated by its solid propellant. The propellant burns for approximately 0.7 seconds, and for the rest of the flight, the target moves without propulsion or control and follows a free-flight motion pattern. Therefore, the dynamics model of the considered system should be based on equations describing motion of a ballistic object (rocket) [10, 11]. A full description of such movement consists of six degrees of freedom (6-DOF) equations of motion (EOM), comprising linear and rotational components in the OXYZ frame of reference [11]. From the previous considerations, however, we may conclude that the target motion can be restricted to three degrees of freedom (3-DOF) movement in the vertical plane OXZ. ICP-89 in 2D space is illustrated in Figure 4.Figure 4
ICP-89 imitator outline in 2D space with essential variables describing its movement.The 3-DOF EOM are derived from Newton’s second law of dynamics and assume that the motion of the target of a massm is caused by external forces acting on it, which include gravity (weight) Fg=mg and aerodynamical forces: thrust FT, drag Fd, and lift Fl. The resultant model is nonlinear and can be very sophisticated; therefore, in practice it is often significantly simplified based on engineering experience. A frequently applied simplified set of 3-DOF EOM is as follows [5, 10, 11]:
(39)ẋ=vcosγ,(40)ż=vsinγ,(41)v̇=gFgFTcosα+γ−Fd−Fgsinγ,(42)γ̇=gFgvFTsinα+γ+Fl−Fgcosγ,(43)Fġ=−CfFT,where x is the horizontal coordinate in OXZ, z is the vertical coordinate in OXZ, v is the target velocity, γ is the pitch angle, α is the angle of attack, Fg,FT,Fd,Fl are external forces (weight, thrust, drag, and lift), Cf is the fuel consumption coefficient, and g is the gravitational acceleration.The drag and lift forces depend on the squared target velocity with respect to the airv, air density ρ, equivalent surface of the target S, and dimensionless drag Cd and lift Cl coefficients:
(44)Fd=12CdSρv2,(45)Fl=12ClSρv2.If we consider that, the first valid measurements of azimuth and elevation angles from the EOTS system become available when the target already moves without propulsion, i.e.,FT=0; the above model given by Equations (39)–(45) can be further simplified. As ICP-89 in its free-flight phase of motion does not consume fuel and does not change its weight, Equation (43) can be eliminated, and the following dynamics model can be formulated:
(46)ẋ=vcosγ,(47)ż=vsinγ,(48)v̇=gmg−Fd−mgsinγ=−12mCdSρv2−gsinγ,(49)γ̇=gmgvFl−mgcosγ=12mClSρv−gvcosγ.Equations (46)–(49) describe an undisturbed target motion in the OXZ plane. In practice, this ideal motion would be disturbed by unknown factors which can be modelled as stochastic processes. The dominant random effects are connected with the moving atmosphere.If we augment the undisturbed model of the target motion, taking into account the influence of wind, the changes of the target position are given as follows [10]:
(50)ẋ=vcosγ+wx,(51)ż=vsinγ+wz,where wx is the horizontal velocity of the wind and wz is the vertical velocity of the wind.The changeable character of wind influences not only kinematic equations of position (Equations (50)–(51)) but also the equations for velocity and pitch angle as follows [7]:
(52)v̇=−12mCdSρv2−gsinγ−ẇxcosγ+ẇzsinγ,(53)γ̇=12mClSρv−gvcosγ+1vẇxsinγ−ẇzcosγ.There exist various sophisticated stochastic models of wind, which generally assume existence of strong spatial and temporal correlation of wind velocity [12]. However, the usefulness of such models is clearly visible only in estimation of long-term effects of the atmospheric mass movements, e.g., in prediction of power generated by wind farms [13]. Their application in the proposed system would significantly complicate the equations, without guarantee of significant improvements of the EKF accuracy. For this reason, authors of some publications neglect the influence of atmospheric mass movement or assume a constant wind model for the whole period of flight [14].Instead, we propose using a simple stochastic model of wind, which takes into account the random temporal and spatial variability of wind velocity. Our model, however, is only an approximation as it does not consider a correlation which normally exists between the horizontal and vertical velocity of wind fluctuations. The proposed model assumes that the horizontal and vertical components of wind are two independent Wiener processes of unknown initial values, modelled as follows [1]:
(54)ẇx=uwx,(55)ẇz=uwz,where uwx and uwz are the independent Gaussian white noises with power spectral densities Swx and Swz.Based on the above assumptions and considerations, we finally obtain the following continuous dynamic model of the target motion, which has the standard form required by the theory and suitable for immediate use in the EKF algorithm:(56)ẋżv̇γ̇ẇxẇz⏟ẋt=vcosγ+wxvsinγ+wz−12mCdSρv2−gsinγ12mClSρv−gvcosγ00⏟+fxt00−cosγ1vsinγ1000−sinγ−1vcosγ01⏟Gtuwxuwz⏟ut.It is worth noticing that in contrast to the OLS and WLS, the variables in the state vectorx include not only target coordinates x,z in the OXZ plane but also other variables, such as velocity v, orientation (pitch angle) γ, and wind velocity components, which influence future states of the object, and therefore, their knowledge is necessary for appropriate prediction of the future target positions and parameters of motion. Thus, the state vector estimated by the EKF contains 6 variables and it is as follows:
(57)x=x,z,v,γ,wx,wzT.The above continuous model given by Equation (56) can be used directly for propagation ahead of the state vector x. Such an operation requires use of one of the methods of numerical integration [15, 16] and is usually realized as a part of data processing in the EKF algorithm [1]. The EKF, however, calculates also error covariance matrices, and these operations require linearization of the nonlinear dynamics model. Moreover, the Kalman filter is a discrete-time algorithm and its equations are based on a discrete version of the dynamics model. For the above reasons, it is necessary to perform linearization and discretization (sampling) of the dynamics model.The linearization is realized through calculation of the Jacobian matrixF=∂fx/∂xx=x^ of the nonlinear function f, at the latest estimated state vector x^ [1]. This yields the following, so-called, fundamental matrix of the system:
(58)F=∂fx∂xx=x^=00cosγ^−v^sinγ^1000sinγ^v^cosγ^0100−CdSρv^m−gcosγ^0000ClSρ2m+gcosγ^v^2gsinγ^v^00000000000000.The model sampling consists in calculation of two matrices: the state transition matrixΦk+1,k and the covariance matrix of discrete process disturbances Qk. The transition matrix describes the state propagation for one sampling period Δt ahead, from a time step k to the next step of calculation k+1. There exist several methods of its calculation based on the known fundamental matrix F [1]. In the presented algorithm, it was calculated as follows:
(59)Φk+1,k=eFΔt≈I+FΔt+FΔt2!2.The covariance matrix of discrete process disturbancesQk represents the uncertainty introduced to the previous knowledge of the state vector at a time step k by cumulative influence of the vector of continuous process disturbances u in the period Δt, between the time steps k and k+1. Various methods of calculation of this matrix, based on the covariance matrix of continuous process disturbances Qc=EuuT, exist [1]. In the presented algorithm, it was calculated as follows:
(60)Qc=EuuT=Swx00Swz,(61)Qc1=GQcGT,(62)Qk≈Qc1Δt+FQc1+Qc1FTΔt22+F2Qc1+2FQc1FT+Qc1FT2Δt36+F3Qc1+3F2Qc1FT+3FQc1FT2+Qc1FT3Δt424.


### 3.3.2. Measurement Model

The measurement model used in the EKF is given by Equation (11) and is the same as the one used in OLS and WLS. Some EKF equations require additional knowledge of the Jacobian matrix H=∂hx/∂xx=x^ of the nonlinear function h∗. This matrix can be calculated similarly to the Jacobian matrix used in the OLS, given by Equations (15)–(23). However, it is necessary to take into account that the state vector in the EKF consists of six states, whereas in the OLS of only two states, thus the number of derivatives and the size of the H matrix will be different:
(63)H=∂hx∂xx=x^=∂z1∂x∂z1∂z∂z1∂v∂z1∂γ∂z1∂wx∂z1∂wz∂z2∂x∂z2∂z∂z2∂v∂z2∂γ∂z2∂wx∂z2∂wz∂z3∂x∂z3∂z∂z3∂v∂z3∂γ∂z3∂wx∂z3∂wz∂z4∂x∂z4∂z∂z4∂v∂z4∂γ∂z4∂wx∂z4∂wzx=x^.Finally, it is clear from Equation (11) that all the derivatives with respect to v,γ,wx,wz in Equation (63) are zeroes, and on the basis of already calculated derivatives over x and z given by Equations (16)–(22), the H matrix in the EKF is stated as follows:
(64)H=x^−x2x^−x22+y22x1−x22+y1−y2200000x^−x1x^−x12+y12x1−x22+y1−y2200000−z^x^−x1x^−x12+y1231x^−x12+y120000−z^x^−x2x^−x22+y2231x^−x22+y220000.The covariance matrix of measurement errorsR in the EKF is calculated the same way as in the WLS algorithm, with the use of Equations (26) and (34).


### 3.3.3. Algorithm of Filtration

Estimation of the state vectorx, changing with time in accordance with the above formulated dynamics model (Equation (56)), based on the measurement vector z, related to the state vector via the measurement model (Equation (11)), can be realized by a nonlinear suboptimal filter [3, 5]. An example of such a filter is the EKF, implemented for that purpose by the authors of this paper. The EKF algorithm can be summarized as follows:
(1)
Assume an initial estimated state vectorx^0∣0. The target position can be initialized in a similar manner as in the OLS and WLS. The rest of the states can be initially assumed to be zeroes
(2)
Assume an initial value of the covariance matrix of filtration errorsP0∣0. Typically, it is initialized as a diagonal matrix with large diagonal values, modelling uncertainty of the initial knowledge of the state vector
(3)
Calculate the predicted state vectorx^k+1∣k at a time step t=k+1Δt, through the numerical integration of the deterministic part of the dynamics model ẋt=fxt, subject to the initial condition xt=x^k∣k at a time step t=kΔt:
(65)x^k+1∣k=∫kΔtk+1Δtfxtdt.
(4)
Calculate the Jacobian matrixF (Equation (58)), for x=x^k∣k
(5)
Calculate the state transition matrixΦk+1,k of the discrete version of the dynamics model (Equation (59)).
(6)
Calculate the covariance matrix of discrete process disturbancesQk (Equations (60)–(62)). The Qk matrix must be recalculated at each step due to changing values of the Jacobian matrix F
(7)
Calculate the covariance matrix of prediction errors:(66)Pk+1∣k=Φk+1,kPk∣kΦTk+1,k+Qk.
(8)
Calculate the Jacobian matrixHk+1 for x=x^k+1∣k (from Equation (64)).
(9)
Calculate the Jacobian matrixTk+1 for the last acquired measurement vector zk+1 (from Equation (34)).
(10)
Calculate the covariance matrixRk+1 (from Equation (26)).
(11)
Calculate the Kalman gain matrixKk+1:
(67)Kk+1=Pk+1∣kHTk+1Hk+1Pk+1∣kHTk+1+Rk+1−1.
(12)
Calculate the corrected state vectorx^k+1∣k+1 at a time step t=k+1Δt, by fusing the predicted state vector x^k+1∣k and the current measurement vector zk+1:
(68)x^k+1∣k+1=x^k+1∣k+Kk+1zk+1−hx^k+1∣k.
(13)
Calculate the covariance matrix of filtration errors:(69)Pk+1∣k+1=I−Kk+1Hk+1Pk+1∣k.
(14)
Recursively repeat steps 3rd to 13thThe above set of steps realized during filtration can be interpreted as the initialization (steps 1st and 2nd), the time update or prediction (steps 3rd to 7th), and the measurement update or filtration (steps 8th to 13th). The initialization is executed only once at the beginning of the filter operation, whereas the steps of prediction and filtration are executed recursively for the whole required period of the EKF operation.


## 3.3.1. Dynamics Model

The use of EKF requires formulation of the dynamics model of the ICP-89 motion in the following form [1]:
(38)ẋt=fxt+Gtut,where f is a nonlinear function, describing changes of the target position and its parameters of flight, x is the state vector, containing variables describing the flight of the ICP-89, G is the matrix of process disturbances, and u is the vector of process disturbances.The elaboration of the dynamics model for the aerial target imitator ICP-89 requires consideration of its physical properties and its way of operation as well as properties of the atmosphere. The photography of the imitator at the initial phase of flight during firing tests is shown in Figure3.Figure 3
ICP-89 aerial target imitator approximately 20 msec after being launched.During tests, the imitator is launched from the firing stand due to the thrust force generated by its solid propellant. The propellant burns for approximately 0.7 seconds, and for the rest of the flight, the target moves without propulsion or control and follows a free-flight motion pattern. Therefore, the dynamics model of the considered system should be based on equations describing motion of a ballistic object (rocket) [10, 11]. A full description of such movement consists of six degrees of freedom (6-DOF) equations of motion (EOM), comprising linear and rotational components in the OXYZ frame of reference [11]. From the previous considerations, however, we may conclude that the target motion can be restricted to three degrees of freedom (3-DOF) movement in the vertical plane OXZ. ICP-89 in 2D space is illustrated in Figure 4.Figure 4
ICP-89 imitator outline in 2D space with essential variables describing its movement.The 3-DOF EOM are derived from Newton’s second law of dynamics and assume that the motion of the target of a massm is caused by external forces acting on it, which include gravity (weight) Fg=mg and aerodynamical forces: thrust FT, drag Fd, and lift Fl. The resultant model is nonlinear and can be very sophisticated; therefore, in practice it is often significantly simplified based on engineering experience. A frequently applied simplified set of 3-DOF EOM is as follows [5, 10, 11]:
(39)ẋ=vcosγ,(40)ż=vsinγ,(41)v̇=gFgFTcosα+γ−Fd−Fgsinγ,(42)γ̇=gFgvFTsinα+γ+Fl−Fgcosγ,(43)Fġ=−CfFT,where x is the horizontal coordinate in OXZ, z is the vertical coordinate in OXZ, v is the target velocity, γ is the pitch angle, α is the angle of attack, Fg,FT,Fd,Fl are external forces (weight, thrust, drag, and lift), Cf is the fuel consumption coefficient, and g is the gravitational acceleration.The drag and lift forces depend on the squared target velocity with respect to the airv, air density ρ, equivalent surface of the target S, and dimensionless drag Cd and lift Cl coefficients:
(44)Fd=12CdSρv2,(45)Fl=12ClSρv2.If we consider that, the first valid measurements of azimuth and elevation angles from the EOTS system become available when the target already moves without propulsion, i.e.,FT=0; the above model given by Equations (39)–(45) can be further simplified. As ICP-89 in its free-flight phase of motion does not consume fuel and does not change its weight, Equation (43) can be eliminated, and the following dynamics model can be formulated:
(46)ẋ=vcosγ,(47)ż=vsinγ,(48)v̇=gmg−Fd−mgsinγ=−12mCdSρv2−gsinγ,(49)γ̇=gmgvFl−mgcosγ=12mClSρv−gvcosγ.Equations (46)–(49) describe an undisturbed target motion in the OXZ plane. In practice, this ideal motion would be disturbed by unknown factors which can be modelled as stochastic processes. The dominant random effects are connected with the moving atmosphere.If we augment the undisturbed model of the target motion, taking into account the influence of wind, the changes of the target position are given as follows [10]:
(50)ẋ=vcosγ+wx,(51)ż=vsinγ+wz,where wx is the horizontal velocity of the wind and wz is the vertical velocity of the wind.The changeable character of wind influences not only kinematic equations of position (Equations (50)–(51)) but also the equations for velocity and pitch angle as follows [7]:
(52)v̇=−12mCdSρv2−gsinγ−ẇxcosγ+ẇzsinγ,(53)γ̇=12mClSρv−gvcosγ+1vẇxsinγ−ẇzcosγ.There exist various sophisticated stochastic models of wind, which generally assume existence of strong spatial and temporal correlation of wind velocity [12]. However, the usefulness of such models is clearly visible only in estimation of long-term effects of the atmospheric mass movements, e.g., in prediction of power generated by wind farms [13]. Their application in the proposed system would significantly complicate the equations, without guarantee of significant improvements of the EKF accuracy. For this reason, authors of some publications neglect the influence of atmospheric mass movement or assume a constant wind model for the whole period of flight [14].Instead, we propose using a simple stochastic model of wind, which takes into account the random temporal and spatial variability of wind velocity. Our model, however, is only an approximation as it does not consider a correlation which normally exists between the horizontal and vertical velocity of wind fluctuations. The proposed model assumes that the horizontal and vertical components of wind are two independent Wiener processes of unknown initial values, modelled as follows [1]:
(54)ẇx=uwx,(55)ẇz=uwz,where uwx and uwz are the independent Gaussian white noises with power spectral densities Swx and Swz.Based on the above assumptions and considerations, we finally obtain the following continuous dynamic model of the target motion, which has the standard form required by the theory and suitable for immediate use in the EKF algorithm:(56)ẋżv̇γ̇ẇxẇz⏟ẋt=vcosγ+wxvsinγ+wz−12mCdSρv2−gsinγ12mClSρv−gvcosγ00⏟+fxt00−cosγ1vsinγ1000−sinγ−1vcosγ01⏟Gtuwxuwz⏟ut.It is worth noticing that in contrast to the OLS and WLS, the variables in the state vectorx include not only target coordinates x,z in the OXZ plane but also other variables, such as velocity v, orientation (pitch angle) γ, and wind velocity components, which influence future states of the object, and therefore, their knowledge is necessary for appropriate prediction of the future target positions and parameters of motion. Thus, the state vector estimated by the EKF contains 6 variables and it is as follows:
(57)x=x,z,v,γ,wx,wzT.The above continuous model given by Equation (56) can be used directly for propagation ahead of the state vector x. Such an operation requires use of one of the methods of numerical integration [15, 16] and is usually realized as a part of data processing in the EKF algorithm [1]. The EKF, however, calculates also error covariance matrices, and these operations require linearization of the nonlinear dynamics model. Moreover, the Kalman filter is a discrete-time algorithm and its equations are based on a discrete version of the dynamics model. For the above reasons, it is necessary to perform linearization and discretization (sampling) of the dynamics model.The linearization is realized through calculation of the Jacobian matrixF=∂fx/∂xx=x^ of the nonlinear function f, at the latest estimated state vector x^ [1]. This yields the following, so-called, fundamental matrix of the system:
(58)F=∂fx∂xx=x^=00cosγ^−v^sinγ^1000sinγ^v^cosγ^0100−CdSρv^m−gcosγ^0000ClSρ2m+gcosγ^v^2gsinγ^v^00000000000000.The model sampling consists in calculation of two matrices: the state transition matrixΦk+1,k and the covariance matrix of discrete process disturbances Qk. The transition matrix describes the state propagation for one sampling period Δt ahead, from a time step k to the next step of calculation k+1. There exist several methods of its calculation based on the known fundamental matrix F [1]. In the presented algorithm, it was calculated as follows:
(59)Φk+1,k=eFΔt≈I+FΔt+FΔt2!2.The covariance matrix of discrete process disturbancesQk represents the uncertainty introduced to the previous knowledge of the state vector at a time step k by cumulative influence of the vector of continuous process disturbances u in the period Δt, between the time steps k and k+1. Various methods of calculation of this matrix, based on the covariance matrix of continuous process disturbances Qc=EuuT, exist [1]. In the presented algorithm, it was calculated as follows:
(60)Qc=EuuT=Swx00Swz,(61)Qc1=GQcGT,(62)Qk≈Qc1Δt+FQc1+Qc1FTΔt22+F2Qc1+2FQc1FT+Qc1FT2Δt36+F3Qc1+3F2Qc1FT+3FQc1FT2+Qc1FT3Δt424.


## 3.3.2. Measurement Model

The measurement model used in the EKF is given by Equation (11) and is the same as the one used in OLS and WLS. Some EKF equations require additional knowledge of the Jacobian matrix H=∂hx/∂xx=x^ of the nonlinear function h∗. This matrix can be calculated similarly to the Jacobian matrix used in the OLS, given by Equations (15)–(23). However, it is necessary to take into account that the state vector in the EKF consists of six states, whereas in the OLS of only two states, thus the number of derivatives and the size of the H matrix will be different:
(63)H=∂hx∂xx=x^=∂z1∂x∂z1∂z∂z1∂v∂z1∂γ∂z1∂wx∂z1∂wz∂z2∂x∂z2∂z∂z2∂v∂z2∂γ∂z2∂wx∂z2∂wz∂z3∂x∂z3∂z∂z3∂v∂z3∂γ∂z3∂wx∂z3∂wz∂z4∂x∂z4∂z∂z4∂v∂z4∂γ∂z4∂wx∂z4∂wzx=x^.Finally, it is clear from Equation (11) that all the derivatives with respect to v,γ,wx,wz in Equation (63) are zeroes, and on the basis of already calculated derivatives over x and z given by Equations (16)–(22), the H matrix in the EKF is stated as follows:
(64)H=x^−x2x^−x22+y22x1−x22+y1−y2200000x^−x1x^−x12+y12x1−x22+y1−y2200000−z^x^−x1x^−x12+y1231x^−x12+y120000−z^x^−x2x^−x22+y2231x^−x22+y220000.The covariance matrix of measurement errorsR in the EKF is calculated the same way as in the WLS algorithm, with the use of Equations (26) and (34).


## 3.3.3. Algorithm of Filtration

Estimation of the state vectorx, changing with time in accordance with the above formulated dynamics model (Equation (56)), based on the measurement vector z, related to the state vector via the measurement model (Equation (11)), can be realized by a nonlinear suboptimal filter [3, 5]. An example of such a filter is the EKF, implemented for that purpose by the authors of this paper. The EKF algorithm can be summarized as follows:
(1)
Assume an initial estimated state vectorx^0∣0. The target position can be initialized in a similar manner as in the OLS and WLS. The rest of the states can be initially assumed to be zeroes
(2)
Assume an initial value of the covariance matrix of filtration errorsP0∣0. Typically, it is initialized as a diagonal matrix with large diagonal values, modelling uncertainty of the initial knowledge of the state vector
(3)
Calculate the predicted state vectorx^k+1∣k at a time step t=k+1Δt, through the numerical integration of the deterministic part of the dynamics model ẋt=fxt, subject to the initial condition xt=x^k∣k at a time step t=kΔt:
(65)x^k+1∣k=∫kΔtk+1Δtfxtdt.
(4)
Calculate the Jacobian matrixF (Equation (58)), for x=x^k∣k
(5)
Calculate the state transition matrixΦk+1,k of the discrete version of the dynamics model (Equation (59)).
(6)
Calculate the covariance matrix of discrete process disturbancesQk (Equations (60)–(62)). The Qk matrix must be recalculated at each step due to changing values of the Jacobian matrix F
(7)
Calculate the covariance matrix of prediction errors:(66)Pk+1∣k=Φk+1,kPk∣kΦTk+1,k+Qk.
(8)
Calculate the Jacobian matrixHk+1 for x=x^k+1∣k (from Equation (64)).
(9)
Calculate the Jacobian matrixTk+1 for the last acquired measurement vector zk+1 (from Equation (34)).
(10)
Calculate the covariance matrixRk+1 (from Equation (26)).
(11)
Calculate the Kalman gain matrixKk+1:
(67)Kk+1=Pk+1∣kHTk+1Hk+1Pk+1∣kHTk+1+Rk+1−1.
(12)
Calculate the corrected state vectorx^k+1∣k+1 at a time step t=k+1Δt, by fusing the predicted state vector x^k+1∣k and the current measurement vector zk+1:
(68)x^k+1∣k+1=x^k+1∣k+Kk+1zk+1−hx^k+1∣k.
(13)
Calculate the covariance matrix of filtration errors:(69)Pk+1∣k+1=I−Kk+1Hk+1Pk+1∣k.
(14)
Recursively repeat steps 3rd to 13thThe above set of steps realized during filtration can be interpreted as the initialization (steps 1st and 2nd), the time update or prediction (steps 3rd to 7th), and the measurement update or filtration (steps 8th to 13th). The initialization is executed only once at the beginning of the filter operation, whereas the steps of prediction and filtration are executed recursively for the whole required period of the EKF operation.


## 4. Simulation Methodology and Results

After formulating the dynamics and the observation model of the EOTS tracking system, it is possible to run the simulation and analyze the operation of designed estimation algorithms. For the purpose of comparison, two of the presented algorithms, i.e., OLS and EKF, were implemented and tested in the Matlab® scientific environment. The WLS algorithm was not implemented, as the standard deviations of all angle measurements realized in the EOTS are the same; thus, the accuracy offered by WLS should be the same as for OLS.The appropriate choice of simulation parameters is crucial if we want to obtain valuable and meaningful results. These parameters include the dynamics and the observation model coefficients, as well as initialization parameters for the OLS and EKF.The dynamics model presented in this paper contains several parameters characteristic for the tracked object and the environment. The geometrical dimensions of the object were chosen to be identical with the real ICP-89 projectile; thus, the calculated surface of the objectS=0.011m2 and its mass m=7.5kg. The drag coefficient Cd was set to 0.12, a value quantitatively consistent with tabular data for conical surfaces. The lift coefficient Cl was assumed to be zero, since the surface of the object is axially symmetric with respect to the velocity vector. For simplicity, constant values for the gravitational acceleration g=9.80665m/s2 and air density ρ=1.2255kg/m3 were chosen. The assumed values are representative for typical conditions met during firing tests.The elements of the covariance matrix of continuous process disturbancesQc, comprising power spectral densities of the white noises in the wind model, were calculated as follows:
(70)Swx=Swz=σw2Δt.where σw is the assumed standard deviation of the Wiener processes of horizontal and vertical wind components at the terminal stage of flight, after a period of Δt. In the simulated scenario σw=5m/s and Δt=24s, which are typical values, representative to the ones met during tests.The primary measurement error covariance matrixR′ in the EKF was determined based on the exhaustive technical documentation of the EOTS measurement system provided by its developer. The standard deviations of all angle measurements realized in the EOTS are equal σα1=σα2=σλ1=σλ2=0.68′′ in degrees measure (approx. 1.05∙10-6 rad). The diagonal elements of R′ were obtained as squares of the above values, previously converted from arc seconds into radians.The methodology of simulations included several steps. Firstly, a reference flight trajectory was generated, using a numerical 4th order Runge-Kutta solution to the dynamic equations of motion (Equations (50)–(55)). For such a trajectory, ideal (noiseless) primary measurement vectors z′ were calculated, with a period of Δt=33ms, representing the time separation between two successive measurements in the EOTS system. Next, pseudorandom zero-mean Gaussian sequences of standard deviations 0.68′′ were added to the noiseless measurements of observation angles. Such simulated sets of noisy measurements were further processed by the implemented OLS and EKF algorithms. The results of estimation by both algorithms were subsequently analyzed and compared.The following figures represent a selection of the obtained results. In Figure5, the reference trajectory, generated in the simulation process, and the ICP-89 flight trajectories estimated by the OLS and EKF are presented. It is noticeable that all the trajectories are close to each other; however, the scale of the figure makes a direct comparison of the estimation accuracy impossible. Such a comparison will be presented further on in this section of the paper.Figure 5
Simulated and estimated trajectories of ICP-89 aerial target imitator.Figures6 and 7 contain horizontal and vertical coordinates of the ICP-89 versus time, generated and estimated by the OLS and EKF.Figure 6
Simulated and estimated horizontal position of ICP-89.Figure 7
Simulated and estimated vertical position of ICP-89.The estimation of velocity, pitch angle, and wind components is not possible in the OLS, but only in the EKF; therefore, the estimation results for those quantities, shown in Figures8–11, comprises only EKF estimates, together with their reference values, generated during simulations.Figure 8
Simulated and estimated velocity of ICP-89.Figure 9
Simulated and estimated pitch angle of ICP-89.Figure 10
Simulated and estimated horizontal component of wind.Figure 11
Simulated and estimated vertical component of wind.To facilitate a comparison of accuracy of the OLS and EKF algorithms, errors of horizontal and vertical coordinates of the ICP-89 target imitator are presented in Figures12 and 13. These figures give a better insight into the differences between the results from EKF and OLS algorithms than those previously presented in Figures 5–7. As can be seen, the EKF significantly outperforms the OLS solution. One can also observe that the estimation accuracy is not constant for the whole period of target tracking.Figure 12
Error of estimation of horizontal position of ICP-89.Figure 13
Error of estimation of vertical position of ICP-89.This effect is due to changeable conditions of observation, i.e., changing geometrical dilution of the precision factor, which depends on the angle of intersection of the lines between the EOTS stations and the target. For the assumed simulation scenario, this angle decreases in the terminal phase of flight, which increases the geometrical dilution of precision and the target localization errors.It is not possible to directly compare the two implemented algorithms with respect to the accuracy of estimation of the other state variables (velocity, pitch angle, and wind velocity components), because the OLS does not provide their estimates. Therefore, in the following Figures14–17 only EKF errors are presented. They are calculated as differences between respective estimates and the actual state values generated in the process of simulation.Figure 14
Error of estimation of ICP-89 velocity.Figure 15
Error of estimation of ICP-89 pitch angle.Figure 16
Error of estimation of horizontal component of wind.Figure 17
Error of estimation of vertical component of wind.The quantitative comparison between the OLS and EKF can be based on the RMS estimation errors of both algorithms. The results of such a comparison for the estimates of target position coordinates, velocity, and pitch angle are gathered in Table1. It should be noticed, however, that the velocity and pitch angle are not directly available from OLS and they have been calculated based on three successive position estimates. Thus, the figures shown in Table 1, representing RMS velocity and pitch angle errors of OLS, are approximate and may only serve for a crude assessment of the OLS accuracy.Table 1
Comparison of OLS and EKF target localization errors.
Error
OLS
EKF
Improvement ratio
RMSδx [m]
21.2·10-3
7.4·10-3
2.87
RMSδz [m]
8.6·10-3
4.4·10-3
1.95
RMSδv [m/s]
5.67
1.03
5.50
RMSδγ [°]
33.5·10-3
6.8·10-3
4.95


## 5. Conclusions

The paper presented a problem of estimation of a ballistic object trajectory and its solutions based on the OLS and EKF algorithms. Both algorithms designed, implemented, and tested by the authors of the paper proved to be useful tools for estimation of an aerial target position and its parameters of motion. The usefulness of the presented approach was demonstrated for a practical problem of estimation of a trajectory and parameters of motion of the aerial target imitator ICP-89, based on the measurements from the electro-optical tracking system with two cinetheodolite stations.The simulations of such a system have shown that the target trajectory can be estimated with high accuracy. As expected, tracking errors depend on the geometry of the EOTS system and increase when the ICP-89 is far from the cinetheodolite stations. This conclusion should be considered when the shooting tests are planned and used for optimal location of the observation stations in the firing range.The presented comparisons of the OLS and EKF accuracy show that the EKF can estimate the target position with 2-3 times better accuracy than the OLS. Moreover, it can directly estimate the target velocity, pitch angle, and components of wind, which is not possible in the OLS. Therefore, the EKF algorithm should be a preferred solution in realization of a practical tracking system.The authors are currently continuing their works on the presented system aimed at implementation of the presented algorithms, especially the EKF, for processing real measurements from the EOTS system, recorded during live firing tests. The works are aimed also at improving the assumed ballistic model of ICP-89 as well as the wind model. It should be emphasized that the results obtained so far are very promising and it is expected that the EKF algorithm, elaborated by the authors, can be implemented in the software used by MIAT and replace algorithm currently used for fire tests.



---

*Source: 3240898-2019-04-22.xml*