*
Jeremy A. Hodgson , BSc(Hons), MSc, MIEE
The MathWorks Ltd,
Cambridge, UK
R
y
R
ψDBS
∆ψs
Point B
cr
Point A
(xT,yT,hT)
x
to the target, and the range rate derived from the To quantify the magnitude of the uncertainty region
Doppler frequency shift. The measurements are related from the covariance matrix we can consider the
to the INS estimates, and the unknown parameters, probability that the true values lie within the
through Equation (4.2). hyperellipsoid given by Equation (4.8).
( x − xˆ ) P −1 ( x − xˆ ) = l 2
T
zR = hR ( xINS , vINS , x , ∆t ) (4.8)
The semi-axis lengths of this hyperellipsoid are defined
( xINS + δ x − δ vx ∆t )
2
=
(xINS + δ x f − δ v ⋅ ∆t ) . ( vINS + δ v ) l = 1 this relates to a probability region of 20%, and a
value of l = 2 relates to a probability region of 74%.
zR
l 2 ( −1/ 2l 2 )
Each measurement is assumed to be independent, and Pr = erf − le (4.9)
has zero mean Gaussian uncertainty with a standard 2 π
deviation given by Equation (4.3). The cost on the accuracy of the position estimates is
σ R = RRes proposed to be the sum of the squares of the ellipsoid
semi-axes ( xr , yr , zr ) defined by the eigenvalues of the
λ cr (4.3)
σ R& = covariance matrix.
2 crDT
The CRLB provides a lower bound on the achievable
5. Trajectory optimization
variance in the estimation of a parameter. For an
unbiased estimator it is given by the inverse of the
Fisher Information Matrix (J) in Equation (4.4). A direct optimization approach was taken which is
based on a finite-dimensional discretization of the
E[( xTrue − xˆ )( xTrue − xˆ )T ] ≥ J −1 (4.4) original control problem. This step reduces the problem
The Fisher Information Matrix for a non-linear least to that of a finite-dimensional constrained optimization
squares batch filter with zero mean Gaussian problem that can be solved using the Sequential
measurement uncertainty is given by the inverse of the Quadratic Programming (SQP) solvers available in the
covariance matrix calculated with true values for the MathWorks Optimization Toolbox9. The benefit of
uncertain parameters6. In this case that implies that the using a direct optimization approach, over the
errors are all zero, and that the INS estimates are the variational approach as described in Bryson5, is the
true values. The covariance matrix is defined in ease with which the problem can be posed, and the
Equation (4.5), where H n is the stacked matrix of relaxed requirements on the initial guess for the
partial derivatives of each measurement equation with solution.
respect to each unknown parameter, and is given in A fixed flight time was chosen, and the parameters to
Equation (4.6). Rn is the stacked uncertainty values for be determined are the sequence of piecewise constant
each measurement, and is given in Equation (4.7). acceleration demands over equidistant time intervals
(Figure 3). In addition to determining the time varying
( )
T −1
−1
P ≡ J −1 = H n Rn H n (4.5) controls, the thrust level, and the initial cross range
offset are set as parameters to be determined.
For a given sequence of demands the dynamic states
are calculated by numerically integrating the state
equations. The scalar cost function, and the vector
inequality constraint functions, are then evaluated and
4
u ( 2)
x ( 0)
u (1)
u (n − 2)
u ( 0) x ( n)
u ( n − 1)
tf t t tf Time
0 2 f 3 f ( n − 1) tf
n n n n
( )
passed to the optimization routines. The cost function t f n −1
J control = ∑ ( a z (k ) − az (k − 1) ) + ( a y (k ) − a y (k − 1) )
2 2
for this paper is made up of the components given in
n
k =0
Equation (5.1).
J cr JP J control T
az (−1) = 1, a y (−1) = 0. (5.4)
J Tot = + + + GTerm GTerm
α cr αP α control The constraints at the final flight time are given in
J cr = Cost on the dwell time Equation (5.5), and are incorporated by augmenting the
(5.1) cost function in (5.1). There are six constraints,
J P = Cost on the CRLB
including the cross range distance, height, speed,
J control = Cost on the acceleration demands angles of approach, and total angle of attack. There is
GTerm = Terminal constraints no constraint on the final downrange location since the
α cr ,α P , α control = Weighting values. optimization runs with a fixed flight time. For the
The cost on the dwell time to achieve a desired cross purposes of calculating the dwell time and the CRLB it
is always assumed that the target is located where the
range resolution is given in Equation (5.2), where t f is
trajectory terminates.
the time of flight, and n is the number of discrete y (n)
intervals that the flight is divided into. Wcr ( R ) is a 100
h (n )
function which weights the cost during the period (5.5)
100
where imaging can take place (Figure 4). The last three
VTerm − V ( n )
nodes are not included in the cost function to represent
10
the transition from imaging, to achieving the desired G Term =
angle of attack at impact. γ Term − γ ( n )
(5π /180 )
t f n − 3 crDT ( k )
2
ψ Term − ψ ( n )
J cr = ∑ .Wcr ( R (k ) ) (5.2)
( 5π /180 )
n k =1 cr
α (n )
The cost on the uncertainty associated with the (5π /180 )
estimates of the target location is given in Equation In addition to minimizing the cost function, at each
(5.3), where xr , yr , zr are the eigenvalues of the subset node the resulting trajectory has to satisfy a number of
of the covariance matrix in (4.6) associated with the interior point constraints on the states. The three
position error estimates. It is assumed that inequality constraints are: -
measurements are taken at each node in the trajectory i) α ≤ α max
when the conditions are satisfactory, and that the dwell
time is chosen to obtain a cross range resolution of cr. ii) σ v ≤ σ v _ max (R < R Acquire )
J P = ( xr2 + yr2 + zr2 ) (5.3) iii) ψ DBS ≥ 5 0 ( R < R Acquire & k < n - 2)
The cost on the acceleration demands is given in
Equation (5.4), where the aim is to smooth the 6. Example optimal trajectory results
demands over the time of flight.
Two resulting optimal trajectories are shown in Figures
5 and 6, where the requirements are specified in Table
3. The first trajectory has no cost on the expected
5
Figure 8: Position estimate uncertainty for trajectory with the cost on the CRLB
10
11