# Attitude Estimation Based on the Spherical Simplex Transformation Modified Unscented Kalman Filter

Authors: Jianwei Zhao; Weimin Jia; Rong Wang; Zhixiang Yan
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

An antenna attitude estimation algorithm is proposed to improve the antenna pointing accuracy for the satellite communication on-the-move. The extrapolated angular acceleration is adopted to improve the performance of the time response. The states of the system are modified according to the modification rules. The spherical simplex transformation unscented Kalman filter is used to improve the precision of the estimated attitude and decrease the calculation of the unscented Kalman filter. The experiment results show that the proposed algorithm can improve the instantaneity of the estimated attitude and the precision of the antenna pointing, which meets the requirement of the antenna pointing.


---

## Body

## 1. Introduction

The great demand for the broadband mobile communication has inspired the development of the satcom on-the-move (SOTM) in recent years. SOTM is a mobile communication system based on the fixed-satellite service. SOTM can provide real-time battlefield information. It enables the commander to execute the command effectively, while the soldiers are far from the command post [1]. Except for the military applications, SOTM is also widely used in the security emergency communications and communication support for major events. SOTM has become a hot field all over the world. Measurement and control system is the core aspect of the system to ensure the normal communication and set up a successful satellite link. When the carrier of the mobile satellite communication is affected by the outside interference factors, measurement and control system guarantees that the beam of the satellite antenna points to the satellite [2]. The length of the control time determines the performance of the SOTM. However, adequate time is necessary for the acquisition and the transmission of the signal from the sensors and extra time is needed to correct the pointing error of the antenna. What is worse is that the outside factors such as the environment of the driven route and the great mobility of the carrier do great harm to the attitude estimation, which also causes time lag for the control. As a result, commands cannot be transmitted in time, and the error cannot be compensated for highly dynamic perturbations at the right time.The attitude estimation is related to the motion state of the carrier and the estimated angles are easily affected by the outside factors. In [3], an adaptive filter is proposed to improve the precision of the estimation. The maneuvering acceleration of the carrier is estimated and corrected to improve the performance of the attitude estimation. However, the adaptive filter based on the covariance matching is a crude method for precise attitude estimation in nature. Therefore, the performance is poor in situation of highly dynamic perturbations for the reason that the windowed innovation sequences cannot catch the carrier dynamics exactly. In [4], an adaptive switch method is adopted to deal with the external acceleration. The attitudes are got by integrating the output of the gyros. The vehicle acceleration is detected by the switch rules. However, it is not sufficient to be suitable for the various dynamics of the carrier and the accuracy of the estimation is restricted. Reference [5] presents a low-cost attitude estimation algorithm based on the GPS/INS integration; the precision of the attitude is improved by the assistance of the GPS-measured course angle and the cascaded Kalman filter. But the cascaded Kalman filter increases the calculation of the system. The instantaneity of the algorithm is worse under the condition of strong maneuvering. Many previous works have been done to improve the precision of the attitude [6–8]. Reference [8] proposes a novel degradation to predict the drift errors of the inertial navigation system. The degrading trend of the drift errors can be predicted in real-time, which improves the precision of the estimated states. However, they all ignored the instantaneity and antijamming capability under strong maneuvering situation.An attitude estimation based on the spherical simplex transformation modified UKF was presented to improve the performance of the SOTM to ensure the antenna pointing to the satellite precisely. The real-time character of the system had been improved by adding the extrapolated angle accelerator to the states of the system. The angle rates of gyroscope could be evaluated precisely. In addition, the rules for the judgment of the motion state increased the antijamming capacity of the system. Strong maneuvering situation could be detected by the setting rules. The predicted covariance was corrected by the weighting function automatically. It overcomes the shortcoming of the traditional Kalman filter, whose covariance cannot be synchronized with the change of target state [9]. What is more, the spherical simplex transformation UKF not only improved the precision of the attitude estimation, but also decreased the calculation of the system [10]. As a result, the proposed attitude estimation system was validated through field tests.The paper is organized as follows. In Section2, we present some preliminaries of the antenna attitude estimation system for SOTM. In Section 3, we elaborate the proposed algorithm in detail. The effectiveness of the proposed attitude estimation system is validated through field tests in Section 4. Finally, the paper is concluded in Section 5.


## 2. Preliminaries

This section offers basic knowledge for the attitude estimation, such as the definition of the reference frames, attitude representation methods, and the algorithms used in this paper.


### 2.1. Reference Frames and System Configuration

Reference frames are the foundation of the attitude measurement system (Figure1). The frames related to attitude estimation are defined as follows.Figure 1
Reference frames.The local geodetic coordinate frame: it is called the north-east-down frame, denoted by then-frame. The vector expressed in the n-frame is expressed by the superscript n
.For instance, R in this frame is denoted by R
n.The vehicle-fixed coordinate frame: it is also called the body coordinate frame (b-frame), which is associated with the vehicle and aligned with its forward, right, and upward axes. The vector expressed in the b-frame is expressed by the superscript b
.For instance, R in this frame is denoted by R
b.The beam coordinate frame: itsz
s-axis points to the target satellite. R
s is defined as a vector expressed in s-frame.Given the longitudeλ and latitude φ of the local earth station and the longitude λ
s of the synchronous orbiting satellite, we can get the azimuth (A), elevation (E), and polarization (V) angles, which are used to give a reference for the antenna beam pointing to the satellite. The formulas are as follows:
(1)
A
=
180
∘
+
tan
-
1
(
tan
(
λ
-
λ
s
)
sin
φ
)
,
E
=
ta
n
-
1
(
cos
⁡
φ
cos
⁡
(
λ
-
λ
s
)
-
R
E
/
(
R
E
+
h
E
)
1
-
[
cos
⁡
φ
cos
⁡
(
λ
-
λ
s
)
]
2
)
,
V
=
ta
n
-
1
(
sin
(
λ
-
λ
s
)
tan
φ
)
,
where R
E is the radius of the earth and h
E is the elevation of the local area.


### 2.2. Attitude Representation

