In the unscented Kalman .lter, both the system dynamics function
x(k +1) = f(x(k),u(k),v(k)) (B-16)
and the measurement function z(k +1) = h(x(k),w(k)) (B-17)
can be nonlinear functions. Because the dynamics are nonlinear, the predicted state mean and covariance
can
no
longer
be
computed
using
Eq.
(B-6)
and
Eq.
(B-7).
Instead,
2N +1 sigma points from the joint distribution of the state x(k) and the noise v(k) are drawn. Let the state portion of the sigma points be denoted χ and the noise portion of the sigma points ξ. Propagating the sigma points through f to obtain {βi = f(χi,u(k),ξi)}, the predicted state and covariance become
2N
x.(k +1 | k)= Wiβi, (B-18)
i=0 2N
P (k +1 | k)= Wi (βi . x.(k +1 | k)) (βi . x.(k +1 | k))T . (B-19)
i=0
Similarly, by sigma-point sampling from the joint distribution of x(k) and w(k) and propagating through the measurement function to obtain the set {yi}, the predicted measurement and covariance are
2N
z.(k +1 | k)= Wiyi, (B-20)
i=0 2N
S(k +1) = Wi (yi . z.(k +1 | k))(yi . z.(k +1 | k))T . (B-21)
i=0
The cross-covariance needed to arrive at the .lter gain and, hence, the updated state estimate and covariance is calculated in a similar way.
There are other ways of extending the Kalman .lter to accomodate nonlinear transformations. The unscented Kalman .lter provides one way in which probability distributions resulting from nonlinear transformations are approximated. The extended Kalman .lter provides another way by not approximating the resulting probability distributions but the nonlinear transformations themselves through some kind of local approximation. This requires computing derivatives. The accuracy of the approximation is dictated by the highest order of the derivatives computed. In many applications, the unscented Kalman .lter is preferable because it does not require these computations and often produces more accurate results.
B.3 HORIZONTAL TRACKER
The relative horizontal motion between the own aircraft and the intruder is estimated using an un-scented Kalman .lter. The tracker receives raw measurements of the form zraw =[rslant χh0 h1 ψ0], where rslant is the slant range to the intruder, χ is the relative bearing of the intruder, h0 is the own aircraft altitude, h1 is the intruder altitude, and ψ0 is the own aircraft heading. All of these measurements, with the exception of the heading, are available to TCAS currently. The heading should be attainable through the onboard avionics. These raw measurements are converted to position measurements in a relative Cartesian coordinate system, z =[xrel yrel hrel], according to
the follow equations:
.
r
2
slant . (h1 . h0)2 sin(θ),
xrel =
.
.
2
yrel = rslant . (h1 . h0)2 cos(θ), (B-22) hrel = h1 . h0,
.
.
where θ ≡ χ + ψ. The state is the position and velocities in the relative Cartesian coordinate system: x =[xrel yrel hrel x˙rel y˙rel h˙rel]. The measurement function is given by the following nonlinear function:
z = h(x, w)= h2(h1(x)+ w), (B-23) where the transformation h1 is given by
.
.
22
xrel + yrel + hrel2
atan2(xrel,yrel) hrel
... ...
˙
[rslant θhrel]= h1([xrel yrel hrel x˙rel y˙rel hrel]) =
(B-24)
and the transformation h2 by
..
2
rslant . hrel2 sin(θ)
中國航空網(wǎng) www.k6050.com
航空翻譯 www.aviation.cn
本文鏈接地址：Robust Airborne Collision Avoidance through Dynamic Programm(52)
