# A Novel Fifth-Degree Cubature Kalman Filter for Real-Time Orbit Determination by Radar

Authors: Zhaoming Li; Wenge Yang; Dan Ding; Yurong Liao
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

A novel fifth-degree cubature Kalman filter is proposed to improve the accuracy of real-time orbit determination by ground-based radar. A fully symmetric cubature rule, approaching the Gaussian weighted integral of a nonlinear function in general form, is introduced, and the specific points and weights are calculated by matching the monomials of degree not greater than five with the exact values. On the basis of the above rule, a novel fifth-degree cubature Kalman filter, which can achieve a higher accuracy than UKF and CKF, is derived under the Bayesian filtering framework. Then, to describe the nonlinear system more accurately, the orbital dynamics equation with J2 perturbation is used as the state equation, and the nonlinear relationship between the radar measurement elements and orbital states is built as the measurement equation. The simulation results show that, compared with the traditional third-degree algorithm, the proposed fifth-degree algorithm has a higher accuracy of orbit determination.


---

## Body

## 1. Introduction

With the increasing number of satellites launched into orbit every year, the monitoring and cataloguing of satellites play an important role in improving the rate of utilisation of space resources and alleviating the pressure on orbit resources. As a type of sensor in space surveillance systems, ground-based radar is equipped without considering the influence of the weather and other special circumstances, and the use of its measurement data for real-time orbit determination is a key technology in space target tracking [1, 2]. Due to the nonlinearity of the satellite orbital dynamics model with the influence of orbital perturbation, the essence of orbit determination in real-time is to achieve the optimal estimation of the orbital state by means of nonlinear filtering technology under the Bayesian framework using the measured ranging, velocity, and angle data with measurement noise, which has important research value.The core issue in nonlinear Kalman filtering is to calculate the intractable multidimensional vector integral such as the “nonlinear function× Gaussian probability density function (pdf),” for which it is difficult to achieve the analytical solution [3, 4]. At present, two methods, including the approximation of the nonlinear function and the approximation of the Gaussian pdf, are mainly taken. In the former method, the nonlinear function is approximated by the polynomial and results in an extended Kalman filter (EKF) [5, 6], divided difference Kalman filter (DDKF) [7], and polynomial Kalman filter (PKF) [8, 9], where the first-order Taylor expansion, the multidimensional Stirling interpolation, and polynomials including Chebyshev and Fourier-Hermit are adopted to approximate the nonlinear function, respectively, in EKF, DDKF, and PKF. However, the aforementioned methods tend to be restricted when the system has strong nonlinearity with high dimensionality. For the latter, the Gaussian pdf is approximated using the deterministic sampling approach, which mainly includes the unscented transform (UT) and spherical-radial rule (SRR). Then, the unscented Kalman filter (UKF) [10, 11] and cubature Kalman filter (CKF) [12–14] are obtained by embedding UT and SRR into the Bayesian filtering framework, respectively, these have a wide range of applications in engineering [15–20], but these two types of algorithm have only third-degree filtering accuracy, which is required to be further improved. In this paper, a novel fifth-degree cubature Kalman filter is proposed without differential operation to improve the filtering accuracy from third degree to fifth degree. The integral points with corresponding weights in the general cubature rule are calculated by matching the monomials of degree no more than five with their exact values in the fully symmetric region. Then, the proposed filtering method, which can achieve a higher accuracy compared to that with UKF and CKF, is deduced by embedding the novel fifth-degree cubature rule into the Bayesian filtering framework. The proposed filtering algorithm is applied in real-time orbit determination, and a more accurate orbit estimate is achieved.The remainder of the paper is organised as follows: the traditional cubature Kalman filter is described in Section2, the novel fifth-degree cubature rule and related Kalman filter are derived in Section 3, the mathematical models used for orbit determination are described in Section 4, the numerical experiment and results are presented in Section 5, and the conclusions are drawn in Section 6.


## 2. The Traditional Cubature Kalman Filter

