Robust Extended Kalman Filtering in Hybrid Positioning Applications

4th WORKSHOP ON POSITIONING, NAVIGATION AND COMMUNICATION 2007 (WPNC’07), HANNOVER, GERMANY
Multiplying (23) by
Mk and defining
I
Nk =Mk , ξk =Mkek, zk =Mk
Hk
zk =Nkxk + ξk
ˆx −
k
yk
yields
(27)
which is in the form of a standard linear least squares
regression problem or equivalently
n
ˆxk =argmin ((zk)i − (N
xk
T k )ixk) 2
(28)
i=1
where (NT k )i is the ith row of Nk. NowE(ξk) =0,
V(ξk) =E(ξkξT k )=I, and the solution to (28) is given by
ˆxk =(N T k Nk) −1 N T k zk =ˆx −
k +Kk(yk − Hk ˆx −
k
) (29)
which will be the posterior mean. The posterior covariance is
chosen to be
ˆPk =(N T k Nk) −1 =(I− KkHk) ˆ P −
k
(30)
where Kk = ˆ P −
k HT k (Hk ˆ P −
k HT k +Rk) −1 is called the Kalman
gain matrix. Equations (29) and (30) are of course similar to
equations (12) and (13).
Huber [2] introduced a class of estimators, called Mestimators,
that minimize other functionals than the squares.
Let ρ(·) denote the functional which will be minimized. Now
using ρ(·) in (28) in place of (·) 2 gives
n
ˆxk =argmin ρ((zk)i − (N
xk
T k )ixk) (31)
i=1
The minimum is obtained by differentiating the functional to
be minimized with respect to (xk)j, j=1,...,nx and setting
these partial derivatives equal to zero. Symbolically this can
written as nx equations
n
(Nk)ijψj((zk)i − (N T k )iˆxk) =0, j =1,...,nx (32)
i=1
where ψj =(∇ρ)j and (Nk)ij is the ijth element of matrix
Nk. The solution of (32) is somewhat difficult to obtain at
least analytically because the ψ-functions may be non-linear.
However, (32) can be approximated with a weighted least
squares problem, namely
n
(Nk)ijωk,i((zk)i − (N T k )iˆxk) =0, j =1,...,nx (33)
i=1
where the weights ωk,i are given by
ψi((zk)i−(N
ωk,i =
T
k
)i ˆx− k )
(zk)i−(NT k
)i ˆx− k
, (zk)i = (NT −
k )iˆx k
1, (zk)i =(NT k
Now the solution of (33) is given by
)iˆx −
k
xk =(N T k ΩkNk) −1 N T k Ωkzk =ˆx −
k +K∗k(yk − Hk ˆx −
k
with covariance
ˆPk =(I− K ∗ kHk)P ∗ k
(34)
) (35)
(36)
58
where K∗ k =P∗ kHT k (R∗ k +HkP∗ kHT k )−1 ,
P∗ k =(ˆ P −
k )1/2Ω −1
k−1,1 (ˆ P −
k )1/2 , R∗ k =R1/2
k Ω−1
k−1,2R1/2 k
Ωk,1 0
Ωk =diag(ωk,1,...,ωk,(nx+ny)) =
0 Ωk,2
(37)
where the weights Ωk,1 ∈ R nx×nx and Ωk,2 ∈ R ny×ny . The
motivation for dividing the weight matrix into two submatrices
is that the Ωk,1 influences only the state update and Ωk,2 only
the state correction with measurements. So if robustness is
desired only in the measurement model one might want to
define Ωk,1 =I. Depending on the application it might be
reasonable to use different ψ-function for different components
in (32).
In this paper ψ(·) = −s(·), where the choices for the
likelihood score s are given in equations (17), (21) and (22).
Figure 1 shows the ψ-functions and the corresponding weight
functions. The filters using this method are denoted with prefix
”WLS”.
B. Bayesian filtering assuming non-gaussian innovation sequence
The following method is based on the results of Masreliez
and Martin [7]. Consider a transformed version of the measurement
model in (8).
Tkyk =TkHkxk +Tkvk
(38)
The transformation matrix Tk = (Hk ˆ P −
k HT k +Rk) −1/2 is
chosen such that TT k Tk =(Hk ˆ P −
k HT k +Rk) −1 , which is easily
obtained for example using the singular value decomposition.
Thus the transformed innovation variable has a standard normal
density distribution.
The Kalman filter calculates the posterior state estimates
using the innovation variable. Under the transformed measurement
model (38) the posterior mean is given by
ˆxk =ˆx −
k + ˆ P −
k HT k T T k s(nk) (39)
where the score s(nk) =−∇ ln p(yk|y1:k−1)|yk=nk and
nk =Tk(yk−Hk ˆx −
) is the transformed innovation. Masreliez
k
and Martin assert that if p(yk|y1:k−1) is the least favorable
density of either Fɛ or Fp then the posterior mean is given as
in (39) and the posterior covariance is given by
ˆPk =(I−KkHk E p(yk|y1:k−1)∇s T
(yk)|yk=nk ) Pˆ −
k (40)
If s = sH or s = sD, then
E p(yk|y1:k−1)∇s T
(yk)|yk=nk =
p(yk|y1:k−1)∇s T (yk)|yk=nk dyk =1− 2Φ(k) (41)
and if s = sM , then
E p(yk|y1:k−1)∇s T
(yk)|yk=nk =
(myp) −2 (1 − p(1 + tan 2 ( 1
)) (42)
2m
The filters using this method are denoted with prefix ”B”