Several attitude representations can be used for vehicle kinematics, such as quaternion, Euler angles, and modified Rodrigues [11]. The representation of Euler angles is the minimal representation that has no redundancy. Therefore, Euler angles are adopted to parameterize the representation of the three-dimensional attitude ψ
,
θ
,
ϕ. The transformation from one frame to another can be realized through three continuous rotations [12]. Each rotation can be represented by one direction cosine matrix. The transformation from the local geodetic coordinate frame to the vehicle-fixed coordinate frame can be represented by the following three matrixes:
(2)
C
1
=
[
cos
⁡
ψ
sin
ψ
0
-
sin
ψ
cos
⁡
ψ
0
0
0
1
]
,
C
2
=
[
cos
⁡
θ
0
-
sin
θ
0
1
0
sin
θ
0
cos
⁡
θ
]
,
C
3
=
[
1
0
0
0
cos
⁡
φ
sin
φ
0
-
sin
φ
cos
⁡
φ
]
.The transformation matrix can be derived by the product of the anterior matrix(3)
C
n
b
=
C
3
C
2
C
1
.
We can get the transformation matrix as follows:
(4)
C
n
b
=
[
C
θ
S
ψ
C
θ
S
ψ
-
S
θ
-
C
φ
S
ψ
+
S
φ
S
θ
C
ψ
C
φ
C
ψ
+
S
φ
S
θ
S
ψ
S
φ
C
θ
S
φ
S
ψ
+
C
φ
S
θ
C
ψ
-
S
φ
C
ψ
+
C
φ
S
θ
S
ψ
C
φ
C
θ
]
,
where the notations C and S represent cosine and sine, respectively.The relationship between the euler angles rates and the measurements of the gyroscope is described as follows [13]:
(5)
[
θ
˙
φ
˙
ψ
˙
]
=
[
0
cos
⁡
φ
-
sin
φ
1
sin
φ
tan
θ
cos
⁡
φ
tan
θ
0
sin
φ
cos
⁡
θ
cos
⁡
φ
cos
⁡
θ
]
[
ω
x
ω
y
ω
z
]
,
where ω
x, ω
y, and ω
z are the outputs of three gyros.


#### 2.2.1. Unscented Kalman Filter

For the reason that trigonometric functions are involved in the Euler angle attitude representation, the extended Kalman filter (EKF) is not in point for its big lineation error. Whereas the unscented Kalman filter is a nonlinear filter that can obtain the estimation to at least second order [14], the expected error is much lower than the EKF. In addition, UKF needs not to calculate the Jacobian matrix used in the EKF, even though the calculation is still larger than the EKF. Now many researches are done to reduce the calculation of the UKF.The nonlinear system model can be given by [15]
(6)
x
k
=
f
(
x
k
-
1
,
ω
k
-
1
)
,
y
k
=
h
(
x
k
,
υ
k
)
,
where x
k is the n
×
1 state vector of the nonlinear system and y
k is the m
×
1 measurement vector. f
(
·
) is the state propagation equation and h
(
·
) is the measurement propagation equation. The process noise ω
k and the measurement noise υ
k are zero-mean Gaussian noise processes with covariances Q
k and R
k, respectively. UKF is briefly presented here. The UKF is performed as follows.(1)
Initialization of the states vectorX
^
0 and the convenience P
0
(7)
X
^
0
=
E
[
X
^
0
]
,
P
0
=
E
[
(
X
0
-
X
^
0
)
(
X
0
-
X
^
0
)
T
]
.
(2)
Choosing of the sampling sigma points(8)
σ
k
⟶
2
n
±
(
n
+
λ
)
[
P
k
+
Q
k
]
,
χ
k
(
0
)
=
X
^
0
,
χ
k
(
i
)
=
σ
k
(
i
)
+
X
^
k
.
(3)
Time update(9)
X
k
-
=
∑
i
=
0
2
n
W
i
(
m
)
(
ζ
k
-
)
i
,
P
k
-
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ζ
k
-
)
i
-
X
k
-
)
(
(
ζ
k
-
)
i
-
X
k
-
)
T
+
Q
k
,
where (
ζ
k
-
)
i
=
f
(
(
χ
k
-
1
)
i
) and W
i
(
m
) is the corresponding weight calculated by the unscented transformation (UT) theory.
(4)
Measurement update(10)
x
^
k
=
x
k
-
+
K
k
(
y
k
-
y
k
-
)
,
P
k
=
P
k
-
-
K
k
P
y
y
K
k
T
,
where
(11)
K
k
=
P
x
y
P
y
y
-
1
,
(
ξ
k
-
)
i
=
h
(
(
ζ
k
-
)
i
)
,
y
k
-
=
∑
i
=
0
2
n
W
i
(
m
)
(
ξ
k
-
)
i
,
P
x
y
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ζ
k
-
)
i
-
x
k
-
)
(
(
ξ
k
-
)
i
-
y
k
-
)
T
,
P
y
y
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ξ
k
-
)
i
-
y
k
-
)
(
(
ξ
k
-
)
i
-
y
k
-
)
T
+
R
k
.Because SOTM is a real-time system, the calculation is one of the keys to ensure the normal operation. Measures must be taken to ensure the precision and the real-time of the system.


## 2.1. Reference Frames and System Configuration

Reference frames are the foundation of the attitude measurement system (Figure1). The frames related to attitude estimation are defined as follows.Figure 1
Reference frames.The local geodetic coordinate frame: it is called the north-east-down frame, denoted by then-frame. The vector expressed in the n-frame is expressed by the superscript n
.For instance, R in this frame is denoted by R
n.The vehicle-fixed coordinate frame: it is also called the body coordinate frame (b-frame), which is associated with the vehicle and aligned with its forward, right, and upward axes. The vector expressed in the b-frame is expressed by the superscript b
.For instance, R in this frame is denoted by R
b.The beam coordinate frame: itsz
s-axis points to the target satellite. R
s is defined as a vector expressed in s-frame.Given the longitudeλ and latitude φ of the local earth station and the longitude λ
s of the synchronous orbiting satellite, we can get the azimuth (A), elevation (E), and polarization (V) angles, which are used to give a reference for the antenna beam pointing to the satellite. The formulas are as follows:
(1)
A
=
180
∘
+
tan
-
1
(
tan
(
λ
-
λ
s
)
sin
φ
)
,
E
=
ta
n
-
1
(
cos
⁡
φ
cos
⁡
(
λ
-
λ
s
)
-
R
E
/
(
R
E
+
h
E
)
1
-
[
cos
⁡
φ
cos
⁡
(
λ
-
λ
s
)
]
2
)
,
V
=
ta
n
-
1
(
sin
(
λ
-
λ
s
)
tan
φ
)
,
where R
E is the radius of the earth and h
E is the elevation of the local area.


## 2.2. Attitude Representation

Several attitude representations can be used for vehicle kinematics, such as quaternion, Euler angles, and modified Rodrigues [11]. The representation of Euler angles is the minimal representation that has no redundancy. Therefore, Euler angles are adopted to parameterize the representation of the three-dimensional attitude ψ
,
θ
,
ϕ. The transformation from one frame to another can be realized through three continuous rotations [12]. Each rotation can be represented by one direction cosine matrix. The transformation from the local geodetic coordinate frame to the vehicle-fixed coordinate frame can be represented by the following three matrixes:
(2)
C
1
=
[
cos
⁡
ψ
sin
ψ
0
-
sin
ψ
cos
⁡
ψ
0
0
0
1
]
,
C
2
=
[
cos
⁡
θ
0
-
sin
θ
0
1
0
sin
θ
0
cos
⁡
θ
]
,
C
3
=
[
1
0
0
0
cos
⁡
φ
sin
φ
0
-
sin
φ
cos
⁡
φ
]
.The transformation matrix can be derived by the product of the anterior matrix(3)
C
n
b
=
C
3
C
2
C
1
.
We can get the transformation matrix as follows:
(4)
C
n
b
=
[
C
θ
S
ψ
C
θ
S
ψ
-
S
θ
-
C
φ
S
ψ
+
S
φ
S
θ
C
ψ
C
φ
C
ψ
+
S
φ
S
θ
S
ψ
S
φ
C
θ
S
φ
S
ψ
+
C
φ
S
θ
C
ψ
-
S
φ
C
ψ
+
C
φ
S
θ
S
ψ
C
φ
C
θ
]
,
where the notations C and S represent cosine and sine, respectively.The relationship between the euler angles rates and the measurements of the gyroscope is described as follows [13]:
(5)
[
θ
˙
φ
˙
ψ
˙
]
=
[
0
cos
⁡
φ
-
sin
φ
1
sin
φ
tan
θ
cos
⁡
φ
tan
θ
0
sin
φ
cos
⁡
θ
cos
⁡
φ
cos
⁡
θ
]
[
ω
x
ω
y
ω
z
]
,
where ω
x, ω
y, and ω
z are the outputs of three gyros.