The core problem in any nonlinear Kalman filter is to calculate the integral∫RnfxNx;x-,Pxdx, for which, in general, it is difficult to find the analytical solution, where fx denotes the arbitrary function, Nx;x-,Px denotes the Gaussian pdf. Specifically, an integral of the form If=∫Rnfxe-xΤxdx in the Cartesian coordinate system is considered. Let x=ry with yΤy=1, where y denotes the direction vector and r≥0 denotes the radius, so that xΤx=r2 and then the integral If can be rewritten in a spherical-radial coordinate system as follows:(1)If=∫0∞∫Unfryrn-1e-r2dσydr,where Un is the surface of the sphere defined by Un=y∈Rn:yΤy=1 and σ· is the area element on Un. Thus, the integral is decomposed into spherical integral Sr and radial integral R, respectively, and approximately represented using numerical integration as follows:(2)Sr=∫Unfrydσy≈∑i=1Lsωs,ifryiR=∫0∞Srrn-1e-r2dr≈∑j=1Lrωr,jSrj,where yi,ωs,i denote the integral points and weights of the spherical integral and Ls denotes the number of integral points. Similarly, rj,ωr,j denote the integral points and weights of the radial integral, and Lr denotes the number of points. From the third-degree spherical-radial cubature rule used in [12], we obtain that(3)Sr=An2n∑i=1nfrξi+f-rξiR=12Γn2Sn2,where An=2πn/Γn/2 is the surface area of the unit sphere, Γz=∫0∞e-ttz-1dt is the Gamma function, and ξi denotes the unit vector with the ith element being 1. Sr is substituted into R, to get(4)If=∑j=1Lr∑i=1Lsωr,jωs,ifrjyi=∑j=1L12Γn2∑i=1nAn2nfrjξi+f-rjξi=πn2n∑i=1nfn2ξi+f-n2ξi,Due to identity equation(5)∫RnfxNx;x-,Pxdx=1πn∫Rnf2Pxx+x-e-xΤxdx,it may be seen that(6)∫RnfxNx;x-,Pxdx=12n∑i=1nfnPxξi+x-+f-nPxξi+x-.The calculation process used in the traditional CKF is listed as follows.Time Update. Evaluate the cubature points x^k-1i.(7)x^k-1i=x^k-1++nPk-1+ξix^k-1n+i=x^k-1+-nPk-1+ξi,i=1,2,…,n.Evaluate the propagated cubature pointsXki.(8)Xki=fx^k-1i.Estimate the predicted statex^k-.(9)x^k-=12n∑i=12nXki.Estimate the predicted error covariancePk-.(10)Pk-=12n∑i=12nXki-x^k-Xki-x^k-Τ+Qk-1.Measurement Update. Evaluate the cubature points x^ki.(11)x^ki=x^k-+nPk-ξi.x^kn+i=x^k--nPk-ξi,i=1,2,…,n.Evaluate the propagated cubature pointsZki.(12)Zki=hx^ki.Estimate the predicted measurementz^k.(13)z^k=12n∑i=12nZki.Estimate the measurement covariance matrixPz.(14)Pz=12n∑i=12nZki-z^kZki-z^kΤ+Rk.Estimate the cross-covariance matrixPxz.(15)Pxz=12n∑i=12nx^ki-x^k-Zki-z^kΤ.Estimate the Kalman gainKk.(16)Kk=PxzPz-1.Estimate the updated statex^k+.(17)x^k+=x^k-+Kkzk-z^k.Estimate the corresponding error covariancePk+.(18)Pk+=Pk--KkPzKkΤ.From the algorithm we see that 2n points are adopted when approximating the Gaussian pdf. To improve the filtering accuracy, more points, with corresponding weights, are needed.


## 3. Fifth-Degree Cubature Rule and Cubature Filtering Algorithm

### 3.1. Fifth-Degree Cubature Rule

