Professional Documents
Culture Documents
(
=
(
(3c)
[ ]
4 3 3 13
13
( )
T
q I q
q
q
+ (
=
(
(3d)
B. Inertial Reference Measurement Model
Approximately gyroscope measurement model is:
( ) ( ) ( ) ( ) ( )
( ) 0
( ) ( ) ( )
g g
d
t t d t b t n t
b t
d t D d t n t
= + + +
= +
(4)
This includes:
( )
g
t
is the output of gyroscope
measurement. ( ) d t is the first-order drift of gyroscope,
which is often described as an application of first-order
Markov process. ( ) b t is the constant drift of gyroscope.
( )
g
n t
is the measurement white noise;
d
n
is the white
noise;
D
(6)
Then, define ( ) F x , ( ) G x as follows:
0.5 ( ) 0.5 ( ) 0.5 ( )
( ) 0 0
0 0 0
g
q q
F x D
(
(
=
(
(
(37a)
0.5 ( ) 0
( ) 0
0 0
q
G x I
(
(
=
(
(
(7b)
This includes:
g
n
,
d
n
is unrelated, white Gaussian
noise with zero normal mean and variance is
2
g
,
2
d
.
Differential equation is lined by Jacobin matrix in
EKF as follows:
1 1
2 2 3 3
| 1 1
1 1
...
2! 3!
k k
k k k
I F T F T F T
= + + + + +
2 2
1 1 1 1
1 1
(1 ...)
2! 3!
k k k k
G T F T F T G
= + + +
(8)
1102
| 1 1 1 1 k k k k k k
X X G W
= +
This includes,
| 1 k k
is the state transition matrix,
1 k
G
is noise matrix,
1 k
W
is zero mean white noise sequence,
T is filtering step.
D. Star Sensor Measurement Model
Missile quaternion attitude is gained through the MLS
single-point attitude determination algorithm, which
calculated the position of the vector of optical centre
according to missile body attitude changes, through the star
pattern recognition. Measurement equation is as follows:
( )
k v
y k Hx n = + (9)
This includes:
[ ]
4 4 4 6
0 H I
=
,
v
n
is approximately
Gaussian white noise with zero normal mean, variance is
2
v
.
IV. EXPERIMENTAL RESULTS AND ANALYSIS
A. Simulation Conditions
Simulation started to be 130 second, which supposed
that pitch was 35 degrees, yaw was 0.05 degrees and roll
was 0 degree, after primary engine shut down according to
measurement amendment program, taking remote-range
ballistic missile as an example. At this point the missile
mainly suffered the Earths gravity which does not have an
impact on the motion of around of mass because it passed
through the centre of missile mass. In addition, inertial force
involved which arising from the earths rotation and coriolis
inertial force were neglected to account because of small
implications. Therefore, the motion of around of missile
mass after primary engine shut down can be considered to
be generated only by the impulse and thrust under the action
of additional disturbing torque. According to the value of the
termination impulse and disturbance torque, angular velocity
was supposed to be less than
0
0.06 / sec in the simulation.
Here, gyro parameters used in the simulation are as
follows.
TABLE II. GYRO PARAMETERS
standard deviation of
white noise
0.05deg/
g
n
h =
the initial values of
constant drift error
[ ] (0) 0.3 0.3 0.3 deg/
T
b h =
time constant involved 1
x y z
h = = =
standard deviation of
drive white noise
0.03deg/
d
n
h =
the initial values of
first-order drift
(0) [0.09648 0.09648 0.09648]deg/ d h =
B. Model and Simulation Result Analysis
1) Simulation model
The angular velocity was a small value according to
project practice. It is supposed pitch to be
0
0.06 / h , yaw to
be
0
0.02 / h , roll to be
0
0.01 / h and additional noise to be a
0.01 multiple of three-axis angular velocity simultaneously
in the simulation. The whole system simulation is employed
under the software MATLAB7.1. Simulation curves of
three-axis angular velocity and angle are shown in Fig 2, 3.
0 100 200 300 400 500 600 700 800 900
0
0.1
0.2
P
i
t
c
h
R
a
t
e
0 100 200 300 400 500 600 700 800 900
0
0.02
0.04
Y
a
w
R
a
t
e
0 100 200 300 400 500 600 700 800 900
0.005
0.01
0.015
t/s
R
o
l
l
R
a
t
e
Figure 2. Simulation curve of three-axis angle rate
0 100 200 300 400 500 600 700 800 900
0
100
200
P
i
t
c
h
0 100 200 300 400 500 600 700 800 900
0
20
40
Y
a
w
0 100 200 300 400 500 600 700 800 900
0
5
10
t/s
R
o
l
l
Figure 3. Simulation curve of three-axis angle
2) Result analysis
To suppose that synchronous control of gyro and star
sensor output was finished. Filter sampling interval was set
to be 1 second, simulation time was set to be 15 minutes,
and gyro drifts were modified once for one second. To avoid
the calculation error of converting quaternion to Euler
angles, results analysis was carried out by quaternion.
Quaternion error simulation curve was shown in Figure 4.
Constant drift error curve was shown in Figure 5. One first-
order drift error curve was shown in Figure 6.
Figure 4. Quaternion estimation error
0 100 200 300 400 500 600 700 800 900
-0.05
0
0.05
q0
0 100 200 300 400 500 600 700 800 900
-0.05
0
0.05
q1
0 100 200 300 400 500 600 700 800 900
-0.05
0
0.05
q2
0 100 200 300 400 500 600 700 800 900
-0.05
0
0.05
t/s
q3
1103
Figure 5. constant drift error
Figure 6. first-order drift error
Analysis from the mean deviation and standard square
deviation of quaternion error and constant, first-order drift
error, the result was shown in Table III, IV.
TABLE III. ANALYSIS OF QUATERNION ERROR
deviation q0 q1 q2 q3
mean 0.007491 -0.0002833 0.0008581 0.005776
standard 0.005479 0.008464 0.009042 0.007147
Filter for SINS/CNS attitude estimation has a fast
convergence speed and good stability. Quaternion Error
deviation is about 1 10
24
. Gyro constant and first-order
drift error deviation is about 1 10
24
respectively.
Comparing the result data with the data of single-point
static attitude estimation and the data of inertial iterative
recursive algorithm (The latter two kinds of algorithms will
not be recounted in paper)
[9-12]
, we can draw conclusion that
missile attitude estimated by EKF is more accuracy than that
of the last two.
V. CONCLUSIONS
We applied EKF to "SINS / CNS" attitude estimation
problem in boost phase, which has a smaller attitude angle
rate. Simulation results show that EKF filter algorithm can
meet the engineering applications with fast convergence and
high-precision. However, the measurement data used in the
simulation program ignored the star sensor installation error,
which hinged together with missile initial condition error
and inertial error, etc. So far, research still remains to isolate
the error separately. In addition, SINS/CNS integrated
guidance system and large angle attitude maneuvers angle
attitude maneuvers is the main trends of development of
ballistic missiles, the missile Attitude Determination and
error separation technology under such conditions are a
worthwhile in-depth study of new problem.
REFERENCES
[1] Shen Gong-xun, Sun Yuan-feng. Information fusion theory in the
INS/CNS/GPS Integrated Navigation System [M]. National Defence
Industrial Press,1998
[2] Liebe, C.C. Accuracy Performance of Star TrackersA Tutorial[J].
IEEE Transactions on Aerospace and Electronic Systems.2002, 38(2):
587-599
[3] Markley, F.L.Berman. Deterministic EKF-like estimator for
pacecraft attitude estimation. American Control Conference, 1994
[4] M.D.Shuter,S.D.Oh.Three-Axis Attitude Determination from Vector
Observations. AIAA-81-41003.1981:70-77
[5] E.J.Lefferts,F.L.Markley. Kalman Filtering fro Spacecraft Attitude
Estimation Journal of Guidance, Control and Dynamics.
1982,5(5):417-429
[6] Zhu Qing-hua. EKF Satellite Attitude Determination Algorithm
based on Gyroscope, and Quaternion [J]. Shanghai Aerospace,2005
[7] J.L.Crassidis, F.L.Markley. Predictive Filtering for Attitude
Estimation Without Rate Sensors [J]. Journal of Guidance, Control,
and Dynamics. 1997,20(3):522-527
[8] Markley, F.L.Berman. Deterministic EKF-like estimator for
pacecraft attitude estimation. American Control Conference, 1994
[9] Bar-Itzhack. RQUEST: A Recursive QUEST Algorithm for
Sequential Attitude Determination [J]. Journal of Guidance, Control,
and Dynamics. 1996, 19(5):1034-1038
[10] Keyong Yu, Strap-dwon Inertial Guidance System[M], The
Secondary Artillery College Press,1992
[11] D.Mortari. Euler-q Algorithm for Attitude Determination from
Vector Observations [J]. Journal of Guidance, Control, and
Dynamics. 1998, 21(2):328-334
[12] Bar-itzhack , R.R.Harman. Optimized TRIAD Algorithm for Attitude
Determination [J]. Journal of Guidance, Control, and Dynamics.
1997,20(1):208-211
TABLE IV. ANALYSIS OF CONSTANT AND FIRST-ORDER DRIFT ERROR
deviation b1 b2 b3 d1 d2 d3
mean 0.0001456 4.278e-005 -0.0005188 -8.085e-005 -0.000339 -0.0002221
standard 0.008492 0.009534 0.01492 0.00763 0.008637 0.01369
d3
t/s
0 100 200 300 400 500 600 700 800 900
-0.5
0
0.5 d1
0 100 200 300 400 500 600 700 800 900
-0.1
0
0.1 d2
0 100 200 300 400 500 600 700 800 900
-0.2
0
0.2
b3
t/s
0 100 200 300 400 500 600 700 800 900
-0.5
0
0.5 b1
0 100 200 300 400 500 600 700 800 900
-0.2
0
0.2 b2
0 100 200 300 400 500 600 700 800 900
-0.5
0
0.5
1104