Track initialization algorithm using optical measurements

A.3 The Henze-Zirkler test

3.2 Track initialization algorithm using optical measurements

• Measurement z = [θ, ϕ, ˙θ, ˙ϕ] • Range limits rmin, rmax

• Covariance for observed parameters Rk Result: Initialized particle distribution {x(i)₀ }N

i=1 Evaluate w0, w1, w2, w3, w4, w5, T (r), S(r)

Compute ˙rmin = −w1/2 − ζ(rmin) and ˙rmax = −w1/2 + ζ(rmin) for i = 1 . . . N do

solution found ← false while not solution found do

Sample r(i) _{∼ U (r}

min, rmax) Sample ˙r(i) ∼ U ( ˙rmin, ˙rmax) Evaluate ζ from (3.29)

if ζ is real and (3.28) holds then solution found ← true

end end

Sample [θ(i), ϕ(i), ˙θ(i), ˙ϕ(i)]0 ∼ N (·; z, R) x(i)₀ ← T−1_([r(i)_{, θ}(i)_{, ϕ}(i)_{, ˙r}(i)_{, ˙}_θ(i)_{, ˙}_ϕ(i)_]0₎ end

3.5 Filtering recursion

which will make the weights quickly go to zero if the newly sampled values x(i)_k are unlikely under the transition kernel f (·|x(i)_k−1). At the beginning of the filtering process, where there is high uncertainty on the target position and velocities induced by the shape of the distribution yielded by the initial orbit determination, this is bound to happen often. There are two particular forms of the importance sampling function π which avoid this problem. The bootstrap filter uses π(x(i)_k ) = f (x(i)_k |x(i)_k−1), which cancels out in (3.31) with the problematic part in the numerator. This proposal, however, does not use measurement information and is bound to be less efficient than a fully adapted proposal distribution [18]. Another option is to use the optimal proposal function π(x(i)_k ) = f (x(i)_k |x_k−1(i) )gk(zk|x(i)_k ), in which case the weights would remain constant through the filtering duration. In a typical SSA application, this distribution is not available for direct sampling, due to the form of the measurement and dynamical models. An approximation of this distribution based on the linearity of the measurement model in the sensor state space will be outlined below.

Although the dynamical model discussed in section 3.2 is a linear, Gaussian approximation in the object’s state space (conditioned a given previous state) and the sensor model is linear and Gaussian in the extended sensor state space discussed in section 3.3, the nonlinear transformation between the two spaces means that a closed form for the updated distribution is not available.

Using a particle representation for the object state provides a straightforward way to map between the two spaces, by simply applying the required transformation to each particle. This is an essential part of the proposed method, whether for radar or optical measurements. The particle distribution is updated as measurements arrive by approximating it as a Gaussian distribution in S∗, and then applying a Kalman update to it before sampling a new set of particles from the updated distribution. This approach is outlined below. To begin, the unweighted particles from the previous time step {x(i)_k−1}N

i=1are used to produce a set of predicted particles by drawing from the transition kernel (3.12):

x(i)_k|k−1∼ N (·; Φ(xk−1, ∆k)xk−1, Qk).

If no measurement is available, then these particles are kept as the predicted distribution. However, if a measurement has been received, the particles are first mapped to S∗ using (3.17) to obtain

3.5 Filtering recursion

The mean and covariance of this set of particles are computed:

after which a linear Kalman Filter update is performed [47]: S = HΣk|k−1H0 + R,

K = Σk|k−1H0S−1,

µk = µk|k−1+ K(zk− Hµk|k−1), Σk = (I − KH)Σk|k−1.

(3.32)

Here, S denotes the innovation covariance and K the Kalman gain. The updated set of particles is drawn from a Gaussian distribution with these parameters

y_k(i)∼ N (·; µk, Σk), (3.33) and finally the updated set of particles is obtained by mapping back to X,

x(i)_k = T−1(y_k(i), ts, Ws). (3.34)

Although this approach has been shown to perform very well, it is important to note that it is not strictly a particle filtering method as the distribution is not weighted. Two concerns with this approach are that approximating the distribution as a Gaussian may cause some loss of information on the shape of the distribution, and sampling from a single distribution discards the particle trajectories. Below, an importance sampling function is sketched which reproduces this hybrid update approach while maintaining the theoretical properties of a SIR filter. To do this, it can be remarked that the correction step can be applied to each individual particle, rather than the distribution mean, to obtain

y_k(i) = y_k|k−1(i) + K(zk− Hy (i)

3.5 Filtering recursion

the mean of this distribution is the following:

E[y_k(i)] = E[y_k|k−1(i) ] + K(zk− HE[y (i) k|k−1]) = µk|k−1+ K(zk− Hµk|k−1),

(3.36)

This means that the covariance will be underestimated by comparison to the previous method by a factor of KRK0. In order to compensate for this, an independent random variable with covariance KRK0 can be added such that the total variance will be (I − KH)Σk|k−1:

y_k(i) = y_k|k−1(i) + K(zk− Hy (i)

k|k−1) + νk (3.38)

where ν_k(i) ∼ N (·; 0, KRK0_{), for example. In this case, the value of the importance} sampling function would be

π(y_k(i)) = N (ν_k(i); 0, KRK0))N (x_k|k−1(i) ; Φ(xk−1, ∆k)xk−1, Qk). (3.39)

An alternative is to inject variance into the resulting distribution by uniformly mov- ing the particles away from the mean. In this case, for a given matrix F , the particles would be computed as

y(i)_k = (I + F )(y(i)_k|k−1+ K(zk− Hy (i)

k|k−1)) − F µk, (3.40)

in which case the mean would be

E[y_k(i)] = (I + F )(E[y(i)_k|k−1] + K(zk− HE[y (i) k|k−1])) − F µk = (I + F )(µk|k−1+ K(zk− Hµk|k−1)) − F µk = µk+ F µk− F µk = µk, (3.41)

3.5 Filtering recursion

which is as required, and the covariance would be

cov[y_k(i)] = (I + F )(I − KH)Σk|k−1(I − KH)0(I + F )0 = (I + F )(Σ∨)(I + F )0

= Σ∨+ F Σ∨+ Σ∨F0+ F Σ∨F0,

(3.42)

where Σ∨ = (I − KH)Σk|k−1− KRK0. To get the required covariance, F must be a solution to

Σ∨+ F Σ∨+ Σ∨F0+ F Σ∨F0 = KRK0. (3.43)

If a symmetry condition is imposed on F , this equation can be solved using the method described in [67]. In this case, the importance function can be evaluated as π(y_k(i)) = N (x(i)_k|k−1; Φ(xk−1, ∆k)xk−1, Qk). (3.44)

Unfortunately, these importance sampling approaches have the problem that the numerator of (3.31) will be either zero or very close to it as the kernel f (x(i)_k |x(i)_k−1) will usually be very narrow (or in the deterministic case just a Dirac delta) compared to the correction applied by the Kalman update, causing degenerate distributions as most weights will go to 0. For this reason, the particle set obtained from (3.33) will be used instead. This can be seen as an approximation of the optimal importance sampling function as the sample is unweighted. A flowchart detailing the filtering process can be seen in Figure 3.3. The prediction and update algorithms can be seen in Algorithms 3.3 and 3.4, respectively.

Algorithm 3.3: Prediction algorithm

In document State space reparametrization for approximating nonlinear models in Bayesian state estimation (Page 62-66)