Converted Measurement Kalman Filter with Nonlinear Equality ...
Converted Measurement Kalman Filter with Nonlinear Equality Constrains Xiaoxue Feng, Yan Liang, Lianmeng Jiao College of Automation Northwestern Polytechnical University Xi’an, China [email protected] Abstract—For nonlinear systems, Converted Measurement Kalman filter as one of various modifications of the Kalman filter can be used to estimate the state with the non-linear measuring equations, effectively. Although the Converted Measurement Kalman filter is powerful tools for nonlinear state estimation, we might have information about a system that the Converted Measurement Kalman filter does not incorporate. For example, we may know that the states satisfy equality or inequality constraints. In this paper we modify the Converted Measurement Kalman filter to exploit this additional information. A target tracking example is presented to illustrate the effectiveness of Converted Measurement Kalman filter with constraints, which gets better filtering performance than the unstrained Converted Measurement Kalman filter provides. Simulation results between first-order and second-order nonlinear state constraints also show that the second-order solution for higher order nonlinearity as present in this paper outperforms the first-order solution. Keywords-state estimation; Converted Measurement Kalman filter; nonlinear equation constrains
I.
INTRODUCTION
Kalman filters are commonly used to estimate the state of a dynamic system based on its state process and measurement models, which has widely applications in many fields, such as navigation and guidance systems, radar tracking, sonar ranging, and satellite or airplane orbit determination[1]. In active sonar and radar systems the measurement of the position of a target is reported in polar coordinates (its range and azimuth with respect to the sensor location). However, in target tracking the target motion can be best modeled in Cartesian coordinates. Tracking in Cartesian coordinates using polar measurement can be handled in two ways. One method is to use an extended Kalman filter (EKF)[2], which incorporates the original measurement in a nonlinear fashion into the target state estimate, resulting in a mixed coordinate filter. However, it has been found that in case cross-range measurement errors of the target position are large, the performance of EKF degrades considerably due to nonnegligible nonlinear effects. The other approach is to use converted measurement Kalman filter (CMKF)[3,4], which converts the polar position measurement to Cartesian coordinates using the familiar nonlinear mapping between the two coordinate systems, yielding a measurement model that is a linear function of the target’s Cartesian state, after which the classical Kalman filtering algorithm can be used to track target entirely in Cartesian coordinates [5].
Apart from the filtering system equations, we might have information about a system that the Kalman filter does not incorporate. For example, we may know that the states satisfy equality or inequality constraints. There are many examples of state-constrained systems in engineering applications. Some of these examples include, fault diagnosis [6], vision-based systems [7], target tracking [8, 9], robotics [10], and navigation [11]. We believe that if we modify the Kalman filter to exploit this additional information, it will get better filtering performance than the Kalman filter provides. Aiming at the estimation of the linear system, there are many methods which incorporate the constraints into the filtering process, such as the projection methods in [12, 13]. In the nonlinear system, there are also some methods to use. E.g., the moving horizon estimation (MHE) is the most accurate method, but it has heavy burden in the computation [14]. In [15, 16], projection methods are used in the sigma points under the unscented Kalman filter (UKF), which can get a better result. In [17], the constraints had been incorporated into the particle filter (PF), and the performance of the system is greatly improved by incorporating these constraints. In [18] Quadrature Kalman filter (QKF) with equality constraints was proposed, which had better error performance than that of extended Kalman filter (EKF) with constraints. Considering the importance of the estimation problem of Kalman filtering with nonlinear constraints, we combine the CMKF with the projection methods through first-order and second-order linearization, and present a new nonlinear filtering method with constraints. Employ the Taylor series expansion of the nonlinear constraints and maintain the first term and second term [19], separately. The approximate linear constraint is obtained and we discuss the two linearized methods influenced on the state estimation results. In this paper, our contribution is that we solve the problem of the CMKF with nonlinear equality constraints, exploit secondorder nonlinear state constraints providing better approximation for higher order nonlinearities and demonstrate the effectiveness of the new method on a nonlinear vehicle tracking example, compared with the unstrained Converted Measurement Kalman filter. The paper is organized as follows. Section II presents a brief summary of nonlinearly constrained state estimation. Section III details the Converted Measurement Kalman Filter to solve the nonlinear state estimation, and the first-order and second-order linearization to extend the projection method to
This paper is supported by the National Natural Science Foundation of China (Grant No. 61135001, 61074179).
1081
nonlinear cases. Section IV presents some simulation results to illustrate the algorithm. Finally, section V closes with some concluding and suggestions for future works. II.
⎧ ∂J T ⎪⎪ ∂x = W ( x − xˆ ) + D λ = 0 ⎨ ⎪ ∂J = Dx − d = 0 ⎩⎪ ∂λ
PROBLEM FORMULATION
By solving equation set (7) we obtain
Consider the following dynamic equation and measurement equation of the system given by:
xk = f k ( xk −1 ) + vk −1
(1)
yk = hk ( xk ) + nk
(2)
x = xˆ − W −1 DT ( DW −1 DT ) −1 ( Dxˆ − d )
(3)
Dxk = d k
(4)
(8)
The method is called projection method via modifying state estimation. That is to say, the current state estimation is projected on the constrained subspace.
where f and h are state transition function and measurement function, they are nonlinear or linear. {vk } and {nk } are noise inputs. They subject to Gauss distribution, zero-mean and with covariance Qk and Rk . The two types of noise are mutually independent. Given the initial state vector and the associated covariance, the problem is to estimate the state vector of every step by using corresponding measurement data. The above description is the general nonlinear filtering problem. Now suppose there are some nonlinear constraints in the state evolvement which are formulated using a equality formula as g ( xk ) = d k
(7)
III.
CONVERTED MEASUREMENT KALMAN FILTER WITH EQUALITY CONSTRAINTS
A. CMKF In 3D target tracking scenario, a sensor measures a target’s position, producing the spherical position measurement
⎡ ρm ⎤ ⎡ ρ ⎤ ⎡ ρm ⎤ Z m = ⎢⎢ β m ⎥⎥ = ⎢⎢ β ⎥⎥ + ⎢⎢ β m ⎥⎥ ⎢⎣ ε m ⎥⎦ ⎢⎣ ε ⎥⎦ ⎢⎣ ε m ⎥⎦ where
[ρ, β ,ε ]
T
(9)
is the target’s true position in spherical T
where g denotes a state constrained function and d k is a vector. Particularly, D denotes a state constrained matrix. In a sense, (3) is a general formulation. They are acquired from the analysis of the state estimation. At this time, the case is called a nonlinear filtering problem with nonlinear or linear equality constraints. For the inequality, we can use the expansion of the equality constraints like [2]. For convenience, in this paper we only discuss the equality constraints.
coordinates (range, azimuth, and elevation) and ⎡⎣ ρ m , β m , ε m ⎤⎦ is a white, zero-mean, Gaussian measurement noise with covariance
One method to deal with the equality linear constraints has been presented by Dan Simon in [12, 13]. It is a projection method in essence. It is an effective method and has a good performance. The detailed derivation is given in the following.
The spherical measurement is transformed to Cartesian coordinate measurement using the classical conversion
Let x denotes the modified state estimation, W denotes arbitrary symmetric positive definite matrix, xˆ denotes the general state estimation before considering the condition of constraints. The estimation problem with state constraints translates into an optimization problem as below
(11)
⎧ min J ( x) = ( x − xˆ ) W ( x − xˆ ) ⎨ ⎩s.t. Dx = d T
(5)
Use the Lagrange multiplier method to solve (5). Firstly, we construct the formula
J ( x, λ ) = ( x − xˆ )T W ( x − xˆ ) + 2λ T ( Dx − d )
(6)
Subsequently, calculate the partial derivatives of (6), and the results of each component are set zero, i.e.,
1082
(
R m = cov ⎡⎣ ρ m , β m , ε m ⎤⎦
T
)
⎡σ ρ2 ⎢ =⎢ 0 ⎢0 ⎣
0
σβ
2
0
0⎤ ⎥ 0 ⎥ . (10) σ ε2 ⎥⎦
⎡ x ⎤ ⎡ ρ cos ε cos β ⎤ η : ⎢⎢ y ⎥⎥ = ⎢⎢ ρ cos ε sin β ⎥⎥ . ⎢⎣ z ⎥⎦ ⎢⎣ ρ sin ε ⎥⎦ So, the converted measurement’s error is ⎡ xm ⎤ ⎡ xm ⎤ ⎡ x ⎤ ⎢ y ⎥ = ⎢ y ⎥ − ⎢ y⎥ ⎢ m⎥ ⎢ m⎥ ⎢ ⎥ ⎣⎢ zm ⎦⎥ ⎣⎢ zm ⎦⎥ ⎣⎢ z ⎦⎥ ⎡ ρ m cos ε m cos β m − ρ cos ε cos β ⎤ = ⎢⎢ ρ m cos ε m sin β m − ρ cos ε sin β ⎥⎥ ⎢⎣ ⎥⎦ ρ m sin ε m − ρ sin ε
.
(12)
Form Equation (12) we can see that the converted measurement error is no longer zero-mean Gaussian distributed because of the nonlinear coordinate transformation. So, debiasing the raw converted measurement is necessary before the classical Kalman filtering.
Now there are several approaches to compute the practical bias and covariance approximations of the converted measurement’s error. Lerro and Bar-Shalom[20, 21] firstly studied explicit solutions for the mean and covariance of the converted 2D measurement and presented a debiased CMKF (DCMKF) algorithm which provides more accurate state estimate than EKF and traditional CMKF. Now e details the process of CMKF as follows.
Substituting the state prediction ( ρ p , β p , ε p ) into Equation (14) and (17), we can get the mean and covariance of the converted measurement error conditioned on the state prediction
R p = R ( ρ , β , ε ) | ρ = ρ p , β = β p ,ε = ε p
(20)
−σ β2
λβ' = E ⎡⎣cos 2β m ⎤⎦ = e
B. First-order linearization nonlinear constraints For the nonlinear constraint (3), we can perform a Taylor series expansion of the constraint equation around xˆk− to obtain
−2σ β2
.
λε = E [ cos ε m ] = e −σ ε
2
(13)
λε' = E [ cos 2ε m ] = e −2σ ε
g ( xk ) ≈ g ( xˆk− ) + g ′( xˆk− )( xk − xˆk− )
2
s
+1/2∑ ei ( xk − xˆk− )T g ′′( xˆk− )( xk − xˆk− )
Then, the real mean of converted measurement error is
{
(19)
and
Let
λβ = E ⎡⎣cos β m ⎤⎦ = e
μ p = μ ( ρ , β , ε ) |ρ = ρ p , β = β p , ε = ε p
μ( ρ , β , ε ) = E [ xm , ym , zm ] | ρ , β , ε T
i =1
}
(14)
= [μ x , μ y , μz ]
T
with elements
where s is the dimension of g ( x) , ei is the ith natural basis vector in R s , and the entry in the pth row and qth column of the n × n matrix g ′′( x) is given by
μ x = ρ cos β cos ε (λβ λε − 1) μ y = ρ sin β cos ε (λβ λε − 1) .
[ g ′′( x)] pq =
(15)
∂ 2 gi ( x) ∂x p ∂xq
(22)
Neglecting the second-order term gives [7]
μ z = ρ sin ε (λε − 1)
g ′( xˆk− ) xk = b − g ( xˆk− ) + g ′( xˆk− ) xˆk−
Let
α y = sin 2 β cosh σ β2 + cos 2 β sinh σ β2 α z = sin 2 ε sinh σ ε2 + cos 2 ε cosh σ ε2
.
(16)
α xy = sin 2 ε sinh σ ε2 + cos 2 ε cosh σ ε2 covariance
of
debiased
converted
R ( ρ , β , ε ) = cov([ xm , ym , zm ] | ρ , β , ε ) T
⎡ Rxx ⎢ = ⎢ Rxy ⎢ Rxz ⎣
Rxy Ryy Ryz
Rxz ⎤ ⎥ Ryz ⎥ Rzz ⎥⎦
(17)
(24)
d = b − g ( xˆk− ) + g ′( xˆk− ) xˆk−
(25)
C. Second-order linearization nonlinear constraints If we keep the second-order term in (21) then the constrained estimation problem can be approximately written as xk+ = arg min x ( x − xˆk+ )T W ( x − xˆk+ )
xT M i x + 2miT x + μi = 0
Rxy = [ ρ 2 (α xy − λβ−2 cos 2 ε ) + σ ρ2α xy ]sin β cos βλβ' λε2 Ryy = [ ρ (α xα xy − sin β cos ε ) + σ ρ α yα xy ]λβ λε 2
2
2
2
2
(26)
such that
Rxx = [ ρ 2 (α xα xy − cos 2 β cos 2 ε ) + σ ρ2α xα xy ]λβ2 λε2
2
D = g ′( xˆk− )
Thus nonlinear constraints are linearized. Sometimes, though, we can do better than simple first-order linearization, as discussed in the following sections.
with elements
Rxz = [ ρ 2 (1 − λε−2 ) + σ ρ2 ]cos β sin ε cos ελβ λε'
(23)
This equation is equivalent to the linear constraint Dxk = d if
α x = sin 2 β sinh σ β2 + cos 2 β cosh σ β2
Then, the real measurement error is
(21)
. (18)
Ryz = [ ρ 2 (1 − λε−2 ) + σ ρ2 ]sin β sin ε cos ελβ λε' Rzz = [ ρ 2 (α z − sin 2 ε ) + σ ρ2α z ]λε2
(i =1,… , s )
(27)
where W is a weighting matrix, and M i , mi and μi can obtained from (21).The optimization problem given in (26)(27) can be solved with a numerical method. A Lagrange multiplier method for solving this problem is given below. Following the constrained Kalman filtering of [7], we can formulate the projection of an unconstrained state estimation
1083
xk+ = arg min x ( x − xˆk+ )T W ( x − xˆk+ ) subject to f ( x) = 0
(28) (29)
Let W = H T H and z = Hxˆ , the formulation in (28) becomes the same as in (26). In a sense, (28) is a more general formulation because it can also be interpreted as a nonlinear constrained measurement update or a projection in the predicted measurement domain. The solution to the constrained optimization (28) can be obtained again using the Lagrangian multiplier technique, as xˆ = G V ( I + λΣ Σ) e(λ ) −1
−1
T
(30)
ei (λ )t j e (λ )σ + 2∑ + m0 = 0 (31) 2 2 2 i (1 + λσ i ) i 1 + λσ i where G is an upper right diagonal matrix resulting from the Cholesky factorization of W = H T H as q (λ ) = ∑
2 i
2 i
W (= H T H ) = GT G
0⎤ ⎡0.5T 2 0 ⎤ ⎢ ⎥ ⎥ 0⎥ T 0 ⎥ xk +1 xk + ⎢ n (38) ⎢0 0.5T 2 ⎥ k T⎥ ⎢ ⎥ ⎥ 1⎦ ⎢⎣ 0 T ⎥⎦ where process noise nk and measurement noise vk subject to Gaussian distribution, zero-mean with covariance 2 Q = diag ([2 2]) and R = diag ([49 0.01 ]) , and they are mutually independent. The initial state estimation covariance is P0 = diag ([25 1 25 1]) . From trajectory of the target, we are easy to know that there is a constraint formulated as below:
⎡1 ⎢0 =⎢ ⎢0 ⎢ ⎣0
8
V , an orthonormal matrix, and Σ , a diagonal matrix with its diagonal elements denoted by σ i , are obtained from the
4 2 0
0
RMSE of velocity x
where U is the other orthonormal matrix of the SVD and L results from the factorization M = LT L , and (34)
θ k = atan(
ξk ) + vθ ζk
k
Target state transition equation used in the tracking is
RMES of position y
0
20
40
120
2
0
20
40
60 80 Simulation Steps: k
100
120
CMKF CMKF with constraints
3 2 1 0
(37)
100
4
4
(36)
60 80 Simulation Steps: k
CMKF CMKF with constraints
6
0
RMSE of velocity y
rk = ξ k + ζ k + vrk 2
120
1
8
Assume that a target does circular movement, the center of the trajectory is the origin of a Cartesian coordinate system, and the radius is r = 100m. The angular velocity is θ = deg/s, and the initial state vector is x0 = [ξ ξ ζ ζ ]T = [100 0 0 10]T .There is a sensor which locates at the origin to measure the trajectory of the target. The sampling interval is T = 1s .The measurement equations are as follows:
100
2
As a nonlinear equation in λ ¸, it is difficult to find a closed-form solution in general for the nonlinear equation q (λ ) = 0 in (31). Numerical root-finding algorithms may be used instead. SIMULATION RESULTS
60 80 Simulation Steps: k
CMKF CMKF with constraints
0
2
40
3
(35)
IV.
20
4
(33)
t = [… ti …]T = V T (G T ) −1 m
(39)
CMKF CMKF with constraints
6
singular value decomposition (SVD) of the matrix LG −1 as
e(λ ) = [… ei (λ )…]T = V T (G T )−1 ( H T z − λ m)
0 0 1 0
Total simulation step is 120, i.e., the target turn around twice centered at original. The above process is repeated for 100 Monte-Carlo simulations.
(32)
LG −1 = U ΣV T
T 1 0 0
g ( xk ) = ξ k 2 + ζ k 2 = 1002
RMES of position x
onto a nonlinear constraint surface as the constrained leastsquare optimization problem
0
20
40
60 80 Simulation Steps: k
100
120
Fig.1 RMSE of CMKF and constrained CMKF
Fig. l show that state estimation error of unconstrained CMKF and CMKF with constraints. It can be seen that the constrained CMKF results in much more accurate estimates of
1084
position than the unconstrained CMKF. The unconstrained filter results in average position errors of about 4 m, while the constrained filter results in position errors of about 1 m. In addition for this simulation scenario, the unconstrained CMKF performs velocity errors identically with constrained CMKF. The reason of this phenomena is that the state constraints just about position x and y, without velocity. In the simulation, for comparison the first-order nonlinear constraints and second-order nonlinear constraints of filtering accuracy of the proposed algorithm, we plot every component of the state estimation results using the unstrained CMKF, CMKF with first-order linearized constraints (LCCMKF) and CMKF with second-order expansion constraints (CCMKF), as shown in Fig.2. It can be seen that the estimation results of LCCMKF is much cruder than CCMKF, even cruder than unstrained CMKF. The reason of this phenomenon is mainly the state constraints is high order nonlinearity. Thus we can conclude that for higher-order nonlinear constraints, the second-order solution as presented in this paper would outperform a first-order solution.
V.
CONCLUSION
In order to solve the state estimation problem with nonlinear constraints in the nonlinear system, in this paper, we incorporated second-order nonlinear state constraints into CMKF. It can be viewed as an extension of CMKF with linear state equality constraints to nonlinear cases. Simulation results demonstrated that, the proposed method got a more accurate error performance than the unconstrained CMKF provides. For higher-order nonlinear constraints, the second-order linearization nonlinear constraints would outperform a firstorder solution. The contribution of this paper enriches the nonlinear filtering under the condition of constraints. In this paper, we utilize the Newton’s method to find the numerical root in (31). Our future works will include searching for more efficient root finding algorithm to solve for the Lagrangian multiplier. Other directions of our future work will show more theoretical results related to convergence and stability for nonlinear constrained filters, such as the UKF and MHE. REFERENCES
200 100 x(m)
[1]
x-Real x-second-order-constr x-constr x-Esti
0
[2]
-100 -200
[3] 0
20
40
100
120
[4]
20
Vx-Real Vx-second-order-constr Vx-constr Vx-Esti
10 Vx(m/s)
60 80 Simulation Steps: k
0
[5]
-10 -20
[6] 0
20
40
60 80 Simulation Steps: k
400
120
[7] [8]
y-Real y-second-order-constr y-constr y-Esti
200 y(m)
100
0
[9]
-200 -400
[10] 0
20
40
100
120
[11] [12]
Vy-Real Vy-second-order-constr Vy-constr Vy-Esti
20 10 Vy(m/s)
60 80 Simulation Steps: k
[13]
0 -10 -20
[14] 0
20
40
60 80 Simulation Steps: k
100
120
[15]
Fig.2 State estimation of different methods
1085
W.Sorensor. "Kalman Filtering: theory and application", IEEE Press, 1985. T.L.Song, J.L.Speyer. "A stochastic analysis of a modified gain extened Kalman Fllter with application to estimation with bearing only measurements ", IEEE Transcation on automatic Control. 1985, 30(10): 940-949 D. Lerro and Y. Bar-Shalom, "Tracking with debiased consistent converted measurements versus EKF" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.29, pp.1015-1022, July 1993. M. Longbin, S. Ziaoquan, Z. Yiyu, S. Zhong Kang, and Y. Bar-Shalom, "Unbiased converted measurements for tracking" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.34, no.3, pp.1023-1027, July 1998. Y. Bar-Shalom and X. R. Li, "Estimation and tracking: principles", Techniques, and Software [M], Norwood, MA: Artech House, 1993. Simon D., Simon D.L. "Kalman filtering with inequality constraints for Turbofan engine health estimation", IEE Proc. Control Theory Appl., 2006, 153, (3), pp. 371–378 Porrill J. "Optimal combination and constraints for geometrical sensor data", Int. J. Robot. Res., 1988, 7, (6), pp. 66–77 Alouani A., Blair W."Use of a kinematic constraint in tracking constant speed, maneuvering targets", IEEE Trans. Autom. Control, 1993, 38, (7), pp. 1107–1111 Wang L., Chiang Y., Chang F. "Filtering method for nonlinear systems with constraints", IEE Proc. Control Theory Appl., 2002, 149, (6), pp. 525–531 Spong M., Hutchinson S., Vidyasagar M.: "Robot modeling and control" (John Wiley & Sons, 2005) Simon D., Chia T. "Kalman filtering with state equality constraints", IEEE Trans. Aerospace Electron. Syst., 2002, 38, (1), pp. 128–136 D. Simon, "Kalman filtering with state constraints: a survey of linear and nonlinear algorithms," IET Control Theory & Applications, 2010, Vol. 4, Iss. 8, pp. 1303–1318. D. Simon and T. Chia, "Kalman filtering with state equality constraints," IEEE Trans on Aerospace and Electronic Systems, vol. 38,no. I, pp. 128136,2002. H. Michalska and D. Mayne, "Moving horizon observers and observer based control," IEEE Trans on Automatic Control, vol. 40, no. 6, pp. 995-1006,1995. S. KoJils, B. A. Foss, T. S. Schei, "Constrained nonlinear state estimation based on the UKF approach," Computers and Chemical Engineering, vol. 33, pp. 1386-1401,2009.
[16] S. Julier and J. LaViola, "On Kalman filtering with nonlinear equality constraints," IEEE Transactions on Signal Processing, vol. 55, no. 6, pp. 2774-2784, 2007. 1200 [17] Y. Boers and H. Driessenm, "Particle filter track-before-detect application using inequality constraints," IEEE Trans on Aerospace and Electronic Systems, vol. 41, no. 4, pp. 1481-1487,2005. [18] Jinguang Chen ， Lili Ma, "Quadrature kalman filter with equality constraints" 2010 IEEE Fifth International Conference on Bio-Inspired Computing: Theories and Applications (BIC-TA), 23-26, Sept. 2010, Changsha, China, pp:1197-1201. [19] Yang C., Blasch E. "Kalman filtering with nonlinear state constraints", IEEE Trans. Aeros. Electron. Syst., 2008, 45, (1), pp. 70–84
1086
[20] P. Suchomski, "Explicit expressions for debiased statistics of 3D converted measurements" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.35, pp.368-370, January 1999. [21] Z. Duan, C. Han, and X. R. Li, "Comments on ‘Unbiased converted measurements for tracking" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.40, no.4, pp.1374-1377, October 2004 [22] Yang, C., Bakich, M., and Blasch, E. "Nonlinear constrained tracking of targets on roads". In Proceedings of the 8th International Conference on Information Fusion, Philadelphia, PA, July 2005, A8-3,1—8. [23] Yang, C., and Blasch, E. "Kalman filtering with nonlinear state constraints". In Proceedings of the 9th International Conference on Information Fusion, Florence Italy, July 2006.
I.
INTRODUCTION
Kalman filters are commonly used to estimate the state of a dynamic system based on its state process and measurement models, which has widely applications in many fields, such as navigation and guidance systems, radar tracking, sonar ranging, and satellite or airplane orbit determination[1]. In active sonar and radar systems the measurement of the position of a target is reported in polar coordinates (its range and azimuth with respect to the sensor location). However, in target tracking the target motion can be best modeled in Cartesian coordinates. Tracking in Cartesian coordinates using polar measurement can be handled in two ways. One method is to use an extended Kalman filter (EKF)[2], which incorporates the original measurement in a nonlinear fashion into the target state estimate, resulting in a mixed coordinate filter. However, it has been found that in case cross-range measurement errors of the target position are large, the performance of EKF degrades considerably due to nonnegligible nonlinear effects. The other approach is to use converted measurement Kalman filter (CMKF)[3,4], which converts the polar position measurement to Cartesian coordinates using the familiar nonlinear mapping between the two coordinate systems, yielding a measurement model that is a linear function of the target’s Cartesian state, after which the classical Kalman filtering algorithm can be used to track target entirely in Cartesian coordinates [5].
Apart from the filtering system equations, we might have information about a system that the Kalman filter does not incorporate. For example, we may know that the states satisfy equality or inequality constraints. There are many examples of state-constrained systems in engineering applications. Some of these examples include, fault diagnosis [6], vision-based systems [7], target tracking [8, 9], robotics [10], and navigation [11]. We believe that if we modify the Kalman filter to exploit this additional information, it will get better filtering performance than the Kalman filter provides. Aiming at the estimation of the linear system, there are many methods which incorporate the constraints into the filtering process, such as the projection methods in [12, 13]. In the nonlinear system, there are also some methods to use. E.g., the moving horizon estimation (MHE) is the most accurate method, but it has heavy burden in the computation [14]. In [15, 16], projection methods are used in the sigma points under the unscented Kalman filter (UKF), which can get a better result. In [17], the constraints had been incorporated into the particle filter (PF), and the performance of the system is greatly improved by incorporating these constraints. In [18] Quadrature Kalman filter (QKF) with equality constraints was proposed, which had better error performance than that of extended Kalman filter (EKF) with constraints. Considering the importance of the estimation problem of Kalman filtering with nonlinear constraints, we combine the CMKF with the projection methods through first-order and second-order linearization, and present a new nonlinear filtering method with constraints. Employ the Taylor series expansion of the nonlinear constraints and maintain the first term and second term [19], separately. The approximate linear constraint is obtained and we discuss the two linearized methods influenced on the state estimation results. In this paper, our contribution is that we solve the problem of the CMKF with nonlinear equality constraints, exploit secondorder nonlinear state constraints providing better approximation for higher order nonlinearities and demonstrate the effectiveness of the new method on a nonlinear vehicle tracking example, compared with the unstrained Converted Measurement Kalman filter. The paper is organized as follows. Section II presents a brief summary of nonlinearly constrained state estimation. Section III details the Converted Measurement Kalman Filter to solve the nonlinear state estimation, and the first-order and second-order linearization to extend the projection method to
This paper is supported by the National Natural Science Foundation of China (Grant No. 61135001, 61074179).
1081
nonlinear cases. Section IV presents some simulation results to illustrate the algorithm. Finally, section V closes with some concluding and suggestions for future works. II.
⎧ ∂J T ⎪⎪ ∂x = W ( x − xˆ ) + D λ = 0 ⎨ ⎪ ∂J = Dx − d = 0 ⎩⎪ ∂λ
PROBLEM FORMULATION
By solving equation set (7) we obtain
Consider the following dynamic equation and measurement equation of the system given by:
xk = f k ( xk −1 ) + vk −1
(1)
yk = hk ( xk ) + nk
(2)
x = xˆ − W −1 DT ( DW −1 DT ) −1 ( Dxˆ − d )
(3)
Dxk = d k
(4)
(8)
The method is called projection method via modifying state estimation. That is to say, the current state estimation is projected on the constrained subspace.
where f and h are state transition function and measurement function, they are nonlinear or linear. {vk } and {nk } are noise inputs. They subject to Gauss distribution, zero-mean and with covariance Qk and Rk . The two types of noise are mutually independent. Given the initial state vector and the associated covariance, the problem is to estimate the state vector of every step by using corresponding measurement data. The above description is the general nonlinear filtering problem. Now suppose there are some nonlinear constraints in the state evolvement which are formulated using a equality formula as g ( xk ) = d k
(7)
III.
CONVERTED MEASUREMENT KALMAN FILTER WITH EQUALITY CONSTRAINTS
A. CMKF In 3D target tracking scenario, a sensor measures a target’s position, producing the spherical position measurement
⎡ ρm ⎤ ⎡ ρ ⎤ ⎡ ρm ⎤ Z m = ⎢⎢ β m ⎥⎥ = ⎢⎢ β ⎥⎥ + ⎢⎢ β m ⎥⎥ ⎢⎣ ε m ⎥⎦ ⎢⎣ ε ⎥⎦ ⎢⎣ ε m ⎥⎦ where
[ρ, β ,ε ]
T
(9)
is the target’s true position in spherical T
where g denotes a state constrained function and d k is a vector. Particularly, D denotes a state constrained matrix. In a sense, (3) is a general formulation. They are acquired from the analysis of the state estimation. At this time, the case is called a nonlinear filtering problem with nonlinear or linear equality constraints. For the inequality, we can use the expansion of the equality constraints like [2]. For convenience, in this paper we only discuss the equality constraints.
coordinates (range, azimuth, and elevation) and ⎡⎣ ρ m , β m , ε m ⎤⎦ is a white, zero-mean, Gaussian measurement noise with covariance
One method to deal with the equality linear constraints has been presented by Dan Simon in [12, 13]. It is a projection method in essence. It is an effective method and has a good performance. The detailed derivation is given in the following.
The spherical measurement is transformed to Cartesian coordinate measurement using the classical conversion
Let x denotes the modified state estimation, W denotes arbitrary symmetric positive definite matrix, xˆ denotes the general state estimation before considering the condition of constraints. The estimation problem with state constraints translates into an optimization problem as below
(11)
⎧ min J ( x) = ( x − xˆ ) W ( x − xˆ ) ⎨ ⎩s.t. Dx = d T
(5)
Use the Lagrange multiplier method to solve (5). Firstly, we construct the formula
J ( x, λ ) = ( x − xˆ )T W ( x − xˆ ) + 2λ T ( Dx − d )
(6)
Subsequently, calculate the partial derivatives of (6), and the results of each component are set zero, i.e.,
1082
(
R m = cov ⎡⎣ ρ m , β m , ε m ⎤⎦
T
)
⎡σ ρ2 ⎢ =⎢ 0 ⎢0 ⎣
0
σβ
2
0
0⎤ ⎥ 0 ⎥ . (10) σ ε2 ⎥⎦
⎡ x ⎤ ⎡ ρ cos ε cos β ⎤ η : ⎢⎢ y ⎥⎥ = ⎢⎢ ρ cos ε sin β ⎥⎥ . ⎢⎣ z ⎥⎦ ⎢⎣ ρ sin ε ⎥⎦ So, the converted measurement’s error is ⎡ xm ⎤ ⎡ xm ⎤ ⎡ x ⎤ ⎢ y ⎥ = ⎢ y ⎥ − ⎢ y⎥ ⎢ m⎥ ⎢ m⎥ ⎢ ⎥ ⎣⎢ zm ⎦⎥ ⎣⎢ zm ⎦⎥ ⎣⎢ z ⎦⎥ ⎡ ρ m cos ε m cos β m − ρ cos ε cos β ⎤ = ⎢⎢ ρ m cos ε m sin β m − ρ cos ε sin β ⎥⎥ ⎢⎣ ⎥⎦ ρ m sin ε m − ρ sin ε
.
(12)
Form Equation (12) we can see that the converted measurement error is no longer zero-mean Gaussian distributed because of the nonlinear coordinate transformation. So, debiasing the raw converted measurement is necessary before the classical Kalman filtering.
Now there are several approaches to compute the practical bias and covariance approximations of the converted measurement’s error. Lerro and Bar-Shalom[20, 21] firstly studied explicit solutions for the mean and covariance of the converted 2D measurement and presented a debiased CMKF (DCMKF) algorithm which provides more accurate state estimate than EKF and traditional CMKF. Now e details the process of CMKF as follows.
Substituting the state prediction ( ρ p , β p , ε p ) into Equation (14) and (17), we can get the mean and covariance of the converted measurement error conditioned on the state prediction
R p = R ( ρ , β , ε ) | ρ = ρ p , β = β p ,ε = ε p
(20)
−σ β2
λβ' = E ⎡⎣cos 2β m ⎤⎦ = e
B. First-order linearization nonlinear constraints For the nonlinear constraint (3), we can perform a Taylor series expansion of the constraint equation around xˆk− to obtain
−2σ β2
.
λε = E [ cos ε m ] = e −σ ε
2
(13)
λε' = E [ cos 2ε m ] = e −2σ ε
g ( xk ) ≈ g ( xˆk− ) + g ′( xˆk− )( xk − xˆk− )
2
s
+1/2∑ ei ( xk − xˆk− )T g ′′( xˆk− )( xk − xˆk− )
Then, the real mean of converted measurement error is
{
(19)
and
Let
λβ = E ⎡⎣cos β m ⎤⎦ = e
μ p = μ ( ρ , β , ε ) |ρ = ρ p , β = β p , ε = ε p
μ( ρ , β , ε ) = E [ xm , ym , zm ] | ρ , β , ε T
i =1
}
(14)
= [μ x , μ y , μz ]
T
with elements
where s is the dimension of g ( x) , ei is the ith natural basis vector in R s , and the entry in the pth row and qth column of the n × n matrix g ′′( x) is given by
μ x = ρ cos β cos ε (λβ λε − 1) μ y = ρ sin β cos ε (λβ λε − 1) .
[ g ′′( x)] pq =
(15)
∂ 2 gi ( x) ∂x p ∂xq
(22)
Neglecting the second-order term gives [7]
μ z = ρ sin ε (λε − 1)
g ′( xˆk− ) xk = b − g ( xˆk− ) + g ′( xˆk− ) xˆk−
Let
α y = sin 2 β cosh σ β2 + cos 2 β sinh σ β2 α z = sin 2 ε sinh σ ε2 + cos 2 ε cosh σ ε2
.
(16)
α xy = sin 2 ε sinh σ ε2 + cos 2 ε cosh σ ε2 covariance
of
debiased
converted
R ( ρ , β , ε ) = cov([ xm , ym , zm ] | ρ , β , ε ) T
⎡ Rxx ⎢ = ⎢ Rxy ⎢ Rxz ⎣
Rxy Ryy Ryz
Rxz ⎤ ⎥ Ryz ⎥ Rzz ⎥⎦
(17)
(24)
d = b − g ( xˆk− ) + g ′( xˆk− ) xˆk−
(25)
C. Second-order linearization nonlinear constraints If we keep the second-order term in (21) then the constrained estimation problem can be approximately written as xk+ = arg min x ( x − xˆk+ )T W ( x − xˆk+ )
xT M i x + 2miT x + μi = 0
Rxy = [ ρ 2 (α xy − λβ−2 cos 2 ε ) + σ ρ2α xy ]sin β cos βλβ' λε2 Ryy = [ ρ (α xα xy − sin β cos ε ) + σ ρ α yα xy ]λβ λε 2
2
2
2
2
(26)
such that
Rxx = [ ρ 2 (α xα xy − cos 2 β cos 2 ε ) + σ ρ2α xα xy ]λβ2 λε2
2
D = g ′( xˆk− )
Thus nonlinear constraints are linearized. Sometimes, though, we can do better than simple first-order linearization, as discussed in the following sections.
with elements
Rxz = [ ρ 2 (1 − λε−2 ) + σ ρ2 ]cos β sin ε cos ελβ λε'
(23)
This equation is equivalent to the linear constraint Dxk = d if
α x = sin 2 β sinh σ β2 + cos 2 β cosh σ β2
Then, the real measurement error is
(21)
. (18)
Ryz = [ ρ 2 (1 − λε−2 ) + σ ρ2 ]sin β sin ε cos ελβ λε' Rzz = [ ρ 2 (α z − sin 2 ε ) + σ ρ2α z ]λε2
(i =1,… , s )
(27)
where W is a weighting matrix, and M i , mi and μi can obtained from (21).The optimization problem given in (26)(27) can be solved with a numerical method. A Lagrange multiplier method for solving this problem is given below. Following the constrained Kalman filtering of [7], we can formulate the projection of an unconstrained state estimation
1083
xk+ = arg min x ( x − xˆk+ )T W ( x − xˆk+ ) subject to f ( x) = 0
(28) (29)
Let W = H T H and z = Hxˆ , the formulation in (28) becomes the same as in (26). In a sense, (28) is a more general formulation because it can also be interpreted as a nonlinear constrained measurement update or a projection in the predicted measurement domain. The solution to the constrained optimization (28) can be obtained again using the Lagrangian multiplier technique, as xˆ = G V ( I + λΣ Σ) e(λ ) −1
−1
T
(30)
ei (λ )t j e (λ )σ + 2∑ + m0 = 0 (31) 2 2 2 i (1 + λσ i ) i 1 + λσ i where G is an upper right diagonal matrix resulting from the Cholesky factorization of W = H T H as q (λ ) = ∑
2 i
2 i
W (= H T H ) = GT G
0⎤ ⎡0.5T 2 0 ⎤ ⎢ ⎥ ⎥ 0⎥ T 0 ⎥ xk +1 xk + ⎢ n (38) ⎢0 0.5T 2 ⎥ k T⎥ ⎢ ⎥ ⎥ 1⎦ ⎢⎣ 0 T ⎥⎦ where process noise nk and measurement noise vk subject to Gaussian distribution, zero-mean with covariance 2 Q = diag ([2 2]) and R = diag ([49 0.01 ]) , and they are mutually independent. The initial state estimation covariance is P0 = diag ([25 1 25 1]) . From trajectory of the target, we are easy to know that there is a constraint formulated as below:
⎡1 ⎢0 =⎢ ⎢0 ⎢ ⎣0
8
V , an orthonormal matrix, and Σ , a diagonal matrix with its diagonal elements denoted by σ i , are obtained from the
4 2 0
0
RMSE of velocity x
where U is the other orthonormal matrix of the SVD and L results from the factorization M = LT L , and (34)
θ k = atan(
ξk ) + vθ ζk
k
Target state transition equation used in the tracking is
RMES of position y
0
20
40
120
2
0
20
40
60 80 Simulation Steps: k
100
120
CMKF CMKF with constraints
3 2 1 0
(37)
100
4
4
(36)
60 80 Simulation Steps: k
CMKF CMKF with constraints
6
0
RMSE of velocity y
rk = ξ k + ζ k + vrk 2
120
1
8
Assume that a target does circular movement, the center of the trajectory is the origin of a Cartesian coordinate system, and the radius is r = 100m. The angular velocity is θ = deg/s, and the initial state vector is x0 = [ξ ξ ζ ζ ]T = [100 0 0 10]T .There is a sensor which locates at the origin to measure the trajectory of the target. The sampling interval is T = 1s .The measurement equations are as follows:
100
2
As a nonlinear equation in λ ¸, it is difficult to find a closed-form solution in general for the nonlinear equation q (λ ) = 0 in (31). Numerical root-finding algorithms may be used instead. SIMULATION RESULTS
60 80 Simulation Steps: k
CMKF CMKF with constraints
0
2
40
3
(35)
IV.
20
4
(33)
t = [… ti …]T = V T (G T ) −1 m
(39)
CMKF CMKF with constraints
6
singular value decomposition (SVD) of the matrix LG −1 as
e(λ ) = [… ei (λ )…]T = V T (G T )−1 ( H T z − λ m)
0 0 1 0
Total simulation step is 120, i.e., the target turn around twice centered at original. The above process is repeated for 100 Monte-Carlo simulations.
(32)
LG −1 = U ΣV T
T 1 0 0
g ( xk ) = ξ k 2 + ζ k 2 = 1002
RMES of position x
onto a nonlinear constraint surface as the constrained leastsquare optimization problem
0
20
40
60 80 Simulation Steps: k
100
120
Fig.1 RMSE of CMKF and constrained CMKF
Fig. l show that state estimation error of unconstrained CMKF and CMKF with constraints. It can be seen that the constrained CMKF results in much more accurate estimates of
1084
position than the unconstrained CMKF. The unconstrained filter results in average position errors of about 4 m, while the constrained filter results in position errors of about 1 m. In addition for this simulation scenario, the unconstrained CMKF performs velocity errors identically with constrained CMKF. The reason of this phenomena is that the state constraints just about position x and y, without velocity. In the simulation, for comparison the first-order nonlinear constraints and second-order nonlinear constraints of filtering accuracy of the proposed algorithm, we plot every component of the state estimation results using the unstrained CMKF, CMKF with first-order linearized constraints (LCCMKF) and CMKF with second-order expansion constraints (CCMKF), as shown in Fig.2. It can be seen that the estimation results of LCCMKF is much cruder than CCMKF, even cruder than unstrained CMKF. The reason of this phenomenon is mainly the state constraints is high order nonlinearity. Thus we can conclude that for higher-order nonlinear constraints, the second-order solution as presented in this paper would outperform a first-order solution.
V.
CONCLUSION
In order to solve the state estimation problem with nonlinear constraints in the nonlinear system, in this paper, we incorporated second-order nonlinear state constraints into CMKF. It can be viewed as an extension of CMKF with linear state equality constraints to nonlinear cases. Simulation results demonstrated that, the proposed method got a more accurate error performance than the unconstrained CMKF provides. For higher-order nonlinear constraints, the second-order linearization nonlinear constraints would outperform a firstorder solution. The contribution of this paper enriches the nonlinear filtering under the condition of constraints. In this paper, we utilize the Newton’s method to find the numerical root in (31). Our future works will include searching for more efficient root finding algorithm to solve for the Lagrangian multiplier. Other directions of our future work will show more theoretical results related to convergence and stability for nonlinear constrained filters, such as the UKF and MHE. REFERENCES
200 100 x(m)
[1]
x-Real x-second-order-constr x-constr x-Esti
0
[2]
-100 -200
[3] 0
20
40
100
120
[4]
20
Vx-Real Vx-second-order-constr Vx-constr Vx-Esti
10 Vx(m/s)
60 80 Simulation Steps: k
0
[5]
-10 -20
[6] 0
20
40
60 80 Simulation Steps: k
400
120
[7] [8]
y-Real y-second-order-constr y-constr y-Esti
200 y(m)
100
0
[9]
-200 -400
[10] 0
20
40
100
120
[11] [12]
Vy-Real Vy-second-order-constr Vy-constr Vy-Esti
20 10 Vy(m/s)
60 80 Simulation Steps: k
[13]
0 -10 -20
[14] 0
20
40
60 80 Simulation Steps: k
100
120
[15]
Fig.2 State estimation of different methods
1085
W.Sorensor. "Kalman Filtering: theory and application", IEEE Press, 1985. T.L.Song, J.L.Speyer. "A stochastic analysis of a modified gain extened Kalman Fllter with application to estimation with bearing only measurements ", IEEE Transcation on automatic Control. 1985, 30(10): 940-949 D. Lerro and Y. Bar-Shalom, "Tracking with debiased consistent converted measurements versus EKF" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.29, pp.1015-1022, July 1993. M. Longbin, S. Ziaoquan, Z. Yiyu, S. Zhong Kang, and Y. Bar-Shalom, "Unbiased converted measurements for tracking" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.34, no.3, pp.1023-1027, July 1998. Y. Bar-Shalom and X. R. Li, "Estimation and tracking: principles", Techniques, and Software [M], Norwood, MA: Artech House, 1993. Simon D., Simon D.L. "Kalman filtering with inequality constraints for Turbofan engine health estimation", IEE Proc. Control Theory Appl., 2006, 153, (3), pp. 371–378 Porrill J. "Optimal combination and constraints for geometrical sensor data", Int. J. Robot. Res., 1988, 7, (6), pp. 66–77 Alouani A., Blair W."Use of a kinematic constraint in tracking constant speed, maneuvering targets", IEEE Trans. Autom. Control, 1993, 38, (7), pp. 1107–1111 Wang L., Chiang Y., Chang F. "Filtering method for nonlinear systems with constraints", IEE Proc. Control Theory Appl., 2002, 149, (6), pp. 525–531 Spong M., Hutchinson S., Vidyasagar M.: "Robot modeling and control" (John Wiley & Sons, 2005) Simon D., Chia T. "Kalman filtering with state equality constraints", IEEE Trans. Aerospace Electron. Syst., 2002, 38, (1), pp. 128–136 D. Simon, "Kalman filtering with state constraints: a survey of linear and nonlinear algorithms," IET Control Theory & Applications, 2010, Vol. 4, Iss. 8, pp. 1303–1318. D. Simon and T. Chia, "Kalman filtering with state equality constraints," IEEE Trans on Aerospace and Electronic Systems, vol. 38,no. I, pp. 128136,2002. H. Michalska and D. Mayne, "Moving horizon observers and observer based control," IEEE Trans on Automatic Control, vol. 40, no. 6, pp. 995-1006,1995. S. KoJils, B. A. Foss, T. S. Schei, "Constrained nonlinear state estimation based on the UKF approach," Computers and Chemical Engineering, vol. 33, pp. 1386-1401,2009.
[16] S. Julier and J. LaViola, "On Kalman filtering with nonlinear equality constraints," IEEE Transactions on Signal Processing, vol. 55, no. 6, pp. 2774-2784, 2007. 1200 [17] Y. Boers and H. Driessenm, "Particle filter track-before-detect application using inequality constraints," IEEE Trans on Aerospace and Electronic Systems, vol. 41, no. 4, pp. 1481-1487,2005. [18] Jinguang Chen ， Lili Ma, "Quadrature kalman filter with equality constraints" 2010 IEEE Fifth International Conference on Bio-Inspired Computing: Theories and Applications (BIC-TA), 23-26, Sept. 2010, Changsha, China, pp:1197-1201. [19] Yang C., Blasch E. "Kalman filtering with nonlinear state constraints", IEEE Trans. Aeros. Electron. Syst., 2008, 45, (1), pp. 70–84
1086
[20] P. Suchomski, "Explicit expressions for debiased statistics of 3D converted measurements" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.35, pp.368-370, January 1999. [21] Z. Duan, C. Han, and X. R. Li, "Comments on ‘Unbiased converted measurements for tracking" [J], IEEE Transactions on Aerospace and Electronic Systems, vol.40, no.4, pp.1374-1377, October 2004 [22] Yang, C., Bakich, M., and Blasch, E. "Nonlinear constrained tracking of targets on roads". In Proceedings of the 8th International Conference on Information Fusion, Philadelphia, PA, July 2005, A8-3,1—8. [23] Yang, C., and Blasch, E. "Kalman filtering with nonlinear state constraints". In Proceedings of the 9th International Conference on Information Fusion, Florence Italy, July 2006.