The integralIf, for which it is difficult to find the analytical solution, can be approximated using the cubature rule Rf=∑i=1Lωifxi by selecting the appropriate cubature points and corresponding weights, where xi denotes the cubature points, ωi denotes the weights that do not depend on the integrand, andL denotes the number of cubature points. We will write x=x1x2⋯xn to denote an arbitrary point in realn-dimensional space. By a monomial of degreed, we mean a function of the form x1i1x2i2⋯xnin, where the indices are nonnegative integers such that i1+i2+⋯+in=d. The following definitions and lemma are introduced.Definition 1 (see [21]).
S is a region in n-dimensional space; given x∈S, the fully symmetric set of x, σx, is the set of all points ±xj1±xj2⋯±xjn, where j1j2⋯jn is any permutation of 12⋯n.Definition 2 (see [21]).
A regionS is said to be fully symmetric if and only if x∈S implies σx⊂S.Definition 3 (see [21]).
An integration ruleR is said to be fully symmetric if and only if, whenever x is an abscissa of the ruleR, every element of σx is an abscissa ofR and the same weight corresponds to all abscissas belonging to a given fully symmetric set.Lemma 4 (see [21]).
A fully symmetric ruleR applied to a fully symmetric n-dimensional region S is of degree d if and only if it is exact for all monomials of degree ≤d of the form(19)x12i1x22i2⋯xn2in,i1≥i2≥⋯≥in.The following cubature rule is considered:(20)Rf=ω1f0⋯0+ω2∑fullsymf±λ0⋯0+ω3∑fullsymf±λ±λ0⋯0.The above rule is fully symmetric; therefore, it will be of degree five if it is exact for the monomials 1,x12, x14, and x12x22; thus the following equations are obtained:(21)I1=ω1+2nω2+2nn-1ω3Ix12=2λ2ω2+4n-1λ2ω3Ix14=2λ4ω2+4n-1λ4ω3Ix12x22=4λ4ω3.For the caseI(f)=∫Rnf(x)e-xΤxdx, we have Ix12i1x22i2⋯xn2in=Γi1+1/2i2+1/2⋯in+1/2, where the Gamma function Γz satisfies Γ1/2=π and Γz+1=zΓz; thus we obtain(22)I1=Γ12⋯Γ12︸n=πn/2Ix12=Γ1+12Γ12⋯Γ12︸n-1=12πn/2Ix14=Γ2+12Γ12⋯Γ12︸n-1=34πn/2Ix12x22=Γ1+12Γ1+12Γ12⋯Γ12︸n-2=14πn/2.Formula (21) is combined with formula (22) to solve the following parameters as(23)λ2=32ω1=n2-7n+1818πn/2ω2=4-n18πn/2ω3=136πn/2.Thus the specific form of ruleRf is achieved by substituting formula (23) into formula (20), and the total number of cubature points required for the rule is 1+2Cn1+4Cn2=2n2+1. Furthermore, the integral ∫Rnfxe-xΤxdx is written using the rule in the following form:(24)∫Rnfxe-xΤxdx=n2-7n+1818πn/2f0⋯0+4-n18πn/2∑fullsymf320⋯0+136πn/2∑fullsymf32320⋯0.The following vectors are defined:(25)ei=32×100010⋮⋮⋱⋮001ipi+=ei+ej,i<jpi-=ei-ej,i<ji,j=1,2,…,n,where ·i denotes the ith column of the matrix. From formula (5), the fifth-degree cubature rule approximating the Gaussian weighted integral of the nonlinear function is obtained by combining formula (24) with the vectors defined in formulae (25) as follows:(26)∫RnfxNx;x-,Pxdx=n2-7n+1818fx-+4-n18∑i=1nf2Pxei+x-+f-2Pxei+x-+136∑i=1nn-1/2f2Pxpi++x-+f-2Pxpi++x-+f2Pxpi-+x-+f-2Pxpi-+x-.


### 3.2. Fifth-Degree Cubature Kalman Filter