### 2.2.1. Unscented Kalman Filter

For the reason that trigonometric functions are involved in the Euler angle attitude representation, the extended Kalman filter (EKF) is not in point for its big lineation error. Whereas the unscented Kalman filter is a nonlinear filter that can obtain the estimation to at least second order [14], the expected error is much lower than the EKF. In addition, UKF needs not to calculate the Jacobian matrix used in the EKF, even though the calculation is still larger than the EKF. Now many researches are done to reduce the calculation of the UKF.The nonlinear system model can be given by [15]
(6)
x
k
=
f
(
x
k
-
1
,
ω
k
-
1
)
,
y
k
=
h
(
x
k
,
υ
k
)
,
where x
k is the n
×
1 state vector of the nonlinear system and y
k is the m
×
1 measurement vector. f
(
·
) is the state propagation equation and h
(
·
) is the measurement propagation equation. The process noise ω
k and the measurement noise υ
k are zero-mean Gaussian noise processes with covariances Q
k and R
k, respectively. UKF is briefly presented here. The UKF is performed as follows.(1)
Initialization of the states vectorX
^
0 and the convenience P
0
(7)
X
^
0
=
E
[
X
^
0
]
,
P
0
=
E
[
(
X
0
-
X
^
0
)
(
X
0
-
X
^
0
)
T
]
.
(2)
Choosing of the sampling sigma points(8)
σ
k
⟶
2
n
±
(
n
+
λ
)
[
P
k
+
Q
k
]
,
χ
k
(
0
)
=
X
^
0
,
χ
k
(
i
)
=
σ
k
(
i
)
+
X
^
k
.
(3)
Time update(9)
X
k
-
=
∑
i
=
0
2
n
W
i
(
m
)
(
ζ
k
-
)
i
,
P
k
-
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ζ
k
-
)
i
-
X
k
-
)
(
(
ζ
k
-
)
i
-
X
k
-
)
T
+
Q
k
,
where (
ζ
k
-
)
i
=
f
(
(
χ
k
-
1
)
i
) and W
i
(
m
) is the corresponding weight calculated by the unscented transformation (UT) theory.
(4)
Measurement update(10)
x
^
k
=
x
k
-
+
K
k
(
y
k
-
y
k
-
)
,
P
k
=
P
k
-
-
K
k
P
y
y
K
k
T
,
where
(11)
K
k
=
P
x
y
P
y
y
-
1
,
(
ξ
k
-
)
i
=
h
(
(
ζ
k
-
)
i
)
,
y
k
-
=
∑
i
=
0
2
n
W
i
(
m
)
(
ξ
k
-
)
i
,
P
x
y
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ζ
k
-
)
i
-
x
k
-
)
(
(
ξ
k
-
)
i
-
y
k
-
)
T
,
P
y
y
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ξ
k
-
)
i
-
y
k
-
)
(
(
ξ
k
-
)
i
-
y
k
-
)
T
+
R
k
.Because SOTM is a real-time system, the calculation is one of the keys to ensure the normal operation. Measures must be taken to ensure the precision and the real-time of the system.


## 2.2.1. Unscented Kalman Filter

For the reason that trigonometric functions are involved in the Euler angle attitude representation, the extended Kalman filter (EKF) is not in point for its big lineation error. Whereas the unscented Kalman filter is a nonlinear filter that can obtain the estimation to at least second order [14], the expected error is much lower than the EKF. In addition, UKF needs not to calculate the Jacobian matrix used in the EKF, even though the calculation is still larger than the EKF. Now many researches are done to reduce the calculation of the UKF.The nonlinear system model can be given by [15]
(6)
x
k
=
f
(
x
k
-
1
,
ω
k
-
1
)
,
y
k
=
h
(
x
k
,
υ
k
)
,
where x
k is the n
×
1 state vector of the nonlinear system and y
k is the m
×
1 measurement vector. f
(
·
) is the state propagation equation and h
(
·
) is the measurement propagation equation. The process noise ω
k and the measurement noise υ
k are zero-mean Gaussian noise processes with covariances Q
k and R
k, respectively. UKF is briefly presented here. The UKF is performed as follows.(1)
Initialization of the states vectorX
^
0 and the convenience P
0
(7)
X
^
0
=
E
[
X
^
0
]
,
P
0
=
E
[
(
X
0
-
X
^
0
)
(
X
0
-
X
^
0
)
T
]
.
(2)
Choosing of the sampling sigma points(8)
σ
k
⟶
2
n
±
(
n
+
λ
)
[
P
k
+
Q
k
]
,
χ
k
(
0
)
=
X
^
0
,
χ
k
(
i
)
=
σ
k
(
i
)
+
X
^
k
.
(3)
Time update(9)
X
k
-
=
∑
i
=
0
2
n
W
i
(
m
)
(
ζ
k
-
)
i
,
P
k
-
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ζ
k
-
)
i
-
X
k
-
)
(
(
ζ
k
-
)
i
-
X
k
-
)
T
+
Q
k
,
where (
ζ
k
-
)
i
=
f
(
(
χ
k
-
1
)
i
) and W
i
(
m
) is the corresponding weight calculated by the unscented transformation (UT) theory.
(4)
Measurement update(10)
x
^
k
=
x
k
-
+
K
k
(
y
k
-
y
k
-
)
,
P
k
=
P
k
-
-
K
k
P
y
y
K
k
T
,
where
(11)
K
k
=
P
x
y
P
y
y
-
1
,
(
ξ
k
-
)
i
=
h
(
(
ζ
k
-
)
i
)
,
y
k
-
=
∑
i
=
0
2
n
W
i
(
m
)
(
ξ
k
-
)
i
,
P
x
y
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ζ
k
-
)
i
-
x
k
-
)
(
(
ξ
k
-
)
i
-
y
k
-
)
T
,
P
y
y
=
∑
i
=
0
2
n
W
i
(
c
)
(
(
ξ
k
-
)
i
-
y
k
-
)
(
(
ξ
k
-
)
i
-
y
k
-
)
T
+
R
k
.Because SOTM is a real-time system, the calculation is one of the keys to ensure the normal operation. Measures must be taken to ensure the precision and the real-time of the system.


## 3. The Proposed Attitude Estimation Algorithm