The following discrete nonlinear dynamic system is considered:(27)xk=fxk-1+wk-1zk=hxk+vkwk~0,Qk,vk~0,Rk,where xk∈Rnx denotes the state vector, zk∈Rnz denotes the measurement vector, f· and h· are known nonlinear functions, and the process noise wk and measurement noise vk are uncorrelated zero mean Gaussian white noise with covariance matrixes Qk and Rk, respectively.The cubature pointsx^ and corresponding weights ω are obtained from the fifth-degree cubature rule shown in formula (26) as follows:(28)x^1=x-x^1+i=x-+2Pxeix^n+1+i=x--2Pxei,i=1,2,…,nx^2n+1+i=x-+2Pxpi+x^n2+3n+2/2+i=x--2Pxpi+x^n2+n+1+i=x-+2Pxpi-x^3n2+n+2/2+i=x--2Pxpi-,i=1,2,…,nn-12ωi=n2-7n+1818,i=14-n18,i=2,3,…,2n+1136,i=2n+2,…,2n2+1.The proposed fifth-degree cubature Kalman filter is deduced by using the points and weights, shown, respectively, in formulae (28), under the Bayesian filtering framework with reference to the third-degree algorithm in [12], and the specific calculation steps are listed as follows.Step 1 (filter initialisation).
(29)
x
^
0
+
=
E
x
0
P
0
+
=
E
x
0
-
x
^
0
+
x
0
-
x
^
0
+
Τ
.Cycle k=1,2,…, and complete the following steps.Step 2 (time update).
Calculate the pointsx^k-1i using thea posteriori state estimation x^k-1+ anda posteriori error covariance matrix Pk-1+.(30)x^k-11=x^k-1+x^k-11+i=x^k-1++2Pk-1+eix^k-1n+1+i=x^k-1+-2Pk-1+ei,i=1,2,…,nx^k-12n+1+i=x^k-1++2Pk-1+pi+x^k-1n2+3n+2/2+i=x^k-1+-2Pk-1+pi+x^k-1n2+n+1+i=x^k-1++2Pk-1+pi-x^k-13n2+n+2/2+i=x^k-1+-2Pk-1+pi-,i=1,2,…,nn-12.
Calculate the nonlinear propagation ofx^k-1i using f·.(31)Xki=fx^k-1i.
Calculate thea priori state estimation x^k- by weighted merging Xki.(32)x^k-=n2-7n+1818Xk1+∑i=22n+14-n18Xki+∑i=2n+22n2+1136Xki.
Calculate thea priori error covariance matrix Pk-.(33)Pk-=n2-7n+1818Xk1-x^k-Xk1-x^k-Τ+∑i=22n+14-n18Xki-x^k-Xki-x^k-Τ+∑i=2n+22n2+1136Xki-x^k-Xki-x^k-Τ+Qk-1.Step 3 (measurement update).
Calculate the pointsx^ki using thea priori state estimation x^k- anda priori error covariance matrix Pk-.(34)x^k1=x^k-x^k1+i=x^k-+2Pk-eix^kn+1+i=x^k--2Pk-ei,i=1,2,…,nx^k2n+1+i=x^k-+2Pk-pi+x^kn2+3n+2/2+i=x^k--2Pk-pi+x^kn2+n+1+i=x^k-+2Pk-pi-x^k3n2+n+2/2+i=x^k--2Pk-pi-,i=1,2,…,nn-12.
Calculate the nonlinear propagation ofx^ki using h·.(35)Zki=hx^ki.
Calculate the predicted measurement valuez^k by weighted merging Zki.(36)z^k=n2-7n+1818Zk1+∑i=22n+14-n18Zki+∑i=2n+22n2+1136Zki.
Calculate the predicted measurement covariance matrixPz.(37)Pz=n2-7n+1818Zk1-z^kZk1-z^kΤ+∑i=22n+14-n18Zki-z^kZki-z^kΤ+∑i=2n+22n2+1136Zki-z^kZki-z^kΤ+Rk.
Calculate the cross-covariance matrixPxz.(38)Pxz=n2-7n+1818x^ki-x^k-Zki-z^kΤ+∑i=22n+14-n18x^ki-x^k-Zki-z^kΤ+∑i=2n+22n2+1136x^ki-x^k-Zki-z^kΤ.
Calculate the Kalman filtering gainKk.(39)Kk=PxzPz-1.
Calculate thea posteriori state estimation x^k+.(40)x^k+=x^k-+Kkzk-z^k.
Calculate thea posteriori error covariance matrix Pk+.(41)Pk+=Pk--KkPzKkΤ.
The pseudocode representing the proposed algorithm is given in Algorithm1.Algorithm 1:The pseudocode of the proposed algorithm. The fifth-degree cubature Kalman filter.
(1) Compute ei=3/2×Ini, pi+=ei+ej,i<j and pi-=ei-ej,i<j;
(2) Input: x^0+, P0+, zk
(3) fork=1toz_lengthdo
(4) Sk-1+←cholPk-1+;
(5) fori=1to2n2+1do
(6)  Calculate x^k-1i using x^k-1+, Sk-1+, ei, pi+ and pi-;
(7)  Xki←fx^k-1i;
(8) end
(9) x^k-←∑i=12n2+1ωiXki;
(10) Pk-←∑i=12n2+1ωiXki-x^k-Xki-x^k-Τ+Qk-1;
(11) Sk-←cholPk-;
(12) fori=1to2n2+1do
(13)  Calculate x^ki using x^k-, Sk-, ei, pi+ and pi-;
(14)  Zki←hx^ki;
(15) end
(16) z^k←∑i=12n2+1Zki;
(17) Pz←∑i=12n2+1ωiZki-z^kZki-z^kΤ+Rk;
(18) Pxz←∑i=12n2+1ωix^ki-x^k-Zki-z^kΤ;
(19) Kk←PxzPz-1;
(20) x^k+←x^k-+Kkzk-z^k;
(21) Pk+←Pk--KkPzKkΤ;
(22) end
(23) Output: x^k+, Pk+Remark 5.
The proposed method is differential free; that is, there is no need to calculate the Jacobian matrix.Remark 6.
Compared with CKF of third degree, the filtering accuracy of the proposed method is improved to fifth degree.Remark 7.
The general method of computation of the cubature rule is given in the proposed filtering method, without dividing the intractable integral into spherical integral and radial integral.


## 3.1. Fifth-Degree Cubature Rule