Methods to improve the performance of the attitude estimation are presented in this section:(
1
) model built based on the addition of the angle acceleration; (
2
) the states modification and its realization; (
3
) the proposed UKF algorithm.


### 3.1. Modeling of the Attitude Estimation System

The traditional attitude estimation uses inertial measurement units with high precision as the sensors. As the angle acceleration has no direct observation, only angle and angle rates are chosen as the states of the system. The estimated errors cannot be ignored for lacking the observation of the angular acceleration. The estimation of the system states may suffer from impact and the response is likely with step response, as is shown in Figure2.Figure 2
The time lag problem.Ignoring the impact of the time lag caused by the signal transmission and processing, the main limiters to the attitude estimation are the sampling rate. In the interval, the disturbance cannot be sensed in time and the caused error cannot be corrected at the same time. The antijamming ability of the system is weak so the error cannot meet the requirements of beam pointing in the situation of strong maneuvering. Therefore, the extrapolated angle acceleration is proposed to be added to the system states.The system states becomeX
=
[
φ
ω
a
ω
]; φ, ω, and a
ω are the angles, angle rates, and the angle accelerations, respectively.The observations of the extrapolated angle acceleration can be obtained by the difference between the adjacent measurements of the angle rates, as follows:(12)
a
ω
k
=
(
ω
k
-
ω
k
-
1
)
d
t
,
where d
t is the sampling time interval.The system equation is(13)
φ
=
A
ω
,
where A is the coefficient matrix, with the following form:
(14)
A
=
[
0
cos
⁡
φ
-
sin
φ
1
sin
φ
tan
θ
cos
⁡
φ
tan
θ
0
sin
φ
cos
⁡
θ
cos
⁡
φ
cos
⁡
θ
]
.It is obvious that the system equation is nonlinear. As the measurements are the attitude, angle rates, and angle accelerations, the measurement equation is(15)
Z
=
[
ϕ
ω
a
ω
]
=
[
I
3
×
3
d
t
·
I
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
d
t
·
I
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
]
X
.The advantage of using extrapolated angular acceleration is that it is immune to the estimated states at the previous time. Additionally, it can also reflect the real states of the carrier.


### 3.2. The States Modification Rules

An algorithm based on the states modification is suggested to improve the antijamming ability of the SOTM in this section.


#### 3.2.1. The States Modification Rules

In order to sense the attitude changes in time, the following three rules are set. They determine whether the estimated value of the attitude angle is to be compensated or not.(a) The gyro outputs of the yaw axis, pitch axis, and roll axis are used to determine whether there is sudden disturbance. The disturbance judgment rule is as follows:(16)
|
ω
x
-
bia
s
x
|
≥
α
,
|
ω
y
-
bia
s
y
|
≥
β
,
|
ω
z
-
bia
s
z
|
≥
γ
,
where bia
s
x, bia
s
y, and bia
s
z are the biases drift of the outputs of the angle rates. α, β, and γ are the thresholds used to detect the disturbance.The first rule means that when any angle rate of the three-axis gyro output exceeds the threshold value, it is necessary to compensate for the predicted value of the state.(b) The adjacent yaw angle changes are adopted to determine whether there is a sudden turn. The rule can be expressed by the following style:(17)
|
ψ
k
-
ψ
k
-
1
|
≥
η
,
where |
ψ
k
-
ψ
k
-
1
| means the error of the adjacent yaw angles. η is the threshold used for the turn detection.The second rule means that the states modification can be determined by the adjacent yaw angle. When the error between the adjacent yaw angles is larger than the threshold, measures should be taken to modify the estimated states.(c) The location information of the carrier is used to detect the sudden disturbance. The rule is given as follows:(18)
|
ψ
k
′
-
ψ
k
-
1
′
|
≥
μ
,
where ψ
′ means measurement yaw angle; it can be obtained by the location information of the carrier. It is different from rule (b) that may still suffer from time lag in fact, as the estimated angle is still slower than the practical value in response. In order to overcome the time lag, the location information can be used to detect the sudden disturbance. μ is the threshold used for the turn detection which can be got from the empirical value of the dynamic carrier test and the product manual. The principle is shown in Figure 3.Figure 3
The principle of using the location information.The aiming yaw angleξ
k
+
1 is derived from the error between the estimated location in time k and the measurement location of the GPS in time k
+
1. The known yaw angle ψ
k
′ is derived from the location in time k and in time k
-
1. When the error between the two yaw angles is small which can be ignored, it means there is no disturbance. On the other hand, a large error means there is a large disturbance. Therefore, the choice of μ plays an important role in the attitude estimation. If μ is larger than the practical one, the turn detection may be inefficient. If μ is smaller than the practical one, the turn detection may be too sensitive which causes the wrong justification.


#### 3.2.2. The States Modification