The integralIf, for which it is difficult to find the analytical solution, can be approximated using the cubature rule Rf=∑i=1Lωifxi by selecting the appropriate cubature points and corresponding weights, where xi denotes the cubature points, ωi denotes the weights that do not depend on the integrand, andL denotes the number of cubature points. We will write x=x1x2⋯xn to denote an arbitrary point in realn-dimensional space. By a monomial of degreed, we mean a function of the form x1i1x2i2⋯xnin, where the indices are nonnegative integers such that i1+i2+⋯+in=d. The following definitions and lemma are introduced.Definition 1 (see [21]).
S is a region in n-dimensional space; given x∈S, the fully symmetric set of x, σx, is the set of all points ±xj1±xj2⋯±xjn, where j1j2⋯jn is any permutation of 12⋯n.Definition 2 (see [21]).
A regionS is said to be fully symmetric if and only if x∈S implies σx⊂S.Definition 3 (see [21]).
An integration ruleR is said to be fully symmetric if and only if, whenever x is an abscissa of the ruleR, every element of σx is an abscissa ofR and the same weight corresponds to all abscissas belonging to a given fully symmetric set.Lemma 4 (see [21]).
A fully symmetric ruleR applied to a fully symmetric n-dimensional region S is of degree d if and only if it is exact for all monomials of degree ≤d of the form(19)x12i1x22i2⋯xn2in,i1≥i2≥⋯≥in.The following cubature rule is considered:(20)Rf=ω1f0⋯0+ω2∑fullsymf±λ0⋯0+ω3∑fullsymf±λ±λ0⋯0.The above rule is fully symmetric; therefore, it will be of degree five if it is exact for the monomials 1,x12, x14, and x12x22; thus the following equations are obtained:(21)I1=ω1+2nω2+2nn-1ω3Ix12=2λ2ω2+4n-1λ2ω3Ix14=2λ4ω2+4n-1λ4ω3Ix12x22=4λ4ω3.For the caseI(f)=∫Rnf(x)e-xΤxdx, we have Ix12i1x22i2⋯xn2in=Γi1+1/2i2+1/2⋯in+1/2, where the Gamma function Γz satisfies Γ1/2=π and Γz+1=zΓz; thus we obtain(22)I1=Γ12⋯Γ12︸n=πn/2Ix12=Γ1+12Γ12⋯Γ12︸n-1=12πn/2Ix14=Γ2+12Γ12⋯Γ12︸n-1=34πn/2Ix12x22=Γ1+12Γ1+12Γ12⋯Γ12︸n-2=14πn/2.Formula (21) is combined with formula (22) to solve the following parameters as(23)λ2=32ω1=n2-7n+1818πn/2ω2=4-n18πn/2ω3=136πn/2.Thus the specific form of ruleRf is achieved by substituting formula (23) into formula (20), and the total number of cubature points required for the rule is 1+2Cn1+4Cn2=2n2+1. Furthermore, the integral ∫Rnfxe-xΤxdx is written using the rule in the following form:(24)∫Rnfxe-xΤxdx=n2-7n+1818πn/2f0⋯0+4-n18πn/2∑fullsymf320⋯0+136πn/2∑fullsymf32320⋯0.The following vectors are defined:(25)ei=32×100010⋮⋮⋱⋮001ipi+=ei+ej,i<jpi-=ei-ej,i<ji,j=1,2,…,n,where ·i denotes the ith column of the matrix. From formula (5), the fifth-degree cubature rule approximating the Gaussian weighted integral of the nonlinear function is obtained by combining formula (24) with the vectors defined in formulae (25) as follows:(26)∫RnfxNx;x-,Pxdx=n2-7n+1818fx-+4-n18∑i=1nf2Pxei+x-+f-2Pxei+x-+136∑i=1nn-1/2f2Pxpi++x-+f-2Pxpi++x-+f2Pxpi-+x-+f-2Pxpi-+x-.


## 3.2. Fifth-Degree Cubature Kalman Filter