Based on the proposed rules, the weighting factor can be chosen to realize the algorithm based on the prediction filter.Theψ
k
′ can be determined by
(19)
ψ
k
′
=
arctan
(
x
^
(
k
∣
k
)
-
x
^
(
k
-
1
∣
k
-
1
)
y
^
(
k
∣
k
)
-
y
^
(
k
-
1
∣
k
-
1
)
)
.Theξ
k
+
1 can be determined by
(20)
ξ
k
+
1
=
arctan
(
z
(
k
+
1
)
-
x
^
(
k
∣
k
)
z
(
k
+
1
)
-
y
^
(
k
∣
k
)
)
.TheΔ
ψ
k
+
1 can be determined by
(21)
Δ
ψ
k
+
1
=
ψ
k
′
-
ξ
k
+
1
,
where |
Δ
ψ
k
+
1
| is uniformly distributed in the interval [
0
π
].When mutation of the carrier occurs,|
Δ
ψ
k
+
1
| increases at the same time. Therefore, |
Δ
ψ
k
+
1
| can be considered as input variables. The Gaussian function with the centers 0 and π can be used as the weighting function of the algorithm. The weights of the previous yaw angle and the real-time measured yaw angle could be determined adaptively according to the changes of |
Δ
ψ
k
+
1
|.The expression of the weighting function is(22)
u
j
(
k
+
1
)
=
exp
⁡
[
-
1
2
(
|
Δ
ψ
k
+
1
|
-
u
j
σ
j
)
2
]
,
j
=
1,2
,
where u
j and σ
j are the center value and the standard deviation of the Gaussian function, respectively. When the center value of the Gaussian function is zero, it means the carrier has no sudden disturbance. When the central value of the Gaussian function is π, itmeans that the carrier has strong maneuvering.The extrapolated angular acceleration can be expressed by(23)
x
¨
^
W
T
(
k
+
1
)
=
2
{
[
z
(
k
+
1
)
-
x
^
(
k
∣
k
)
]
/
Δ
t
-
x
˙
^
(
k
∣
k
)
}
Δ
t
,
where x
¨
^
W
T is the extrapolated angular acceleration, z
(
k
+
1
) is the measured value of the three-dimensional attitude angle, x
˙
^
(
k
∣
k
) is the estimated values for the last time, and Δ
t is sampling interval of the sensor.As the angular acceleration is based on the measurement dates, it is easily affected by the measurement noise. Under conditions of weak maneuver, the algorithm has good stability and the accuracy meets the actual communication needs. In order to ensure the predicted value of the angular acceleration to have both good stability and real-time ability during the strong maneuver, the predicted angular acceleration is modified by the extrapolation angular acceleration according to the carrier’s Kinematics equation. The expression is given by(24)
x
¨
^
L
C
(
k
+
1
∣
k
)
=
u
1
(
k
+
1
)
x
¨
^
(
k
+
1
∣
k
)
+
u
2
(
k
+
1
)
x
¨
^
W
T
(
k
+
1
∣
k
)
u
1
(
k
+
1
)
+
u
2
(
k
+
1
)
,
where x
¨
^
(
k
+
1
∣
k
) is the predicted value of the angular acceleration.The predicted value of the angular rates can be obtained by the previous predicted value and the predicted angular acceleration:(25)
x
˙
^
L
C
(
k
+
1
∣
k
)
=
x
˙
^
(
k
∣
k
-
1
)
+
T
x
¨
^
L
C
(
k
+
1
∣
k
)
.By improving the model and algorithm, the one-step prediction states value can be modified according to the states modification roles automatically. The one-step states can be obtained by(26)
x
^
NEW
(
k
+
1
∣
k
)
=
u
1
(
k
+
1
)
x
^
(
k
+
1
∣
k
)
+
u
2
(
k
+
1
)
x
^
L
C
(
k
+
1
∣
k
)
u
1
(
k
+
1
)
+
u
2
(
k
+
1
)
,
where x
^
(
k
+
1
∣
k
) is the one-step prediction state from Kalman filter and x
^
L
C
(
k
+
1
∣
k
) is the measurement prediction state value.In conclusion, when the carrier moves normally, we can getu
1
(
k
+
1
)
≈
1,  u
1
(
k
+
1
)
≈
0. In fact, it is similar to the traditional algorithm. When the carrier moves with strong maneuver, u
1
(
k
+
1
)
≈
0 and u
1
(
k
+
1
)
≈
1. In this situation, the precision of the attitude estimation is improved by the modification. The final estimated value of the attitude is
(27)
x
^
(
k
+
1
∣
k
+
1
)
=
x
^
NEW
(
k
+
1
∣
k
)
+
K
k
(
k
+
1
)
×
[
z
(
k
+
1
)
-
K
(
k
+
1
)
x
^
(
k
+
1
k
)
]
.


### 3.3.  The Spherical Simplex Transformation Prediction UKF

The calculation of UKF is related to the number of sigma points. More sigma points mean more computation. For then-dimensional state vector, the standard UKF adopts symmetric sampling strategy; 2
n
+
1 sigma points are needed to approximate the states’ probability distribution [16]. In order to decrease the computation of the UKF, symmetric sampling is replaced by super spherical sampling. The distance between the sigma points and the center point is the same, and the weight is also the same. n
+
1 sampling points are needed to describe its mean and variance. Adding the mean value of the states, the total number of the super spherical sampling UKF is n
+
2, and the computation decreases obviously. The algorithm is as follows:(1)
chooseW
0,
(2)
making sure of the weight ofW
i
(28)
W
i
=
1
-
W
0
n
+
1
,
i
=
1,2
,
…
,
n
+
1
,
(3)
scale correction(29)
ω
i
=
{
1
+
W
0
-
1
α
2
,
i
=
0
W
i
α
2
,
i
≠
0
,
where α
(
0
≤
α
≤
1
) is the scaling factor that can control the distance between the sampling points and the mean value of the sigma points,
(4)
initialization of the vector sequence(30)
χ
0
1
=
[
0
]
,
χ
1
1
=
[
-
1
2
ω
1
]
,
χ
2
1
=
[
1
2
ω
1
]
,
(5)
for the dimension of the inputj
=
2
,
…
,
n, the iteration formula is
(31)
χ
j
i
=
{
[
χ
0
j
0
]
,
i
=
0
[
χ
j
i
-
1
j
(
j
+
1
)
ω
1
]
,
i
=
1
,
…
,
j
[
0
j
-
1
j
j
(
j
+
1
)
ω
1
]
,
i
=
j
+
1
,
where j is the dimension of the vector and i is the sequence of the sampling points,
(6)
for any of the means and covariance, sigma can be obtained by the following formula:(32)
χ
i
=
x
-
+
S
χ
j
i
,
i
=
0,1
,
…
,
n
+
1
,
where S is the square root of the covariance matrix by Cholesky decomposition.The process of the proposed algorithm is shown in Figure4. MEMS sensor such as gyro and accelerometer and GPS are integrated to give the initial output of the attitude and the angle rates, and then the modification rules are used for the justification on the states of the carrier. The symmetric sampling transformation prediction UKF is adopted eventually to improve the performance of the estimation.Figure 4
The process of the proposed algorithm.


## 3.1. Modeling of the Attitude Estimation System

The traditional attitude estimation uses inertial measurement units with high precision as the sensors. As the angle acceleration has no direct observation, only angle and angle rates are chosen as the states of the system. The estimated errors cannot be ignored for lacking the observation of the angular acceleration. The estimation of the system states may suffer from impact and the response is likely with step response, as is shown in Figure2.Figure 2
The time lag problem.Ignoring the impact of the time lag caused by the signal transmission and processing, the main limiters to the attitude estimation are the sampling rate. In the interval, the disturbance cannot be sensed in time and the caused error cannot be corrected at the same time. The antijamming ability of the system is weak so the error cannot meet the requirements of beam pointing in the situation of strong maneuvering. Therefore, the extrapolated angle acceleration is proposed to be added to the system states.The system states becomeX
=
[
φ
ω
a
ω
]; φ, ω, and a
ω are the angles, angle rates, and the angle accelerations, respectively.The observations of the extrapolated angle acceleration can be obtained by the difference between the adjacent measurements of the angle rates, as follows:(12)
a
ω
k
=
(
ω
k
-
ω
k
-
1
)
d
t
,
where d
t is the sampling time interval.The system equation is(13)
φ
=
A
ω
,
where A is the coefficient matrix, with the following form:
(14)
A
=
[
0
cos
⁡
φ
-
sin
φ
1
sin
φ
tan
θ
cos
⁡
φ
tan
θ
0
sin
φ
cos
⁡
θ
cos
⁡
φ
cos
⁡
θ
]
.It is obvious that the system equation is nonlinear. As the measurements are the attitude, angle rates, and angle accelerations, the measurement equation is(15)
Z
=
[
ϕ
ω
a
ω
]
=
[
I
3
×
3
d
t
·
I
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
d
t
·
I
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
]
X
.The advantage of using extrapolated angular acceleration is that it is immune to the estimated states at the previous time. Additionally, it can also reflect the real states of the carrier.


## 3.2. The States Modification Rules

An algorithm based on the states modification is suggested to improve the antijamming ability of the SOTM in this section.


### 3.2.1. The States Modification Rules

In order to sense the attitude changes in time, the following three rules are set. They determine whether the estimated value of the attitude angle is to be compensated or not.(a) The gyro outputs of the yaw axis, pitch axis, and roll axis are used to determine whether there is sudden disturbance. The disturbance judgment rule is as follows:(16)
|
ω
x
-
bia
s
x
|
≥
α
,
|
ω
y
-
bia
s
y
|
≥
β
,
|
ω
z
-
bia
s
z
|
≥
γ
,
where bia
s
x, bia
s
y, and bia
s
z are the biases drift of the outputs of the angle rates. α, β, and γ are the thresholds used to detect the disturbance.The first rule means that when any angle rate of the three-axis gyro output exceeds the threshold value, it is necessary to compensate for the predicted value of the state.(b) The adjacent yaw angle changes are adopted to determine whether there is a sudden turn. The rule can be expressed by the following style:(17)
|
ψ
k
-
ψ
k
-
1
|
≥
η
,
where |
ψ
k
-
ψ
k
-
1
| means the error of the adjacent yaw angles. η is the threshold used for the turn detection.The second rule means that the states modification can be determined by the adjacent yaw angle. When the error between the adjacent yaw angles is larger than the threshold, measures should be taken to modify the estimated states.(c) The location information of the carrier is used to detect the sudden disturbance. The rule is given as follows:(18)
|
ψ
k
′
-
ψ
k
-
1
′
|
≥
μ
,
where ψ
′ means measurement yaw angle; it can be obtained by the location information of the carrier. It is different from rule (b) that may still suffer from time lag in fact, as the estimated angle is still slower than the practical value in response. In order to overcome the time lag, the location information can be used to detect the sudden disturbance. μ is the threshold used for the turn detection which can be got from the empirical value of the dynamic carrier test and the product manual. The principle is shown in Figure 3.Figure 3
The principle of using the location information.The aiming yaw angleξ
k
+
1 is derived from the error between the estimated location in time k and the measurement location of the GPS in time k
+
1. The known yaw angle ψ
k
′ is derived from the location in time k and in time k
-
1. When the error between the two yaw angles is small which can be ignored, it means there is no disturbance. On the other hand, a large error means there is a large disturbance. Therefore, the choice of μ plays an important role in the attitude estimation. If μ is larger than the practical one, the turn detection may be inefficient. If μ is smaller than the practical one, the turn detection may be too sensitive which causes the wrong justification.


### 3.2.2. The States Modification

Based on the proposed rules, the weighting factor can be chosen to realize the algorithm based on the prediction filter.Theψ
k
′ can be determined by
(19)
ψ
k
′
=
arctan
(
x
^
(
k
∣
k
)
-
x
^
(
k
-
1
∣
k
-
1
)
y
^
(
k
∣
k
)
-
y
^
(
k
-
1
∣
k
-
1
)
)
.Theξ
k
+
1 can be determined by
(20)
ξ
k
+
1
=
arctan
(
z
(
k
+
1
)
-
x
^
(
k
∣
k
)
z
(
k
+
1
)
-
y
^
(
k
∣
k
)
)
.TheΔ
ψ
k
+
1 can be determined by
(21)
Δ
ψ
k
+
1
=
ψ
k
′
-
ξ
k
+
1
,
where |
Δ
ψ
k
+
1
| is uniformly distributed in the interval [
0
π
].When mutation of the carrier occurs,|
Δ
ψ
k
+
1
| increases at the same time. Therefore, |
Δ
ψ
k
+
1
| can be considered as input variables. The Gaussian function with the centers 0 and π can be used as the weighting function of the algorithm. The weights of the previous yaw angle and the real-time measured yaw angle could be determined adaptively according to the changes of |
Δ
ψ
k
+
1
|.The expression of the weighting function is(22)
u
j
(
k
+
1
)
=
exp
⁡
[
-
1
2
(
|
Δ
ψ
k
+
1
|
-
u
j
σ
j
)
2
]
,
j
=
1,2
,
where u
j and σ
j are the center value and the standard deviation of the Gaussian function, respectively. When the center value of the Gaussian function is zero, it means the carrier has no sudden disturbance. When the central value of the Gaussian function is π, itmeans that the carrier has strong maneuvering.The extrapolated angular acceleration can be expressed by(23)
x
¨
^
W
T
(
k
+
1
)
=
2
{
[
z
(
k
+
1
)
-
x
^
(
k
∣
k
)
]
/
Δ
t
-
x
˙
^
(
k
∣
k
)
}
Δ
t
,
where x
¨
^
W
T is the extrapolated angular acceleration, z
(
k
+
1
) is the measured value of the three-dimensional attitude angle, x
˙
^
(
k
∣
k
) is the estimated values for the last time, and Δ
t is sampling interval of the sensor.As the angular acceleration is based on the measurement dates, it is easily affected by the measurement noise. Under conditions of weak maneuver, the algorithm has good stability and the accuracy meets the actual communication needs. In order to ensure the predicted value of the angular acceleration to have both good stability and real-time ability during the strong maneuver, the predicted angular acceleration is modified by the extrapolation angular acceleration according to the carrier’s Kinematics equation. The expression is given by(24)
x
¨
^
L
C
(
k
+
1
∣
k
)
=
u
1
(
k
+
1
)
x
¨
^
(
k
+
1
∣
k
)
+
u
2
(
k
+
1
)
x
¨
^
W
T
(
k
+
1
∣
k
)
u
1
(
k
+
1
)
+
u
2
(
k
+
1
)
,
where x
¨
^
(
k
+
1
∣
k
) is the predicted value of the angular acceleration.The predicted value of the angular rates can be obtained by the previous predicted value and the predicted angular acceleration:(25)
x
˙
^
L
C
(
k
+
1
∣
k
)
=
x
˙
^
(
k
∣
k
-
1
)
+
T
x
¨
^
L
C
(
k
+
1
∣
k
)
.By improving the model and algorithm, the one-step prediction states value can be modified according to the states modification roles automatically. The one-step states can be obtained by(26)
x
^
NEW
(
k
+
1
∣
k
)
=
u
1
(
k
+
1
)
x
^
(
k
+
1
∣
k
)
+
u
2
(
k
+
1
)
x
^
L
C
(
k
+
1
∣
k
)
u
1
(
k
+
1
)
+
u
2
(
k
+
1
)
,
where x
^
(
k
+
1
∣
k
) is the one-step prediction state from Kalman filter and x
^
L
C
(
k
+
1
∣
k
) is the measurement prediction state value.In conclusion, when the carrier moves normally, we can getu
1
(
k
+
1
)
≈
1,  u
1
(
k
+
1
)
≈
0. In fact, it is similar to the traditional algorithm. When the carrier moves with strong maneuver, u
1
(
k
+
1
)
≈
0 and u
1
(
k
+
1
)
≈
1. In this situation, the precision of the attitude estimation is improved by the modification. The final estimated value of the attitude is
(27)
x
^
(
k
+
1
∣
k
+
1
)
=
x
^
NEW
(
k
+
1
∣
k
)
+
K
k
(
k
+
1
)
×
[
z
(
k
+
1
)
-
K
(
k
+
1
)
x
^
(
k
+
1
k
)
]
.


## 3.2.1. The States Modification Rules

In order to sense the attitude changes in time, the following three rules are set. They determine whether the estimated value of the attitude angle is to be compensated or not.(a) The gyro outputs of the yaw axis, pitch axis, and roll axis are used to determine whether there is sudden disturbance. The disturbance judgment rule is as follows:(16)
|
ω
x
-
bia
s
x
|
≥
α
,
|
ω
y
-
bia
s
y
|
≥
β
,
|
ω
z
-
bia
s
z
|
≥
γ
,
where bia
s
x, bia
s
y, and bia
s
z are the biases drift of the outputs of the angle rates. α, β, and γ are the thresholds used to detect the disturbance.The first rule means that when any angle rate of the three-axis gyro output exceeds the threshold value, it is necessary to compensate for the predicted value of the state.(b) The adjacent yaw angle changes are adopted to determine whether there is a sudden turn. The rule can be expressed by the following style:(17)
|
ψ
k
-
ψ
k
-
1
|
≥
η
,
where |
ψ
k
-
ψ
k
-
1
| means the error of the adjacent yaw angles. η is the threshold used for the turn detection.The second rule means that the states modification can be determined by the adjacent yaw angle. When the error between the adjacent yaw angles is larger than the threshold, measures should be taken to modify the estimated states.(c) The location information of the carrier is used to detect the sudden disturbance. The rule is given as follows:(18)
|
ψ
k
′
-
ψ
k
-
1
′
|
≥
μ
,
where ψ
′ means measurement yaw angle; it can be obtained by the location information of the carrier. It is different from rule (b) that may still suffer from time lag in fact, as the estimated angle is still slower than the practical value in response. In order to overcome the time lag, the location information can be used to detect the sudden disturbance. μ is the threshold used for the turn detection which can be got from the empirical value of the dynamic carrier test and the product manual. The principle is shown in Figure 3.Figure 3
The principle of using the location information.The aiming yaw angleξ
k
+
1 is derived from the error between the estimated location in time k and the measurement location of the GPS in time k
+
1. The known yaw angle ψ
k
′ is derived from the location in time k and in time k
-
1. When the error between the two yaw angles is small which can be ignored, it means there is no disturbance. On the other hand, a large error means there is a large disturbance. Therefore, the choice of μ plays an important role in the attitude estimation. If μ is larger than the practical one, the turn detection may be inefficient. If μ is smaller than the practical one, the turn detection may be too sensitive which causes the wrong justification.


## 3.2.2. The States Modification

Based on the proposed rules, the weighting factor can be chosen to realize the algorithm based on the prediction filter.Theψ
k
′ can be determined by
(19)
ψ
k
′
=
arctan
(
x
^
(
k
∣
k
)
-
x
^
(
k
-
1
∣
k
-
1
)
y
^
(
k
∣
k
)
-
y
^
(
k
-
1
∣
k
-
1
)
)
.Theξ
k
+
1 can be determined by
(20)
ξ
k
+
1
=
arctan
(
z
(
k
+
1
)
-
x
^
(
k
∣
k
)
z
(
k
+
1
)
-
y
^
(
k
∣
k
)
)
.TheΔ
ψ
k
+
1 can be determined by
(21)
Δ
ψ
k
+
1
=
ψ
k
′
-
ξ
k
+
1
,
where |
Δ
ψ
k
+
1
| is uniformly distributed in the interval [
0
π
].When mutation of the carrier occurs,|
Δ
ψ
k
+
1
| increases at the same time. Therefore, |
Δ
ψ
k
+
1
| can be considered as input variables. The Gaussian function with the centers 0 and π can be used as the weighting function of the algorithm. The weights of the previous yaw angle and the real-time measured yaw angle could be determined adaptively according to the changes of |
Δ
ψ
k
+
1
|.The expression of the weighting function is(22)
u
j
(
k
+
1
)
=
exp
⁡
[
-
1
2
(
|
Δ
ψ
k
+
1
|
-
u
j
σ
j
)
2
]
,
j
=
1,2
,
where u
j and σ
j are the center value and the standard deviation of the Gaussian function, respectively. When the center value of the Gaussian function is zero, it means the carrier has no sudden disturbance. When the central value of the Gaussian function is π, itmeans that the carrier has strong maneuvering.The extrapolated angular acceleration can be expressed by(23)
x
¨
^
W
T
(
k
+
1
)
=
2
{
[
z
(
k
+
1
)
-
x
^
(
k
∣
k
)
]
/
Δ
t
-
x
˙
^
(
k
∣
k
)
}
Δ
t
,
where x
¨
^
W
T is the extrapolated angular acceleration, z
(
k
+
1
) is the measured value of the three-dimensional attitude angle, x
˙
^
(
k
∣
k
) is the estimated values for the last time, and Δ
t is sampling interval of the sensor.As the angular acceleration is based on the measurement dates, it is easily affected by the measurement noise. Under conditions of weak maneuver, the algorithm has good stability and the accuracy meets the actual communication needs. In order to ensure the predicted value of the angular acceleration to have both good stability and real-time ability during the strong maneuver, the predicted angular acceleration is modified by the extrapolation angular acceleration according to the carrier’s Kinematics equation. The expression is given by(24)
x
¨
^
L
C
(
k
+
1
∣
k
)
=
u
1
(
k
+
1
)
x
¨
^
(
k
+
1
∣
k
)
+
u
2
(
k
+
1
)
x
¨
^
W
T
(
k
+
1
∣
k
)
u
1
(
k
+
1
)
+
u
2
(
k
+
1
)
,
where x
¨
^
(
k
+
1
∣
k
) is the predicted value of the angular acceleration.The predicted value of the angular rates can be obtained by the previous predicted value and the predicted angular acceleration:(25)
x
˙
^
L
C
(
k
+
1
∣
k
)
=
x
˙
^
(
k
∣
k
-
1
)
+
T
x
¨
^
L
C
(
k
+
1
∣
k
)
.By improving the model and algorithm, the one-step prediction states value can be modified according to the states modification roles automatically. The one-step states can be obtained by(26)
x
^
NEW
(
k
+
1
∣
k
)
=
u
1
(
k
+
1
)
x
^
(
k
+
1
∣
k
)
+
u
2
(
k
+
1
)
x
^
L
C
(
k
+
1
∣
k
)
u
1
(
k
+
1
)
+
u
2
(
k
+
1
)
,
where x
^
(
k
+
1
∣
k
) is the one-step prediction state from Kalman filter and x
^
L
C
(
k
+
1
∣
k
) is the measurement prediction state value.In conclusion, when the carrier moves normally, we can getu
1
(
k
+
1
)
≈
1,  u
1
(
k
+
1
)
≈
0. In fact, it is similar to the traditional algorithm. When the carrier moves with strong maneuver, u
1
(
k
+
1
)
≈
0 and u
1
(
k
+
1
)
≈
1. In this situation, the precision of the attitude estimation is improved by the modification. The final estimated value of the attitude is
(27)
x
^
(
k
+
1
∣
k
+
1
)
=
x
^
NEW
(
k
+
1
∣
k
)
+
K
k
(
k
+
1
)
×
[
z
(
k
+
1
)
-
K
(
k
+
1
)
x
^
(
k
+
1
k
)
]
.


## 3.3.  The Spherical Simplex Transformation Prediction UKF

The calculation of UKF is related to the number of sigma points. More sigma points mean more computation. For then-dimensional state vector, the standard UKF adopts symmetric sampling strategy; 2
n
+
1 sigma points are needed to approximate the states’ probability distribution [16]. In order to decrease the computation of the UKF, symmetric sampling is replaced by super spherical sampling. The distance between the sigma points and the center point is the same, and the weight is also the same. n
+
1 sampling points are needed to describe its mean and variance. Adding the mean value of the states, the total number of the super spherical sampling UKF is n
+
2, and the computation decreases obviously. The algorithm is as follows:(1)
chooseW
0,
(2)
making sure of the weight ofW
i
(28)
W
i
=
1
-
W
0
n
+
1
,
i
=
1,2
,
…
,
n
+
1
,
(3)
scale correction(29)
ω
i
=
{
1
+
W
0
-
1
α
2
,
i
=
0
W
i
α
2
,
i
≠
0
,
where α
(
0
≤
α
≤
1
) is the scaling factor that can control the distance between the sampling points and the mean value of the sigma points,
(4)
initialization of the vector sequence(30)
χ
0
1
=
[
0
]
,
χ
1
1
=
[
-
1
2
ω
1
]
,
χ
2
1
=
[
1
2
ω
1
]
,
(5)
for the dimension of the inputj
=
2
,
…
,
n, the iteration formula is
(31)
χ
j
i
=
{
[
χ
0
j
0
]
,
i
=
0
[
χ
j
i
-
1
j
(
j
+
1
)
ω
1
]
,
i
=
1
,
…
,
j
[
0
j
-
1
j
j
(
j
+
1
)
ω
1
]
,
i
=
j
+
1
,
where j is the dimension of the vector and i is the sequence of the sampling points,
(6)
for any of the means and covariance, sigma can be obtained by the following formula:(32)
χ
i
=
x
-
+
S
χ
j
i
,
i
=
0,1
,
…
,
n
+
1
,
where S is the square root of the covariance matrix by Cholesky decomposition.The process of the proposed algorithm is shown in Figure4. MEMS sensor such as gyro and accelerometer and GPS are integrated to give the initial output of the attitude and the angle rates, and then the modification rules are used for the justification on the states of the carrier. The symmetric sampling transformation prediction UKF is adopted eventually to improve the performance of the estimation.Figure 4
The process of the proposed algorithm.


## 4. Experimental Validation and Discussions

In order to verify the performance of the proposed algorithm, the experimental platform was constructed. It consists of outdoor and indoor parts. The outdoor part is composed of a high precision attitude and heading reference system XW-ADU7612, a low-cost miniature inertial measurement unit XW-IMU5220, and a single baseline GPS XW-ADU3601. Attitude can be acquired by fusing the output of the XW-IMU5220 and XW-ADU3601. The reference system XW-ADU7612 can provide the attitude to an accuracy of 0.1° (in dynamic and baseline >2 m) for comparison. The algorithm is running in a DSP processor with the frequency 400 MHz in real-time in the indoor parts. The experimental carrier and equipment are shown in Figure5.Figure 5
The experiment platform.The experiment was conducted in a country road. The route of the mobile satellite communication earth station carrier can be seen from Figure6. It is suitable for validating the performance of the algorithm for some strong maneuvers intervals. The thresholds α, β, and γ in the disturbance judgment rule from gyros are all set to be 0.01 rad/s. The adjacent yaw angle change η is set to be 1.5 rad. The parameter u to detect the sudden disturbance in the location information of the carrier is set to be 1.5 rad.Figure 6
The traveling routine of the carrier.The attitude estimated by the integration of XW-IMU5220 and XW-ADU3601 using EKF could be seen from Figures7 and 8. Accurate estimation of the attitude could be got when the carrier moved in a straight line uniformly. The errors of the estimated attitude were all with 0.5° that met the requirement for the mobile satellite communication. However, the errors were increasing when the carrier made a sharp turn, such as in the intervals 14.3 s–27.6 s, 50 s–65 s, and 80 s–110 s. The estimated attitudes value had large error due to the strong maneuvering. The maximum error of the yaw angle was beyond 300° in short time. The error of the pitch angle and the roll angle was within 3° that could not meet the basic requirement of the satellite communication. In order to improve the precision of the attitude estimation, modification must be taken to decrease the effect of the sudden disturbance.Figure 7
The attitude estimation by EKF without prediction filter.Figure 8
The attitude error by EKF without modification.In order to overcome the problem, the extrapolated angular acceleration was adopted to improve the performance of the time response. The states of the system were modified according to the modification rules. The results of the algorithm are shown from Figures9, 10, and 11.Figure 9
The roll angle estimated by MEKF.Figure 10
The pitch angle estimated by MEKF.Figure 11
The yaw angle estimated by MEKF.After the modification, it was obvious that the precision of the attitude estimation had been improved; the error of the pitch angle and the roll angle was within 0.5°, which met the requirement of the antenna pointing. However the error of the yaw angle was still beyond 0.5° in time interval 80 s–110 s. So UKF was proposed to improve the precision. However, the execution time was so long that it could not meet the real-time character of the SOTM. Spherical simplex transformation UKF was adopted to improve the precision of the attitude and to decrease the calculation of the realization of the SOTM. As can be seen from Figure12, the errors of attitude are within 0.5° that meets the requirement of the antenna pointing.Figure 12
The errors of the attitude estimation by the proposed algorithm.In order to verify the efficiency of the algorithm, the RMSE and the execution time were listed in Table1. It is obvious that the RMSE of the suggested MUKF was better than other algorithms. Besides, the execution time was 3.34, which was comparable with MEKF. So the suggested algorithm has a desirable precision and moderate execution time which is suitable for the application of SOTM.Table 1
The RMSE comparison of the four algorithms.
Algorithm
RMSE
Execution time
EKF
6.8
3.28
MEKF
2.8
3.32
UKF
4.6
3.48
MUKF
0.47
3.34


## 5. Conclusion

An attitude estimation algorithm based on the spherical simplex transformation modified UKF is proposed to estimate the attitudes of the vehicle and keep the antenna pointing to the satellite precisely. The extrapolated angle accelerator improves the real-time character of the system. Besides, the antijamming capacity of the system has been increased for the rules for the judgment of the motion state.The test results demonstrate that the performance of attitude estimation was improved by the state modification and spherical simplex transformation UKF. The errors of the estimated attitudes were within 0.5° and the least RMSE were all less than 0.47. Besides, the execution time of the suggested algorithm was comparable with EKF. Compared with UKF, the spherical simplex transformation had greatly decreased the executing time. The proposed attitude estimation system is validated through field test that meets the requirement of the antenna pointing. The algorithm has a good application prospect which will greatly accelerate the development of the SOTM.



---

*Source: 925914-2014-09-30.xml*