The following discrete nonlinear dynamic system is considered:(27)xk=fxk-1+wk-1zk=hxk+vkwk~0,Qk,vk~0,Rk,where xk∈Rnx denotes the state vector, zk∈Rnz denotes the measurement vector, f· and h· are known nonlinear functions, and the process noise wk and measurement noise vk are uncorrelated zero mean Gaussian white noise with covariance matrixes Qk and Rk, respectively.The cubature pointsx^ and corresponding weights ω are obtained from the fifth-degree cubature rule shown in formula (26) as follows:(28)x^1=x-x^1+i=x-+2Pxeix^n+1+i=x--2Pxei,i=1,2,…,nx^2n+1+i=x-+2Pxpi+x^n2+3n+2/2+i=x--2Pxpi+x^n2+n+1+i=x-+2Pxpi-x^3n2+n+2/2+i=x--2Pxpi-,i=1,2,…,nn-12ωi=n2-7n+1818,i=14-n18,i=2,3,…,2n+1136,i=2n+2,…,2n2+1.The proposed fifth-degree cubature Kalman filter is deduced by using the points and weights, shown, respectively, in formulae (28), under the Bayesian filtering framework with reference to the third-degree algorithm in [12], and the specific calculation steps are listed as follows.Step 1 (filter initialisation).
(29)
x
^
0
+
=
E
x
0
P
0
+
=
E
x
0
-
x
^
0
+
x
0
-
x
^
0
+
Τ
.Cycle k=1,2,…, and complete the following steps.Step 2 (time update).
Calculate the pointsx^k-1i using thea posteriori state estimation x^k-1+ anda posteriori error covariance matrix Pk-1+.(30)x^k-11=x^k-1+x^k-11+i=x^k-1++2Pk-1+eix^k-1n+1+i=x^k-1+-2Pk-1+ei,i=1,2,…,nx^k-12n+1+i=x^k-1++2Pk-1+pi+x^k-1n2+3n+2/2+i=x^k-1+-2Pk-1+pi+x^k-1n2+n+1+i=x^k-1++2Pk-1+pi-x^k-13n2+n+2/2+i=x^k-1+-2Pk-1+pi-,i=1,2,…,nn-12.
Calculate the nonlinear propagation ofx^k-1i using f·.(31)Xki=fx^k-1i.
Calculate thea priori state estimation x^k- by weighted merging Xki.(32)x^k-=n2-7n+1818Xk1+∑i=22n+14-n18Xki+∑i=2n+22n2+1136Xki.
Calculate thea priori error covariance matrix Pk-.(33)Pk-=n2-7n+1818Xk1-x^k-Xk1-x^k-Τ+∑i=22n+14-n18Xki-x^k-Xki-x^k-Τ+∑i=2n+22n2+1136Xki-x^k-Xki-x^k-Τ+Qk-1.Step 3 (measurement update).
Calculate the pointsx^ki using thea priori state estimation x^k- anda priori error covariance matrix Pk-.(34)x^k1=x^k-x^k1+i=x^k-+2Pk-eix^kn+1+i=x^k--2Pk-ei,i=1,2,…,nx^k2n+1+i=x^k-+2Pk-pi+x^kn2+3n+2/2+i=x^k--2Pk-pi+x^kn2+n+1+i=x^k-+2Pk-pi-x^k3n2+n+2/2+i=x^k--2Pk-pi-,i=1,2,…,nn-12.
Calculate the nonlinear propagation ofx^ki using h·.(35)Zki=hx^ki.
Calculate the predicted measurement valuez^k by weighted merging Zki.(36)z^k=n2-7n+1818Zk1+∑i=22n+14-n18Zki+∑i=2n+22n2+1136Zki.
Calculate the predicted measurement covariance matrixPz.(37)Pz=n2-7n+1818Zk1-z^kZk1-z^kΤ+∑i=22n+14-n18Zki-z^kZki-z^kΤ+∑i=2n+22n2+1136Zki-z^kZki-z^kΤ+Rk.
Calculate the cross-covariance matrixPxz.(38)Pxz=n2-7n+1818x^ki-x^k-Zki-z^kΤ+∑i=22n+14-n18x^ki-x^k-Zki-z^kΤ+∑i=2n+22n2+1136x^ki-x^k-Zki-z^kΤ.
Calculate the Kalman filtering gainKk.(39)Kk=PxzPz-1.
Calculate thea posteriori state estimation x^k+.(40)x^k+=x^k-+Kkzk-z^k.
Calculate thea posteriori error covariance matrix Pk+.(41)Pk+=Pk--KkPzKkΤ.
The pseudocode representing the proposed algorithm is given in Algorithm1.Algorithm 1:The pseudocode of the proposed algorithm. The fifth-degree cubature Kalman filter.
(1) Compute ei=3/2×Ini, pi+=ei+ej,i<j and pi-=ei-ej,i<j;
(2) Input: x^0+, P0+, zk
(3) fork=1toz_lengthdo
(4) Sk-1+←cholPk-1+;
(5) fori=1to2n2+1do
(6)  Calculate x^k-1i using x^k-1+, Sk-1+, ei, pi+ and pi-;
(7)  Xki←fx^k-1i;
(8) end
(9) x^k-←∑i=12n2+1ωiXki;
(10) Pk-←∑i=12n2+1ωiXki-x^k-Xki-x^k-Τ+Qk-1;
(11) Sk-←cholPk-;
(12) fori=1to2n2+1do
(13)  Calculate x^ki using x^k-, Sk-, ei, pi+ and pi-;
(14)  Zki←hx^ki;
(15) end
(16) z^k←∑i=12n2+1Zki;
(17) Pz←∑i=12n2+1ωiZki-z^kZki-z^kΤ+Rk;
(18) Pxz←∑i=12n2+1ωix^ki-x^k-Zki-z^kΤ;
(19) Kk←PxzPz-1;
(20) x^k+←x^k-+Kkzk-z^k;
(21) Pk+←Pk--KkPzKkΤ;
(22) end
(23) Output: x^k+, Pk+Remark 5.
The proposed method is differential free; that is, there is no need to calculate the Jacobian matrix.Remark 6.
Compared with CKF of third degree, the filtering accuracy of the proposed method is improved to fifth degree.Remark 7.
The general method of computation of the cubature rule is given in the proposed filtering method, without dividing the intractable integral into spherical integral and radial integral.


## 4. Mathematical Model for Orbit Determination

### 4.1. Orbital Dynamics Model

Satellites in orbit are subjected to various perturbations, mainly including nonspherical gravitational perturbation, third body gravitational perturbation, atmospheric drag perturbation, and solar radiation pressure perturbation, among which the J2 nonspherical gravitational perturbation is the most influential perturbation. To describe the in-orbit motion of the satellite more accurately, the orbital dynamics model with J2 perturbation in the earth central fixed (ECF) coordinate system is used as follows to describe the orbit of the satellite [22]:(42)x˙=vxy˙=vyz˙=vzv˙x=ωe2x+2ωe·vy+μxr3J2Rer7.5z2r2-1.5-1+fxv˙y=ωe2y-2ωe·vx+μyr3J2Rer7.5z2r2-1.5-1+fyv˙z=μzr3J2Rer7.5z2r2-4.5-1+fzr=x2+y2+z2,where xyzvxvyvzΤ denotes the position and velocity of satellite in ECF, μ denotes the earth gravity constant, J2 denotes the harmonic coefficient, Re denotes the radius of the earth, ωe denotes the angular velocity of the earth, and fxfyfzΤ is the sum of other perturbations, which can be approximated as zero mean Gaussian white noise in the study. Formula (42) can be written in discrete state equation form as follows:(43)xk=fxk-1+wk-1,where xk=xkykzkvx,kvy,kvz,kΤ denotes the orbital state at time k and wk denotes the process noise.


### 4.2. Radar Measurement Model

The radar measurement model is described in local horizontal (LH) coordinate system, and the transformation matrix from ECF to LH is as follows:(44)M=-sin⁡λcos⁡λ0-sin⁡φcos⁡λ-sin⁡φsin⁡λcos⁡φcos⁡φcos⁡λcos⁡φsin⁡λsin⁡φ,where λ and φ denote the geocentric longitude and the geocentric latitude of radar, respectively, which can also be represented by geocentric coordinates xcyczc. We define ρ=ρxρyρzρ˙xρ˙yρ˙zΤ as the satellite orbital states in LH, and the following equations are obtained:(45)ρxρyρz=M·x-xcy-ycz-zc,ρ˙xρ˙yρ˙z=M·vxvyvz.The geometric relationship between orbital states and radar measurement valuesRR˙AE is obtained as follows:(46)R=ρx2+ρy2+ρz2R˙=ρxρ˙x+ρyρ˙y+ρzρ˙zρx2+ρy2+ρz2A=arctan⁡ρyρxE=arctan⁡ρzρx2+ρy2,where R denotes the ranging value, R˙ denotes the velocity value, A denotes the azimuth angle, and E denotes the elevation angle. Formula (46) is written in the following measurement equation form:(47)zk=hxk+vk,where zk=RkR˙kAkEkΤ denotes the measurement values at time k and vk denotes the measurement noise.


## 4.1. Orbital Dynamics Model

Satellites in orbit are subjected to various perturbations, mainly including nonspherical gravitational perturbation, third body gravitational perturbation, atmospheric drag perturbation, and solar radiation pressure perturbation, among which the J2 nonspherical gravitational perturbation is the most influential perturbation. To describe the in-orbit motion of the satellite more accurately, the orbital dynamics model with J2 perturbation in the earth central fixed (ECF) coordinate system is used as follows to describe the orbit of the satellite [22]:(42)x˙=vxy˙=vyz˙=vzv˙x=ωe2x+2ωe·vy+μxr3J2Rer7.5z2r2-1.5-1+fxv˙y=ωe2y-2ωe·vx+μyr3J2Rer7.5z2r2-1.5-1+fyv˙z=μzr3J2Rer7.5z2r2-4.5-1+fzr=x2+y2+z2,where xyzvxvyvzΤ denotes the position and velocity of satellite in ECF, μ denotes the earth gravity constant, J2 denotes the harmonic coefficient, Re denotes the radius of the earth, ωe denotes the angular velocity of the earth, and fxfyfzΤ is the sum of other perturbations, which can be approximated as zero mean Gaussian white noise in the study. Formula (42) can be written in discrete state equation form as follows:(43)xk=fxk-1+wk-1,where xk=xkykzkvx,kvy,kvz,kΤ denotes the orbital state at time k and wk denotes the process noise.


## 4.2. Radar Measurement Model

The radar measurement model is described in local horizontal (LH) coordinate system, and the transformation matrix from ECF to LH is as follows:(44)M=-sin⁡λcos⁡λ0-sin⁡φcos⁡λ-sin⁡φsin⁡λcos⁡φcos⁡φcos⁡λcos⁡φsin⁡λsin⁡φ,where λ and φ denote the geocentric longitude and the geocentric latitude of radar, respectively, which can also be represented by geocentric coordinates xcyczc. We define ρ=ρxρyρzρ˙xρ˙yρ˙zΤ as the satellite orbital states in LH, and the following equations are obtained:(45)ρxρyρz=M·x-xcy-ycz-zc,ρ˙xρ˙yρ˙z=M·vxvyvz.The geometric relationship between orbital states and radar measurement valuesRR˙AE is obtained as follows:(46)R=ρx2+ρy2+ρz2R˙=ρxρ˙x+ρyρ˙y+ρzρ˙zρx2+ρy2+ρz2A=arctan⁡ρyρxE=arctan⁡ρzρx2+ρy2,where R denotes the ranging value, R˙ denotes the velocity value, A denotes the azimuth angle, and E denotes the elevation angle. Formula (46) is written in the following measurement equation form:(47)zk=hxk+vk,where zk=RkR˙kAkEkΤ denotes the measurement values at time k and vk denotes the measurement noise.


## 5. The Numerical Experiment

The simulation platform is built using the Satellite Tool Kit (STK) and MATLAB, the satellite runs in low-earth sun-synchronous orbit, and the reference orbit data is generated by the High-Precision Orbit Prediction (HPOP) algorithm. In HPOP, the 21-order earth gravity model is taken into account, the atmospheric drag coefficientCd=2.2, the Jacchia-Roberts model is adopted as the atmospheric density model, the solar radiation pressure coefficient Cr=1, the area-mass ratio is 0.02 m2/kg, the third body gravitational perturbations of sun and moon are considered, and the tidal perturbation is considered. The accuracy of ranging, velocity, and angle values is assumed to be 20 m, 0.1 m/s, and 0.015°, respectively. The six orbital elements, including semimajor axis (a), eccentricity (e), inclination (i), Right Ascension of Ascending Node (RAAN), argument of perigee (ω), true anomaly (f), and the latitude and longitude of the radar station, are listed in Table 1.Table 1
The Six orbital elements and the latitude and longitude of the radar station.
Epoch time/UTCG
a/km
e
i/°
Ω/°
ω/°
f/°
Longitude/°
Latitude/°
1 Jul, 2015, 13:05:00
6778.137
0
97.035
279.066
0
0
108.261
29.783The initial filter statex^0+ = -2705129594559818201432177-12527324Τ.The initial covariance matrixP0+ = diag106106106102102102.The access time from radar station to satellite is from 1 July, 2015, 16:14:00 to 1 July, 2015, 16:21:00, and the root mean square error (RMSE) is adopted to evaluate the real-time orbit determination results. The filtering cycle is 1 s, and we ran 200 Monte-Carlo simulations. The UKF and CKF are compared in this experiment to validate the performance of the proposed algorithm. The RMSEs of the three algorithms are shown in Figure1, and the statistical average RMSEs are summarised in Table 2. From the results it may be seen that the orbit determination accuracy obtained by UKF is almost consistent with that of CKF due to the two algorithms being made to adopt the third-degree deterministic sampling method. By contrast with CKF, the proposed fifth-degree cubature Kalman filter is capable of achieving a higher accuracy, and the position accuracy is increased by 3.204 m with velocity accuracy increased by 0.04 m/s. For a low-earth-orbit satellite, the atmospheric drag perturbation has an influence on the orbit, meaning that the state equation (formula (42)) cannot describe the orbit exactly, and if the high-precision orbit perturbation model is used, the excessive computation demand will impair the filtering algorithm’s real-time performance; however, the errors caused by the orbital model are generally acceptable due to the access time of the LEO satellite being short.Table 2
Average RMSE of three algorithms.
Algorithm
Position RMSE (m)
Velocity RMSE (m·s−1)
UKF
27.180
0.362
CKF
27.148
0.347
Proposed algorithm
23.944
0.307Figure 1
Orbit determination RMSEs of three algorithms.
(a)
Position RMSRs
(b)
Velocity RMSEs


## 6. Conclusion

In this paper, a novel fifth-degree cubature Kalman filter is proposed to improve the accuracy of real-time orbit determination by ground-based radar. The integral points and weights in the general cubature rule are solved by matching the monomials with degree not greater than five with their exact values, and then the fifth-degree cubature rule is deduced. The proposed novel fifth-degree cubature Kalman filter, which can achieve a higher filtering accuracy than UKF and CKF, is derived by using the aforementioned rule based on the Bayesian filtering framework. The simulation results show that the position accuracies achieved by CKF and the proposed algorithm are 27.148 m and 23.944 m, respectively, with the velocity accuracies being 0.347 m/s and 0.307 m/s, respectively. Compared with the results of CKF, the position accuracy and velocity accuracy are improved by 3.204 m and 0.04 m/s, respectively, thus verifying the validity of the proposed algorithm.



---

*Source: 8526804-2017-08-24.xml*