Crassidis J.L.Predictive filtering for nonlinear systems
.pdfPREDICTIVE FILTERING FOR NONLINEAR SYSTEMS
John L. Crassidis
Assistant Professor, Member AIAA
Department of Mechanical Engineering
Catholic University of America
Washington, D.C. 20064
Abstract.
In this paper, a real-time predictive filter is derived for nonlinear systems. This provides a method of determining optimal state estimates in the presence of significant error in the assumed (nominal) model. The new real-time nonlinear filter determines (“predicts”) the optimal model error trajectory so that the measurement-minus-estimate covariance statistically matches the known measurement-minus-truth covariance. The optimal model error is found by using a onetime step ahead control approach. Also, since the continuous model is used to determine state estimates, the filter avoids discrete state jumps, as opposed to the extended Kalman filter. The predictive filter is used to estimate the attitude of a spacecraft with the utilization of attitude sensor measurements and rate integrating gyro measurements. Results using this new algorithm indicate that the real-time predictive filter accurately estimates the attitude of the spacecraft by determining actual gyro bias errors.
Introduction
Conventional filter methods, such as the Kalman filter [1], have proven to be extremely useful in a wide range of applications, including: noise reduction of signals, trajectory tracking of moving objects, and in the control of linear or nonlinear systems. The essential feature of the Kalman filter is the utilization of state-space formulations for the system model. Errors in the dynamic system model are treated as “process noise,” since system models are not usually improved or updated during the estimation process. The process noise is essentially used to “shift” the emphasis from the model to the measurements.
The Kalman filter satisfies an optimality criterion which minimizes the trace of the covariance of the estimate error between the system model responses and actual measurements. Statistical properties of the process noise and measurement error are used to determine an “optimal” filter design. Therefore, model characteristics are combined with sequential measurements in order to obtain state estimates which meliorate both the measurements and model responses.
.Copyright c 1996 by John L. Crassidis. Published by the American Institute of Aeronautics and Astronautics, Inc. with permission.
F. Landis Markley
Staff Engineer, Associate Fellow AIAA
Guidance and Control Branch, Code 712
Goddard Space Flight Center
Greenbelt, MD 20771
In the Kalman filter, the errors in the system model are assumed to be represented by a zero-mean Gaussian noise process with known covariance. However, in actual practice the noise covariance is usually determined by an ad hoc and/or heuristic estimation approach which may result in suboptimal filter designs. Other applications also determine a steady-state gain directly, which may even produce unstable filter designs [2]. Also, in many cases such as nonlinearities in the actual system responses or non-stationary processes, the assumption of a Gaussian model error process can lead to severely degraded state estimates.
In addition to nonlinear model errors, the actual assumed model may be nonlinear (e.g., three-dimensional kinematic and dynamic equations [3]). The filtering problem for nonlinear systems is considerably more difficult and admits a wider variety of solutions than does the linear problem [4]. The extended Kalman filter is a widely used algorithm for nonlinear estimation and filtering [5]. The essential feature of this algorithm is the utilization of a first-order Taylor series expansion of the model and output system equations. The extended Kalman filter retains the linear calculation of the covariance and gain matrices, and it updates the state estimate using a linear function of the measurement residual; however, it uses the original nonlinear equations for state propagation and in the output system equation [5]. But, the model error statistics are still assumed to be represented by a zero-mean Gaussian noise process.
A new approach for performing optimal state estimation in the presence of significant model error has been developed by Mook and Junkins [6]. This algorithm, called the Minimum Model Error (MME) estimator, unlike most filter and smoother algorithms, does not assume that the model error is represented by a Gaussian process. Instead, the model error is determined during the MME estimation process. The algorithm determines the corrections added to the assumed model such that the model and corrections yield an accurate representation of the system behavior. This is accomplished by solving system optimality conditions and an output error covariance constraint. Therefore, accurate state estimates can be determined without the use of precise system representations in the assumed model. Also, the MME estimator can be applied to systems with nonlinear models. The MME estimates are determined from a solution of a two- point-boundary-value-problem ([6-7]). Therefore, the MME
1
American Institute of Aeronautics and Astronautics
estimator is a batch (off-line) estimator which must utilize post-experiment measurements.
The filter algorithm developed in this paper can be implemented in real-time (as can the Kalman filter). However, the algorithm is not limited to Gaussian noise characteristics for the model error. Essentially, this new algorithm combines the good qualities of both the Kalman filter (i.e., a real-time estimator) and the MME estimator (i.e., determines actual model error trajectories). The new algorithm is based on a predictive tracking scheme first introduced by Lu [8]. Although the problem shown in [8] is solved from a control standpoint, the algorithm developed in this paper is reformulated as a filter and estimator with a stochastic measurement process. Therefore, the new algorithm is known as a predictive filter. The advantages of the new algorithm include: (i) the model error is assumed unknown and is estimated as part of the solution, (ii) the model error may take any form (even nonlinear), and (iii) the algorithm can be implemented on-line to both filter noisy measurements and estimate state trajectories.
The organization of this paper proceeds as follows. First, the basic equations and concepts used for the filter development are reviewed. Then, a predictive filter is derived for nonlinear systems. This approach determines optimal state estimates in real-time by minimizing a quadratic cost function consisting of a measurement residual term and a model error term. Then, the concept of the covariance constraint is introduced for determining the optimal model error weighting matrix. Finally, an example involving the estimation of a spacecraft from attitude sensors and gyros is presented.
Nonlinear Predictive Filter
Preliminaries
In this section, the nonlinear predictive filter algorithm is derived. This development is based upon the duality which exists between the predictive controller for nonlinear systems by Lu [8] and a general estimation problem. In the nonlinear predictive filter it is assumed that the state and output estimates are given by a preliminary model and a to-be- determined model error vector, given by
|
|
|
|
|
|
|
|
ˆ |
(t ))+G (xˆ (t ))d (t ) |
|
|
|
|
(1a) |
||||
|
|
|
|
|
|
|
|
x (t )= f (xˆ |
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
yˆ (t )= c (xˆ (t )) |
|
|
|
|
|
(1b) |
||
where |
f |
|
|
n |
|
|
|
|
|
ˆ |
|
|
|
n |
|
|||
|
R |
is sufficiently differentiable, |
|
R |
is the |
|||||||||||||
|
|
x(t ) |
|
|
||||||||||||||
state |
estimate |
vector, d (t ) Rq represents |
the |
model |
error |
|||||||||||||
vector, |
G (xˆ (t )):Rn → Rn×q is |
the model-error |
distribution |
|||||||||||||||
matrix, |
|
c (xˆ (t )) Rm is |
the |
measurement |
vector, |
and |
||||||||||||
|
ˆ |
|
R |
m |
is the estimated output vector. |
State-observable |
||||||||||||
|
y (t ) |
|
|
|||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
discrete measurements are assumed for Equation (1b) in the following form
|
|
|
|
|
y |
k = c (x (tk ))+vk |
(2) |
||
|
|
|
|
|
|
|
|||
where |
y |
k |
Rm |
is the measurement vector at time tk , x (tk ) |
|||||
is the |
|
true state vector, and v |
k |
Rm |
represents the |
||||
|
|
|
|
|
|
|
|
|
measurement noise vector which is assumed to be a zeromean, Gaussian white-noise distributed process with
E{vk }= 0 |
(3a) |
E{vk vTl }= Rδkl |
(3b) |
where R Rm×m is a positive-definite |
measurement |
covariance matrix. |
|
A Taylor series expansion of the output estimate in Equation (1b) is given by
|
yˆ (t + ∆t )≈ yˆ (t )+ z (xˆ (t ), ∆t )+ Λ(∆t )S (xˆ (t ))d (t ) |
(4) |
||||||||
|
|
|
|
|
|
|
||||
where the ith row of z (xˆ (t ), ∆t ) |
is given by |
|
||||||||
|
|
|
|
ˆ |
∆ |
t ) |
= pi |
∆tk k |
(5) |
|
|
|
zi (x (t ), |
|
∑ |
k ! |
Lf (ci ) |
||||
|
|
|
|
|
|
|
k =1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
where pi , i =1, 2,…, m , is the lowest order of the derivative
ˆ |
in which any component of d (t ) first appears |
|||||||||||||
of ci (x(t )) |
||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
(t ) |
on |
due to successive differentiation and substitution for xi |
||||||||||||||
the right side. |
Lk (c )is a |
k th |
order Lie derivative, defined |
|||||||||||
|
|
f |
|
i |
|
|
|
|
|
|
|
|
|
|
by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Lk |
(c |
)= c |
|
|
|
|
|
|
for k = 0 |
|
|
||
|
f |
i |
|
|
i |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
k −1 |
(ci ) |
|
|
|
|
|
(6) |
||
|
Lk |
(c |
) |
= |
∂ Lf |
|
|
f |
for k ≥1 |
|
|
|||
|
|
|
|
|
|
|||||||||
|
f |
i |
|
|
∂ xˆ |
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
|
|
|||
Λ(∆t ) Rm×m is a diagonal matrix with elements given by |
|
|||||||||||||
|
|
λ |
|
= ∆t pi |
, |
|
i =1,2,…, m |
|
(7) |
|||||
|
|
ii |
|
pi ! |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
S (xˆ (t )) Rm×q is a matrix with each ith row given by |
|
|
||||||||||||
si |
={Lg1 Lpfi −1 (ci ) ,…, Lgq |
Lpfi −1 (ci ) }, |
|
(8) |
||||||||||
|
|
|
|
|
i =1, 2,…, m |
|
|
|
where the Lie derivative with respect to Lg j in Equation (8) is defined by
2
American Institute of Aeronautics and Astronautics
|
|
|
|
|
|
∂ Lpi −1 |
(c |
) |
|
|
|
|
|
L |
Lpi −1 |
(c |
) |
≡ |
f |
i |
|
g |
j |
, |
j =1, 2,…, q |
(9) |
|
|
|
|
|||||||||||
|
g j |
f |
i |
|
|
∂ xˆ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Equation (8) is in essence a generalized sensitivity matrix for nonlinear systems.
Nonlinear Filtering
A cost functional consisting of the weighted sum square of the measurement-minus-estimate residuals plus the weighted sum square of the model correction term is minimized, given by
( |
) |
|
2 { |
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
J d (t ) |
|
= |
1 |
|
y |
(t |
+ ∆ |
t ) |
− ˆ |
+ ∆ |
t ) |
T |
R |
−1 |
|
y (t |
+ ∆ |
t ) |
− ˆ |
+ ∆ |
t ) |
||||||||||||
|
|
|
|
|
|||||||||||||||||||||||||||||
|
|
|
|
|
|
|
y (t |
|
|
|
|
|
|
y (t |
|
|
|||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||
|
|
|
|
|
|
|
|
|
|
+ |
1 |
dT (t )W d |
(t ) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||
|
|
|
|
|
|
|
|
|
|
2 |
|
(10) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
where W Rq×q |
is positive semidefinite. |
|
Also, a constant |
|
|||||||||||||||||||||||||||||
sampling |
|
rate |
|
is |
|
assumed |
|
so |
|
that |
y |
(t + ∆t )≡ |
y |
k +1 . |
|
||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Substituting Equation (4), and minimizing Equation (10) with respect to d (t ) leads to the following model error
Covariance Constraint
The weighting matrix (W ) in Equation (11) can be
determined on the basis that the measurement-minus-estimate error covariance matrix must match the measurement-minus- truth error covariance matrix (see [6]). This condition is referred to as the “covariance constraint,” shown as
1 |
N |
|
|
||
∑{ek −e}{ek −e}T |
≈ R |
(14) |
|||
|
N |
||||
|
k =0 |
|
|
||
|
|
|
|
||
where e is the sample mean of y − yˆ , |
and N |
is a large |
number. A test for whiteness can be based upon the autocorrelation function matrix of the measurement residual [5]. The maximum likelihood estimate of the m×m autocorrelation function matrix for N samples is given by
Ck = |
1 |
N |
e eT |
(15) |
N |
|
|||
|
∑ i i−k |
|
||
|
|
i=k |
|
|
A 95% confidence interval for whiteness using a finite sample length is given by [5]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−1 |
|
|
ρiik |
|
≤1.96 / N1/ 2 |
(16) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||
d (t ) |
|
{ |
( |
|
t )S (x) |
T |
R |
−1 |
( |
|
t )S (x) |
|
W} |
|
|
|
|
|
|
|
= − |
∆ |
|
Λ |
∆ |
+ |
|
|
|
|
|
|
|||||||||
|
Λ |
|
ˆ |
|
|
|
|
ˆ |
|
(11) |
where ρii corresponds to the diagonal elements resulting by |
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
× Λ(∆t )S (xˆ) T R−1 z (xˆ, ∆t )− y (t + ∆t )+ yˆ (t )
By using the matrix inversion lemma [9], the model error in Equation (11) can be re-written as
d (t ) |
= − |
|
|
|
ˆ |
|
∆ |
t ) |
− |
|
y (t |
+ ∆ |
t ) |
+ |
ˆ |
|
|
(12) |
|||||
|
|
M (t ) z |
(x, |
|
|
|
|
|
|
y |
(t ) |
||||||||||||
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
M (t ) |
= |
W |
−1 |
|
− Λ |
|
∆ |
|
|
ˆ |
T |
|
|
|
||||||||
|
|
|
I |
|
|
|
( t )S |
(x) |
|
|
|
|
|||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−1 Λ |
|
|
|
|
|
|
T |
|
|
−1 |
|
|||||
× Λ ∆ |
|
|
ˆ |
|
( |
∆ |
|
|
|
ˆ |
+ |
R} |
|
(13) |
|||||||||
{ |
( |
t )S (x)W |
|
|
|
|
t )S (x) |
|
|
||||||||||||||
× Λ |
( |
∆ |
|
|
ˆ |
W |
−1 |
|
Λ |
|
∆ |
|
|
ˆ |
T |
R |
−1 |
|
|||||
|
t )S (x) |
|
|
) |
|
( t )S (x) |
|
|
|
The measurement at time tk +1 is used in Equation (12) to find the appropriate d for the nonlinear propagation of the state estimates from time tk to time tk +1 . By looking ahead to the next measurement to compute d , the predictive filter gives a
continuous state estimate without the jumps exhibited by the Kalman filter. The matrix W serves to weight the amount of model error added to correct the assumed model in Equation
(1). As W decreases, more model error is added to correct the model, so that the estimates more closely follow the measurements. As W increases, less model error is added, so that the estimates more closely follow the propagated model.
normalizing the autocorrelation matrix by the zero-lag elements, given by
ρiik = |
cii |
(17) |
k |
||
cii |
||
|
0 |
|
If the confidence interval in Equation (16) and the covariance constraint in Equation (14) are met, then the weighting matrix is optimal. Therefore, the proper balance between model error and measurement residual has been achieved. If the measurement residual covariance is higher than the known measurement error covariance (R), then W should be
decreased to less penalize the model error. Conversely, if the residual covariance is lower than the known covariance, then W should be increased so that less unmodeled dynamics are added to the assumed system model.
The sample measurement covariance can be determined from a recursive relationship given by [10]
ˆ |
|
|
ˆ |
|
|
1 |
|
|
k |
|
|
|
|
|
|
|
|
|
|
|
|
T |
ˆ |
|
|
|
|
R |
+ |
= R |
+ |
|
|
|
|
|
(e |
k |
+1 |
−e |
k |
)(e |
k+1 |
−e |
k |
) |
− R |
|
(18a) |
||||||
|
|
|
|
|
|||||||||||||||||||||||
|
k |
1 |
|
k |
|
k +1 |
|
|
|
|
|
|
|
|
|
k |
|
||||||||||
|
|
|
|
|
|
k +1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
|
|
e k +1 = e k + |
|
1 |
(ek +1 −e k ) |
|
|
|
|
|
(18b) |
||||||||||||
|
|
|
|
|
|
k |
+1 |
|
|
|
|
|
|||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
→ R . |
|
|
|
|
|||
The covariance constraint is met when Rk |
|
|
|
|
Even though the model error is determined by Equation (11) or (12), it still involves stochastic processes. Therefore, a
3
American Institute of Aeronautics and Astronautics
covariance of the model error can be derived. First, the |
Stability |
covariance constraint is re-written as |
Filter Stability |
|
( |
|
k |
− ˆ |
k )( |
|
k |
− ˆ |
k ) |
|
= |
|
||
|
|
|
|
|
|
T |
|
||||||
E |
y |
|
|
y |
|
y |
|
|
y |
|
|
|
R |
Substituting Equation (2) into Equation (19), and using
E{yk vTk }= E{vk yTk }= E{yˆk vTk }= E{vk yˆTk }= 0
leads to
E{yk yTk }− yˆk yˆTk = R
If Equation (14) is met, the process is stationary so that
E{yk +1 yTk +1}− yˆk +1 yˆTk +1 = R
(19) |
The effect of W on filter stability and bandwidth can be |
||||||||||||||||||||||||||||||
|
determined by applying a discrete error analysis. The filter |
||||||||||||||||||||||||||||||
|
residual is given by |
|
|
|
|
= |
|
|
|
+ ∆ |
|
|
− ˆ |
|
+ ∆ |
|
|
|
|
|
|
||||||||||
|
|
|
e(t |
+∆ |
t ) |
y (t |
t ) |
(t |
t ) |
|
|
(27) |
|||||||||||||||||||
(20) |
|
|
|
|
|
|
|
|
|
|
|
y |
|
|
|
||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
Substituting Equations (4) and (12) into Equation (27) leads to |
|||||||||||||||||||||||||||||||
|
|||||||||||||||||||||||||||||||
|
e(t |
+∆ |
t ) |
= |
|
|
−Λ |
( |
∆ |
|
|
|
|
ˆ |
|
|
|
|
ˆ |
(t |
+ ∆ |
t ) |
(28) |
||||||||
|
|
|
|
I |
|
|
|
|
t )S (x)M |
(t ) e |
|
||||||||||||||||||||
(21) |
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
(t |
+∆ |
t ) |
≡ |
y (t |
+ ∆ |
t ) |
− |
ˆ |
− |
ˆ |
|
∆ |
t ) |
|
(29) |
|||||||||||||
|
|
e |
|
|
|
|
|
|
|
|
|
|
y (t ) |
|
z (x, |
|
|
||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(22)which is the predicted measurement residual at t + ∆t assuming d = 0 .
For constant a sampling interval, Equation (22) is equivalent |
|
If |
S is square and full rank, then |
ΛS M is also full rank. |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
to |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
AsW → 0 , |
|
then ΛS M → I , |
|
and (I −ΛS M )→ 0 . |
This |
|||||||||||||||||||||
|
|
E |
{ |
y |
(t |
+ ∆ |
t )y |
T |
(t |
+ ∆ |
t ) |
= |
|
ˆ |
|
+ ∆ |
|
|
|
ˆT |
(t |
+ ∆ |
t ) |
+ |
R |
|
(23) |
|
|
|||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
y (t |
|
|
t )y |
|
|
|
|
|
|
|
|
approaches a deadbeat response for |
the |
filter |
dynamics. |
|||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||
As long as the process remains stationary, Equation (23) is |
AsW → ∞, then M → 0 , and (I −ΛS M )→ I . This yields a |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
filter response with discrete eigenvalues approaching the unit |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
valid even if the covariance constraint is not satisfied. Also, |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
circle. As |
|
long as the covariance matrix is positive, the |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
since the optimal model error solution in Equation (11) is a |
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
discrete eigenvalues of the filter will lie within the unit circle. |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
function of the stochastic measurement noise process, |
a test |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
Therefore, the filter remains contractive [11]. |
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
for the whiteness of the “determined” model error can be |
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
Robustness |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
found by using the correlation function in Equations |
(15)- |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||
(17), replacing |
|
e |
with d . |
|
If the model error is sufficiently |
|
Lu [12] has shown that the dual control problem achieves |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
white, then the |
|
covariance of |
|
the model |
|
error |
|
can |
|
also be |
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
input/output linearization, and asymptotic tracking of any |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
determined using a recursive formula shown in Equation (18), |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
given |
trajectory if pi ≤ 4 . |
An |
|
analysis of |
the |
robustness |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
again replacing |
|
e |
with d . |
|
Another form for the model error |
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
covariance can be determined by using Equation (11), and |
properties |
|
in |
the |
face |
of |
unmodeled |
dynamics |
for pi |
=1 , |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
assuming that |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
W |
= |
0 |
, |
and |
square |
and |
nonsingular |
ˆ |
has |
also |
been |
|||||||||||||||||||||||
|
|
|
|
|
|
|
|
} |
|
|
{ |
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
S (x) |
||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
{ |
ˆ |
|
|
|
|
|
|
+ ∆ |
= |
|
|
|
+ ∆ |
|
|
|
|
ˆT |
|
= |
|
|
|
|
|
|
shown |
ˆ |
|
|
|
m×3 |
|
In |
this |
|
|
≤ |
|
the |
case of pi =1 , W |
≠ 0 , |
||||||||||||||||||||||||
|
|
|
E |
|
|
y (t )v |
|
|
(t |
|
|
t ) |
|
|
E |
|
|
v (t |
|
|
t )y |
|
(t ) |
|
|
|
0 |
|
|
(24a) |
and S (x) |
|
|
R |
|
, |
where |
|
m |
|
|
3 |
is |
considered. |
The |
|||||||||||||||||||||||||||
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
|
} |
|
|
{ |
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
T |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
T |
|
|
|
|
|
|
|
|
|
continuous output estimate for |
pi =1 is given by [12] |
|
|||||||||||||||||||||||||||||||||
|
|
|
|
E |
|
|
z (t )v |
(t + ∆t ) |
|
= |
E |
|
|
v (t + ∆t )z |
(t ) |
|
= 0 |
|
|
(24b) |
|
|||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ = |
|
|
(c) |
+ |
ˆ |
|
|
|
(30) |
|||||
which leads to |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
y |
L f |
|
|
S (x)d |
|
|
|||||||||||||||||
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||
|
{ |
|
|
|
|
|
|
|
} |
= |
|
|
|
|
|
|
ˆ |
|
|
− |
|
ˆ |
|
+ ∆ |
|
|
+ |
|
|
|
|
|
+ |
|
|
|
|
|
|
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||
E d (t )d |
T |
(t ) |
M (t ) |
|
|
(t ) |
|
(t |
t ) |
z (t ) |
R |
M |
T |
(t )(25) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
y |
|
|
y |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Lf (c1 ) |
|
|
|
|
|||||||||||||||||||||||||||||||||
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
L f |
|
|
|
|
|
|
|
|
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
≡ a aT |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(c)≡ |
|
|
|
|
|
|
|
|
(31) |
||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
a |
|
|
|
for any a |
|
|
|
|
|
|
|
|
|
|
|
|
|
(26) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Lf (cm ) |
|
|
|
||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Therefore, the relative magnitude of the model error can now be determined. In fact, if the determined model error process is truly white, then the inverse of the weighting matrix (W )
can be shown to be the maximum likelihood estimate of the model error covariance. This can be used to determine an adaptive scheme for determining W to satisfy the covariance constraint (which will be reported at a later time).
Suppose that the unmodeled errors are introduced into the output estimate by
|
|
ˆ |
= |
|
|
|
|
|
+ ∆ |
|
+ |
ˆ |
|
+ ∆ |
ˆ |
(32) |
||||||
|
|
y |
|
L f (c) |
|
|
|
|
|
L f (c) |
|
|
S (x) |
|
S (x) d |
|||||||
and suppose that L f (c) |
and ∆L f |
(c) |
are bounded by |
|
||||||||||||||||||
|
L f |
(c) |
|
|
|
≤ n1, |
|
|
|
|
∆L f (c) |
|
≤ n2, |
|
for all x X |
(33) |
||||||
|
|
|
|
|
|
|
4
American Institute of Aeronautics and Astronautics
Furthermore, assume that ∆S (xˆ) is represented by
|
|
|
|
|
|
|
|
∆ |
ˆ |
= δ |
ˆ |
ˆ |
|
(34) |
|
|
|
|
|
|
|
|
|
S (x) |
|
(x)S |
(x) |
|
|
where |
δ |
|
ˆ |
is a scalar, continuous function with bound given |
||||||||||
|
(x) |
|||||||||||||
|
− |
|
< δ |
ˆ |
< |
n3 . Assuming |
|
|
|
|
||||
by |
|
1 |
|
|
|
(x) |
|
that |
the |
model |
errors and |
|||
measurement |
|
errors |
are |
isotropic |
leads |
toW = w I , |
and R = r I . Then, the matrix inverse in Equation (11) can be written as (suppressing arguments)
{ |
|
|
|
|
} |
|
|
|
|
|
|
|
|
[ |
|
T |
−1 |
|
−1 |
|
{( |
|
) |
|
} |
−1 |
|
ΛS |
] |
R ΛS +W |
|
= |
v −σ |
I +C |
|
(35) |
|||||
|
|
|
|
|
|
|
|||||||
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
C ≡ ∆t2 |
ST S |
|
|
|
|
(36a) |
||||
|
|
|
|
|
r |
|
|
|
|
|
|
|
|
|
|
|
σ = |
∆t2 tr (ST |
S ) |
|
|
|
|
(36b) |
|||
|
|
|
|
2 r |
|
|
|
|
|
|
|
|
|
|
|
|
|
v = w +σ |
|
|
|
|
|
|
(36c) |
By the Cayley-Hamilton theorem, any meromorphic function of C can be expressed as a quadratic in C [13], yielding
{(v −σ )I +C}−1 = |
1 |
(α I + β C +C2 ) |
|
|
(37) |
|||||||||||||||||||||||
γ |
|
|
||||||||||||||||||||||||||
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
α = v2 −σ2 + k |
|
|
|
|
|
|
|
|
|
(38a) |
||||||||||||
|
|
|
|
|
|
|
|
β = −(v +σ ) |
|
|
|
|
|
|
|
|
|
(38b) |
||||||||||
|
|
γ = (v −σ )α + ∆ = wα + ∆ |
|
|
|
|
(38c) |
|||||||||||||||||||||
|
k = tr (adjC ) |
= ∆t24 |
tr |
adj |
(ST S ) |
|
(38d) |
|||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
r |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
∆ = ∆t36 |
det (ST S ) |
|
|
|
|
|
|
|
|
(38e) |
|||||||||||||
|
|
|
|
|
|
|
|
r |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Therefore, the error dynamics become |
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||
e = − |
∆t |
|
(1+δ )Q e + y − L |
f |
|
−∆L |
f |
|
|
|
||||||||||||||||||
r γ |
|
|
|
(39) |
||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
|
|
|
|
∆t2 |
(1 |
+δ )Q L |
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||
|
|
+ |
f |
− y |
|
|
|
|
|
|
||||||||||||||||||
where |
|
|
|
r γ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
( |
S ST |
) |
+ |
∆t2 |
( |
S ST |
|
) |
2 |
+ |
|
∆t4 |
( |
S ST |
) |
3 |
|
||||||||||
|
|
r |
β |
|
|
|
r2 |
|
(40) |
|||||||||||||||||||
Q ≡ α |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Now, define a Lyapunov functionV = e 2 |
|
2 . |
Using |
the |
||||||||||||||||||||||||
norm inequality [14], and the fact that |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
eT Q e ≥ |
|
|
|
|
1 |
|
|
|
|
eT e |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(41) |
||||||||||||||||||||||||||||||||||||||||||||||
leads to |
|
|
|
|
|
|
|
|
|
|
|
|
|
Q−1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||
|
|
|
|
V ≤ − |
2 ∆t |
(1+δ |
) |
V + |
|
eT |
|
|
|
|
|
y − L f −∆L f |
|
|
|
|
|
(42) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
r γ |
Q−1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
∆t2 |
|
(1+δ ) |
|
|
T |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
+ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
e |
|
|
|
|
|
|
Q |
|
|
|
|
|
L f |
− y |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
r γ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(4 z) |
|||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
a b ≤ z a2 +b2 |
|||||||||||||||||||||||||||||||||||||||||
Next, using the well known inequality |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
for any a , b , |
|
and z > 0 , |
|
and |
|
|
defining |
|
|
ξ ≡ ∆t (1+δ ) |
(r γ ) |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
yields |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2ξ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
y |
|
− L f |
|
−∆L f |
|
|
|
2 |
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
V ≤ − |
|
|
|
|
|
|
|
|
|
+ 4 z |
V + |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||
|
Q−1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
4 z |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(43) |
||||||
|
|
|
|
|
∆t2ξ2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
+ |
|
|
|
|
|
Q |
|
|
|
|
|
|
|
L f − y |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
4 z |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||
Substituting 4 z = ξ |
|
|
Q−1 |
|
leads to |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
V ≤ − |
|
|
|
|
|
|
ξ |
|
|
|
|
|
|
V +b |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(44) |
||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Q−1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||
b ≡ |
|
|
|
Q−1 |
|
|
|
|
1 |
|
|
y − L f |
|
|
−∆L f |
|
|
2 + ∆t2ξ |
|
|
Q |
|
2 |
|
|
|
|
L f − y |
|
2 |
|
(45) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
ξ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||
Therefore, Equation (44) can be solved to yield |
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
b |
|
|
|
|
Q−1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Q |
−1 |
|
|
|
|
|
|
|
|
|
b |
|
|
|
Q−1 |
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
V ≤ |
V |
|
− |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
e−ξ t |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
+ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(46) |
||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
|
|
|
|
|
|
|
ξ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ξ |
|
|
||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
where V ≥ b Q−1 |
|
ξ |
|
|
|
is required to maintain the inequality. |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
0 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||
Defining the bounds |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
θ = inf (1+δ )> 0 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(47a) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
m |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
y |
|
|
∞ = tmax0, t |
∑ |
|
|
y |
i |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(47b) |
||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
f i=1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||
and using the matrix norm inequality again leads to |
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
e |
|
≤ |
2 |
|
r γ |
|
|
Q−1 |
|
|
|
µ |
2 |
+ |
|
|
2 ∆t |
|
|
|
|
|
Q−1 |
|
|
|
|
|
|
Q |
|
µ2 |
|
(48) |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
∆tθ |
|
|
|
|
|
|
|
|
|
|
1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
|||||||||||||||||||||||
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
5
American Institute of Aeronautics and Astronautics
µ |
= |
|
|
|
|
y |
|
|
−n |
−n |
|
(49a) |
|||||||||||
|
|
|
|
||||||||||||||||||||
1 |
|
|
|
|
|
|
|
|
|
1 |
2 |
|
|
||||||||||
|
|
|
|
|
|||||||||||||||||||
|
|
|
|
|
|
|
|
|
∞ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
µ2 |
= θ |
(n1 − |
|
|
|
|
y |
|
|
|
|
∞ ) |
(49b) |
||||||||||
|
|
|
|
||||||||||||||||||||
|
|
||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Assuming that S ST ≤ s1 for all S ST leads to the following bound on Q
|
Q |
|
|
|
≤α s + ∆t2 |
|
β |
|
s2 |
+ ∆t4 |
s3 |
(50) |
|
|
|
|
|
|
|||||||||
|
|
|
|
|
1 |
r |
|
|
|
1 |
r2 |
1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
Also, tr (ST S )≤ 3 s1 and det (ST S )≤ s13 , which leads to
σ ≤ |
|
3∆t2 |
|
s |
(51a) |
|
|
|
|
|
|||
|
|
2 r |
1 |
|
||
|
|
|
|
|
||
k ≤ |
9 ∆t4 |
|
s2 |
(51b) |
||
|
|
|
|
|||
|
|
r2 |
1 |
|
||
|
|
|
|
|
||
∆ ≤ |
|
∆t6 |
s3 |
(51c) |
||
|
|
|
||||
|
|
r3 |
1 |
|
||
|
|
|
|
|
Substituting Equation (51) into Equations (38) and (50) leads to the following bounds on Q and γ
can also be determined using a Cayley-Hamilton expansion, but becomes increasingly more complicated.
Numerical Stability
If the system is unobservable then ST S is not full rank. However, the filter can compensate for this by adding more model correction. It can be shown that the filter remains stable for bounded model uncertainties as long as
|
|
(v −σ )α + ∆ > 0 |
(55) |
||
If ST S is |
not full |
rank, then ∆ = 0 , which |
leads to the |
||
following condition |
|
|
|
|
|
|
|
v > |
∆t2 |
tr (ST S ) |
(56) |
|
|
2 r |
|||
|
|
|
|
|
|
Therefore, |
the filter |
remains |
contractive as long as w > 0 . |
This condition is always met, but Equation (56) can be used to help determine any numerical difficulties (i.e., large values of σ may produce numerical difficulties). One possible solution is to make r as large as possible. However, then w will be adjusted to meet the covariance constraint, so that the numerical difficulties remain. Another solution to this problem is to use smaller sampling interval, but this may not be possible. A more practical solution is to utilize a “U − D ” factorization of Equation (13) [15].
|
|
≤ w2s |
|
4 w∆t |
2s2 |
|
13 |
∆t4s3 |
||||||||
|
Q |
+ |
|
|
1 |
+ |
|
|
|
|
1 |
|
||||
|
|
|
|
|
|
|
|
|
||||||||
|
|
|
|
1 |
|
|
r |
|
|
|
|
|
|
r2 |
||
|
|
|
|
|
|
|
|
|
|
|
|
|
||||
3 |
|
3 w2∆t2s |
|
|
9 ∆t4s2 |
|
∆t6s3 |
|||||||||
|
|
|
1 |
|
|
|
|
1 |
|
|
1 |
|||||
γ ≤ w |
+ |
|
|
|
+ |
|
|
|
|
|
+ |
r3 |
||||
|
r |
|
r2 |
|
|
|
A bound on Q−1 is found by writing it as
|
|
|
|
|
|
|
∆t |
2 |
|
|
|
|
|
|
|
|
|
−1 |
|
−1 |
|
Q |
−1 |
= γ |
|
|
|
|
|
T |
|
|
|
|
S |
T |
|||||||
|
|
S |
r |
S |
|
|
S + wI |
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
≤ γ |
|
|
(S ST ) |
−1 |
|
|
|
|
|
∆t |
2 |
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||
|
|
|
|
|
|
|
|
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
ST S + wI |
|
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
r |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(52a) |
Cases |
|
|
|
|
|
|
|
|
|
|
|
|
Case 1. Let pi |
=1 for the output system. |
Equations |
|||||||||
(52b) |
(5), (7) and (8) reduce to |
|
|
|
|
|
|
|
|
|
|
|
z |
= ∆ |
t H |
ˆ |
|
ˆ |
|
||||||
|
|
|
|
(x) f |
(x) |
(57a) |
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
≡ |
∂ |
yˆ |
|
|
(57b) |
|
|
|
|
|
|
|
|
|
|||||
|
|
|
H (x) |
|
∂ xˆ |
|
|
|||||
|
|
|
|
Λ = ∆t I |
|
|
(57c) |
|||||
|
|
S |
= |
|
|
ˆ |
|
ˆ |
(57d) |
|||
|
|
|
|
H (x)G (x) |
(53)Therefore, the model error trajectory in Equation (12) is given by
|
|
|
|
|
|
(S ST ) |
−1 |
|
|
|
|
∆t |
2 |
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||
|
|
|
|
≤ γ |
|
|
|
|
|
|
|
ST S |
|
|
|||||||||||
|
|
|
|
|
|
|
|
|
|
r |
|
|
+ w |
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Assuming that |
|
|
|
(S ST )−1 |
|
|
|
≤ s2 |
|
|
|
for all S ST |
leads to |
||||||||||||
|
|
|
|
|
|
|
|||||||||||||||||||
|
|
|
|
|
Q−1 |
|
≤ γ s |
|
|
|
2 |
|
|
|
(54) |
||||||||||
|
|
|
|
|
|
|
∆t |
|
s |
+ w |
|||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
r |
|
|
1 |
|
|
||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Therefore Equations (48), (52), and (54) define the bound for the error dynamics under unmodeled uncertainty. Similar results can be obtained for1 < pi ≤ 4 . The case where q > 3
|
|
|
|
|
|
|
|
|
H GW −1GT HT |
+ ∆t−2 R |
−1 |
|
||||||||||||||||||
d = −∆t I −W −1GT HT |
|
H G |
||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(58) |
||||
|
H |
|
|
R |
|
|
{y |
(t ) |
|
y (t |
|
|
|
t ) |
|
t H f } |
||||||||||||||
W G |
T |
T |
|
|
|
|
|
|
|
|
||||||||||||||||||||
× −1 |
|
|
|
−1 |
|
|
|
ˆ |
|
|
|
|
− |
|
|
+ ∆ |
|
|
+ ∆ |
|
|
|
|
|
||||||
Case 2. Consider the following system |
|
|
|
|
|
|||||||||||||||||||||||||
|
|
|
|
|
|
|
ˆ |
|
|
= f |
|
|
(xˆ , xˆ |
|
) |
|
|
|
|
|
|
|
|
(59a) |
||||||
|
|
|
|
|
|
|
x |
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||
|
|
|
|
|
|
|
1 |
|
|
|
|
|
|
1 |
|
1 |
2 |
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||
|
|
ˆ |
|
|
= f |
|
|
|
(xˆ |
|
|
)+G |
(xˆ |
|
|
)d |
|
|
|
|
(59b) |
|||||||||
|
|
x |
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
|
|
|
|
2 |
|
|
|
|
2 |
|
|
2 |
|
2 |
|
|
2 |
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
yˆ = c (xˆ1 ) |
|
|
|
|
|
|
|
|
|
|
(59c) |
|||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
6
American Institute of Aeronautics and Astronautics
with pi = 2 . Equation (59a) usually defines the kinematics,
and Equation (59b) usually defines the dynamics of a system. Equations (5), (7) and (8) now become
|
+ ∆t |
2 |
|
∂ L1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
∂ L1 |
|
|
|
|
|
|
|
|
|||||||
z = ∆t L1 |
|
|
|
|
f |
|
|
f |
|
(xˆ |
|
, xˆ |
|
)+ |
|
|
|
f |
|
f |
|
|
(xˆ |
|
) |
(60a) |
|||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||
f |
|
2 |
|
∂ xˆ |
|
|
|
1 |
|
|
1 |
|
|
2 |
|
|
∂ xˆ |
2 |
|
|
2 |
|
2 |
|
|
||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||
|
|
|
|
|
1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||
|
L1 |
≡ |
∂ c |
|
f |
|
|
(xˆ |
|
, xˆ |
|
|
)+ |
|
∂ c |
|
f |
|
|
(xˆ |
|
) |
|
|
(60b) |
||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||
|
f |
|
|
|
|
|
|
1 |
|
1 |
|
|
2 |
|
|
|
|
|
|
|
|
2 |
|
|
|
2 |
|
|
|
|
|||
|
|
|
∂ xˆ1 |
|
|
|
|
|
|
∂ xˆ2 |
|
|
|
|
|
|
|
|
|
|
|||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
Λ = |
∆t2 |
I |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(60c) |
||||||
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ω |
|
|
|
|
|
−[ω×] |
|
|
|
|
|
(64a) |
|||
Ω(ω)≡ |
|
|
|
|
|
|
|
|
||||
|
|
|
|
−ωT |
|
|
|
|
0 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
q I |
|
+ q |
× |
|
|||||
Ξ( |
|
)≡ |
|
4 |
3×3 |
|
|
|
13 |
|
|
|
q |
|
|
|
|
|
|
|
|
|
(64b) |
||
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
|
− |
q |
T |
|
|
|
|
|
|
|
|
|
|
13 |
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
where I3×3 is a 3×3 identity matrix. The 3×3 dimensional matrices [ω×] and q13 × are referred to as cross product
|
|
|
|
|
|
S = |
∂ L1f |
|
G2 (xˆ2 ) |
(60d) |
matrices since a ×b = [a ×]b , with |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
∂ xˆ2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
−a3 |
|
|
a2 |
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||
Spacecraft Attitude Estimation |
|
|
|
|
|
|
|
[a ×] |
≡ |
|
a |
|
|
|
|
0 |
|
|
|
|
−a |
|
|
|
|
|
|
|
(65) |
|||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
3 |
|
|
|
|
|
a |
|
|
|
|
|
|
1 |
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−a |
|
|
|
|
|
|
0 |
|
|
|
|
|
|
|
|
||||||||||||||||||
In this section, the predictive filter is used to determine |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
|
|
1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||
the attitude of a spacecraft using gyros and attitude sensor |
Since a three degree-of-freedom attitude system is represented |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
measurements. First, a brief review of attitude kinematics and |
by a four-dimensional vector, the quaternions cannot be |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
gyro dynamics is shown. Next, a brief review of the Kalman |
independent. |
|
This |
condition |
|
|
|
leads |
|
to |
the |
following |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
filter for attitude estimation is shown. Then, a predictive filter |
normalization constraint |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||
for attitude estimation is developed. Finally, simulation |
|
|
|
|
|
|
|
|
qT q = qT |
q |
|
|
|
|
+ q42 |
|
=1 |
|
|
|
|
|
|
|
(66) |
|||||||||||||||||||||||||||||||||||||||||||||||||
results using the predictive filter to estimate the attitude of the |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||
Tropical Rainfall Measurement Mission (TRMM) spacecraft |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
13 |
|
|
13 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||
The measurement model is assumed to be of the form |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
[16] are shown. |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
Attitude Kinematics and Gyro Dynamics |
|
given by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
BB = A( |
q |
)BI |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(67) |
||||||||||||||||||||||||||||||||||||||||||
The attitude is assumed to be represented by the |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||
quaternion, defined as |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
where BI |
|
is a |
3×1 dimensional vector of some reference |
||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
q |
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
object (e.g., a vector to the sun or to a star, or the Earth’s |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
q = |
|
|
|
13 |
|
|
|
|
(61) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
magnetic field vector) in a reference coordinate system, |
BB is |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
q |
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
4 |
|
|
|
|
|
a 3×1 dimensional vector |
|
|
defining |
the |
components |
of the |
|||||||||||||||||||||||||||||||||||||||||||||||||
with |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
corresponding reference vector measured in the spacecraft |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
q1 |
|
|
|
|
|
|
|
|
|
body frame, and |
A(q) |
is given by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||
|
|
q |
|
|
|
≡ |
|
= |
|
ˆ |
|
θ |
(62a) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||
|
|
13 |
|
|
|
q2 |
|
nsin ( |
/ 2) |
A |
|
q |
|
= |
q2 |
−qT q |
|
|
I |
|
|
|
|
+ 2 q |
|
qT |
|
−2 q |
q |
|
× |
(68) |
||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
q3 |
|
|
|
|
|
|
|
|
|
|
( |
|
) |
|
( 4 |
|
|
|
13 |
|
13 ) |
|
|
3×3 |
|
|
|
|
|
|
|
|
|
|
13 |
|
|
13 |
|
4 |
|
13 |
|
|
|
||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
q4 = cos(θ / 2) |
|
|
|
(62b) |
where A SO (3), which is a Lie group of orthogonal matrices |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
where nˆ is a unit vector corresponding to the axis of rotation, |
with determinant 1 (i.e., AT A = I and det (A)=1). |
A simpler |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
and θ is the angle of rotation. |
|
|
|
|
|
|
|
|
form for the attitude matrix in Equation (68) is given by |
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
The quaternion kinematic equations of motion are derived |
|
|
|
|
|
|
|
|
A(q)= −ΞT (q)Ψ(q) |
|
|
|
|
|
|
|
(69) |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||
by using the spacecraft’s angular velocity ( ω ), |
|
where |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||
|
|
|
|
|
|
1 |
Ω(ω) |
|
|
|
|
|
1 |
Ξ( |
|
)ω |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||
|
q |
= |
q |
= |
q |
(63) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−q |
|
|
|
|
|
|
|
|
+ q |
|
|
|
|
|
× |
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||
|
2 |
2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
I |
3×3 |
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||
where Ω(ω) and Ξ( |
|
|
) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Ψ( |
|
|
|
) |
|
|
|
|
|
|
|
4 |
|
|
|
|
|
|
|
13 |
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
q |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(70) |
|||||||||||||||
q |
are defined as |
|
|
|
|
|
|
|
|
|
|
≡ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
q |
T |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
13 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
7 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
American Institute of Aeronautics and Astronautics |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Also, another useful identity is given by |
component of d first appears in qˆ is two, so that pi = 2 . |
Ψ(q)ω = Γ(ω)q |
for any ω3×1 |
(71) |
Therefore, the Λ , z |
||||
|
|||||||
where |
|
|
|
|
|
|
|
|
|
−[ω×] |
−ω |
(72) |
|
||
|
|
Γ(ω)≡ |
|
|
|
|
|
|
|
|
ωT |
|
|
|
|
|
|
|
0 |
|
|
||
|
|
|
|
|
|
|
|
The true angular velocity is assumed to be modeled by
(73) where
, and e quantities are given by
Λ = |
∆t2 |
|
(79a) |
|||
2 |
|
I |
|
|||
|
|
|
|
|
|
|
1 |
+ |
∆t2 |
2 |
(79b) |
||
z = ∆tL f |
2 |
L f |
||||
|
|
|
|
|||
e = BB − A(qˆ)BI |
(79c) |
|||||
|
|
|
|
|
|
|
|
ω is |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ωg is |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Lk |
|
|
≡ |
Lk |
(c |
) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(80) |
|||||||||||||||||||||||||
where |
the |
true |
|
|
angular |
velocity, |
the |
gyro- |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
f |
i |
|
|
|
|
|
f |
|
i |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||
determined angular velocity, and ωb |
is the gyro drift vector, |
Using the definitions in Equations (64b), (71), and (72), the |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
which is modeled by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
derivative of Equation (78) with respect to qˆ |
|
is |
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ωb = |
η |
2 |
|
|
|
|
|
|
|
|
|
|
|
(74) |
|
|
|
∂ c |
|
= −2ΞT |
(qˆ)Γ(BI ) |
|
|
|
|
|
|
|
|
|
(81) |
|||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||
The 3×1 vectors, |
η |
|
|
|
|
|
|
|
|
|
|
η |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
∂ qˆ |
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||
1 and |
2 , are assumed to be modeled by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||
Therefore, L1f is given by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||
Gaussian white-noise processes with |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
E η |
|
(t ) |
|
= 0 |
|
i =1, 2 |
|
|
|
|
|
|
|
(75a) |
1 |
|
|
|
|
|
|
|
T |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
{ |
|
|
|
i |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
L f |
|
= −Ξ |
|
|
(qˆ)Γ(BI )Ξ(qˆ)ωˆ |
|
|
|
|
(82) |
||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||
|
|
|
(t )ηT |
(t′) |
|
|
|
|
|
(t −t′) |
|
|
(75b) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||
|
E η |
|
|
= Q δ |
|
|
δ |
i, j = |
1, 2 |
The derivative of Equation (82) with respect to qˆ |
is given by |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
{ |
|
|
i |
|
|
|
|
j |
} |
|
|
|
|
|
i |
ij |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
∂ L1f |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||
Predictive Filtering |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
T |
( |
|
) |
|
( |
|
|
|
|
I ) |
|
|
|
|
|
||||||||||||||||||||||||||||||||||||
The nonlinear predictive filter minimizes the following |
|
|
∂ qˆ |
= 2 |
ωˆ |
× Ξ |
|
qˆ |
|
Γ |
|
|
B |
|
|
|
|
|
|
|
|
|
(83) |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||
J = 1 {BB − A(qˆ)BI |
} |
|
|
|
|
R−1 {BB − A(qˆ)BI } |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
is given |
|||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
The derivative of Equation (82) with respect to |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
ωb |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
T |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
1 |
|
|
|
|
t+∆t |
|
|
|
|
|
|
|
|
|
|
|
|
t+∆t |
(76) |
by |
1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
T |
(t )W d (t ) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
+ |
|
d |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
∂ |
L f |
|
|
= ΞT |
(qˆ)Γ(BI )Ξ(qˆ) |
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
(84) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
subject to |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
∂ωˆb |
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Therefore, L2f is given by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||
|
|
|
|
ˆ |
|
|
1 |
Ω(ωˆ )qˆ |
|
|
|
1 |
Ξ(qˆ)ωˆ, |
qˆ (t0 )= q |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||
|
|
|
q |
= |
|
|
= |
|
|
|
0 |
|
|
(77a) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||
|
|
|
2 |
|
2 |
|
|
|
f |
|
|
|
|
|
ΞT |
( |
qˆ |
) |
Γ |
( |
B |
I |
) |
Ξ |
( |
qˆ |
) |
ωˆ |
|
(85) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
|
(t0 )= 03×1 |
|
|
|
|
|
|
|
|
|
|
|
|
L2 = ωˆ |
× |
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||
|
|
|
|
|
|
|
|
|
|
|
ωb = d, |
|
|
|
|
|
|
|
|
|
|
|
|
(77b) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ωb |
|
|
|
|
|
|
|
The S matrix, which is formed using Equation (8) is given by |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ωˆ = ωg −ωˆb |
|
|
|
|
|
|
|
|
|
|
(77c) |
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
S = ΞT ( |
qˆ |
)Γ(BI )Ξ( |
qˆ |
)= − A( |
qˆ |
)BI × |
(86) |
||||||||||||||||||||||||||||||||||||||||||||||||||||
This has the form given by Equation (59), with |
|
|
xˆ1 = qˆ |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
A( |
|
)BI |
× |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
and xˆ2 =ωˆb . Since the body measurements (BB ) |
|
|
|
The 3×3 matrix |
qˆ |
|
|
|
has |
at |
|
most rank |
2, which |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
are used as |
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
the required tracking trajectories, the output |
vector |
in |
reflects the fact that there is no information about rotations |
|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
Equation (2) is given by |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
around the current measurement vector [17]. |
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ˆ |
|
= |
A |
|
ˆ |
|
|
|
|
|
|
|
|
|
|
|
(78) |
The extension to multiple measurement sets is achieved |
|||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
c (x) |
|
(q)BI |
|
|
|
|
|
|
|
|
|
|
|
by stacking these measurements, e.g., |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||
Since c depends on qˆ |
|
and not explicitly on ωˆb , the lowest |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||
order |
time |
|
derivative |
|
|
of |
Equation |
(78) |
in |
which |
|
any |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
8 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
American Institute of Aeronautics and Astronautics |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
e |
|
|
|
|
|
|
|
|
field reference is modeled using a 10th order International |
||||||||||||||
|
|
|
|
|
|
1 |
|
|
|
|
|
|
|
Geomagnetic |
Reference Field |
(IGRF) model |
[19]. |
TAM |
|||||||||||
|
|
|
|
e = |
|
e2 |
|
|
|
|
|
|
(87) |
sensor noise is modeled by a Gaussian white-noise process |
|||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
with a mean of zero and a standard deviation of 0.5 mG. |
|
|||||||||||||
|
|
|
|
|
em |
|
|
|
|
|
|
|
The two DSS’s each have a field of view |
of |
about |
||||||||||||||
|
|
|
|
|
|
|
sen |
|
|
|
|
|
|
|
|||||||||||||||
where msen |
is the total number of vector measurement sets |
50°× 50° (see [16] for more details). The two DSS’s combine |
|||||||||||||||||||||||||||
to provide sun measurements for about 2/3 of a complete |
|||||||||||||||||||||||||||||
available at time tk . |
|
|
|
|
|
|
|
|
|
|
|
orbit. |
The DSS sensor noise is also modeled by a Gaussian |
||||||||||||||||
An advantage of the nonlinear predictive filter over the |
white-noise process |
with a |
mean |
of |
zero |
and |
a |
standard |
|||||||||||||||||||||
deviation of 0.05°. The gyro “measurements” are simulated |
|||||||||||||||||||||||||||||
conventional |
|
Kalman filter |
for |
attitude |
estimation |
is |
that |
||||||||||||||||||||||
|
using |
Equations (73) |
and (74), |
with |
|
a gyro noise |
standard |
||||||||||||||||||||||
quaternion |
normalization |
is |
|
always |
preserved. |
|
The |
|
|||||||||||||||||||||
|
|
deviation of 0.062 deg/hr, a ramp noise standard deviation of |
|||||||||||||||||||||||||||
normalization |
constraint |
leads |
|
to |
a |
singularity |
in |
the |
|||||||||||||||||||||
|
0.235 deg/hr/hr, and an initial drift of -0.1 deg/hr. |
|
|
|
|||||||||||||||||||||||||
covariance matrix of the Kalman filter. Several methods are |
|
|
|
||||||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||||
shown in references [17] and [18] which overcome this |
A plot of the roll, pitch, and yaw attitude errors using the |
||||||||||||||||||||||||||||
difficulty. However, since Equation (77) is used to determine |
propagated gyro model for a typical simulation run is shown |
||||||||||||||||||||||||||||
the quaternion estimate, normalization is always maintained in |
in Figure 3. |
Clearly, the gyro model used in the simulation |
|||||||||||||||||||||||||||
the nonlinear |
predictive |
filter. |
|
Also, the predictive |
filter |
produces significant errors in pitch. |
Also, the roll and yaw |
||||||||||||||||||||||
determines the model-error trajectory as part of the solution, |
errors eventually drift to large errors. |
|
A plot of the gyro-bias |
||||||||||||||||||||||||||
as opposed to the Kalman filter which assumes that this error |
estimates using the predictive filter is shown in Figure 4. The |
||||||||||||||||||||||||||||
is modeled by a Gaussian process with known covariance. |
predictive filter is able to accurately estimate for the gyro- |
||||||||||||||||||||||||||||
Attitude Estimation of TRMM |
|
|
|
|
|
|
biases. A plot of the predictive filter covariance is shown in |
||||||||||||||||||||||
|
|
|
|
|
|
Figure 5. This shows that the y-axis gyro-bias covariance is |
|||||||||||||||||||||||
In this section, the predictive filter is used to estimate the |
|||||||||||||||||||||||||||||
smaller than |
the x and z-axes. |
This |
is most likely due to |
||||||||||||||||||||||||||
attitude of the TRMM spacecraft from vector measurement |
roll/yaw quarter-orbit coupling for Earth-pointing spacecraft |
||||||||||||||||||||||||||||
observations and gyro measurements. The TRMM spacecraft |
(i.e., a yaw rate error becomes a roll angle error as the |
||||||||||||||||||||||||||||
(see Figure 1) is due to be launched in 1997 with a nominal |
spacecraft rotates in the orbital plane). |
A plot of the attitude |
|||||||||||||||||||||||||||
mission lifetime of 42 months. |
The main objectives of this |
errors using the predictive filter is shown in Figure 6. |
|
|
|||||||||||||||||||||||||
mission |
include: (i) to obtain |
|
multi-year measurements of |
A plot of the quaternion normalization for the Kalman |
|||||||||||||||||||||||||
tropical rainfall, (ii) to understand how interactions between |
|||||||||||||||||||||||||||||
filter and the predictive filter is shown in Figure 7. |
For the |
||||||||||||||||||||||||||||
the sea, |
air, |
|
and land masses |
produce |
changes |
in |
global |
||||||||||||||||||||||
|
Kalman filter, the top of Figure |
7 |
shows |
the post-update |
|||||||||||||||||||||||||
rainfall and climate, and (iii) to help improve the modeling of |
|||||||||||||||||||||||||||||
normalization |
error, |
where |
the |
quaternion |
is |
explicitly |
|||||||||||||||||||||||
rainfall processes and their influence on global circulation. |
|||||||||||||||||||||||||||||
normalized before being propagated to the next measurement. |
|||||||||||||||||||||||||||||
The spacecraft is three-axis stabilized in a near circular |
|||||||||||||||||||||||||||||
The Kalman filter has larger errors at the start of the |
|||||||||||||||||||||||||||||
(350 km) orbit with an inclination of 35°. Figure 2 shows the |
simulation run due to large initial updates. |
This shows that |
|||||||||||||||||||||||||||
position of the spacecraft for a typical orbit. |
The nominal |
the linearization assumption in the Kalman filter becomes |
|||||||||||||||||||||||||||
Earth-pointing mission mode requires a rotation once per orbit |
more invalid as the updates in the state estimates become |
||||||||||||||||||||||||||||
about the spacecraft’s y-axis. |
|
The |
attitude |
determination |
larger. |
However, from the bottom plot of Figure 7, it is clear |
|||||||||||||||||||||||
hardware consists of an Earth Sensor Assembly (ESA), |
that the predictive filter is able accurately estimate the attitude |
||||||||||||||||||||||||||||
Digital Sun Sensors (DSS), Coarse Sun Sensors (CSS), a |
of the spacecraft, while preserving quaternion normalization |
||||||||||||||||||||||||||||
Three-Axis Magnetometer (TAM), and gyroscopic rate |
in the filter (to within numerical accuracy). |
|
|
|
|
|
|||||||||||||||||||||||
sensors. |
The attitude control hardware includes three |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||||||
Magnetic Torquer Bars (MTB) which are used to provide |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
magnetic momentum unloading capability, and a Reaction |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
Wheel Assembly (RWA) which consists of four wheels in a |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
pyramidal arrangement to maximize momentum storage |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
capability along a preferred axis. |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||
Primary attitude knowledge is determined by the ESA |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
and gyros. However, in the event of ESA failure, a |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
contingency mode is used which utilizes the FSS, TAM, and |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
gyros. This mode is used for attitude estimation in the |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
simulations |
for this paper. |
|
The |
simulated |
spacecraft |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||
completes an orbit in approximately 90 minutes, giving a |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||||||||||||||
rotation rate of 236 deg/hr about the spacecraft’s y-axis while |
|
|
Figure 1 TRMM Spacecraft |
|
|
|
|
|
|||||||||||||||||||||
holding the remaining axis rotations near zero. The magnetic |
|
|
|
|
|
|
|
||||||||||||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
9 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
American Institute of Aeronautics and Astronautics |
|
|
|
|
|
|
|
|
|
|
Latitude (Deg)
|
|
|
|
Geodetic Position on Earth |
|
|
|
|
|
|
|
Predictive Filter Covariance |
|
|
|
|||||||
90 |
|
|
|
|
|
|
|
|
|
|
|
|
10−3 |
|
|
|
|
|
||||
80 |
|
|
|
|
|
|
|
|
|
|
|
(Deg^2/Hr^2) |
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
70 |
|
|
|
|
|
|
|
|
|
|
|
|
10−4 |
|
|
|
|
|
|
|
|
|
60 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
50 |
|
|
|
|
|
|
|
|
|
|
|
X |
10−5 |
|
|
|
|
|
|
|
|
|
40 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
||
|
|
|
|
|
|
|
|
|
|
|
|
|
||||||||||
30 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
20 |
|
|
|
|
|
|
|
|
|
|
|
(Deg^2/Hr^2)Y |
10−3 |
|
|
|
|
|
|
|
|
|
−20 |
|
|
|
|
|
|
|
|
|
|
|
10−5 |
|
|
|
|
|
|
|
|
||
10 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
|
|
|
|
|
|
|
|
|
10−4 |
|
|
|
|
|
|
|
|
|
−10 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−30 |
|
|
|
|
|
|
|
|
|
|
|
|
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
−40 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−50 |
|
|
|
|
|
|
|
|
|
|
|
(Deg^2/Hr^2)Z |
10−3 |
|
|
|
|
|
|
|
|
|
−90 |
|
|
|
|
|
|
|
|
|
|
|
−5 |
|
|
|
|
|
|
|
|
||
−60 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−70 |
|
|
|
|
|
|
|
|
|
|
|
|
10−4 |
|
|
|
|
|
|
|
|
|
−80 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
0 |
30 |
60 |
90 |
120 |
150 |
180 |
150 |
120 |
90 |
60 |
30 |
0 |
10 |
|
|
|
|
|
|
|
|
|
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
||||||||||||||
|
|
|
|
|
Longitude (Deg) |
|
|
|
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Time (Hr) |
|
|
|
|
||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Figure 2 TRMM Spacecraft Position |
Figure 5 Predictive Filter Covariance |
Errors Between Truth and Propagated Gyro Solution
|
0.1 |
|
|
|
|
|
|
|
|
|
0.1 |
Roll (Deg) |
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
|
|
|
|
|
Roll (Deg) |
0 |
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
||
|
−0.1 |
|
|
|
|
|
|
|
|
|
−0.1 |
|
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
|
|
|
0 |
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
|
|
|
|
|
|
0.1 |
Pitch (Deg) |
|
|
|
|
|
|
|
|
|
|
|
−0.5 |
|
|
|
|
|
|
|
|
Pitch (Deg) |
0 |
|
|
|
|
|
|
|
|
|
|
|||
−1 |
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
−0.1 |
|
|
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
|
|
|
0 |
|||||||||
|
|
|
|
|
|
|
|
|
|
|
|
0.1 |
|
|
|
|
|
|
|
|
|
0.1 |
Yaw (Deg) |
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
|
|
|
|
|
Yaw (Deg) |
0 |
|
|
|
|
|
|
|
|
|
|
|||
−0.1 |
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
−0.1 |
|
|
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
|
|
|
0 |
|||||||||
|
|
|
|
|
Time (Hr) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Figure 3 Propagated Gyro Attitude Errors
Errors Between Truth and Predictive Filter Solution
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
|
|
Time (Hr) |
|
|
|
|
Figure 6 Predictive Filter Attitude Errors
Predictive Filter Gyro Drift Estimation
|
0 |
|
|
|
|
|
|
|
|
X (Deg/Hr) |
−0.1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−0.2 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
0 |
|
0 |
|
|
|
|
|
|
|
|
Y (Deg/Hr) |
−0.1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−0.2 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
0 |
|
0 |
|
|
|
|
|
|
|
|
Z (Deg/Hr) |
−0.1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−0.2 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
0 |
||||||||
|
|
|
|
|
Time (Hr) |
|
|
|
|
Figure 4 Predictive Filter Gyro-Biases Estimates
|
x 10 |
−5 |
Kalman Filter |
|
|
|
|
1 |
|
|
|
|
|
(Db) |
0.5 |
|
|
|
|
|
0 |
|
|
|
|
|
|
Norm |
|
|
|
|
|
|
−0.5 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
−1 |
0.1 |
0.2 |
0.3 |
0.4 |
0.5 |
|
0 |
|||||
|
|
|
Time (Hr) |
|
|
|
x 10 |
−10 |
Kalman Filter |
|
|
|
5 |
|
|
|
|
|
4 |
|
|
|
|
(Db) |
3 |
|
|
|
|
2 |
|
|
|
|
|
Norm |
|
|
|
|
|
1 |
|
|
|
|
|
|
|
|
|
|
|
|
0 |
|
|
|
|
|
−1 |
|
4 |
6 |
8 |
|
2 |
|
Time (Hr)
−15 |
Predictive Filter |
x 10 |
|
|
4 |
|
(Db) |
2 |
|
0 |
||
Norm |
||
−2 |
||
|
−4
0 |
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
|
|
|
|
Time (Hr) |
|
|
|
|
Figure 7 Quaternion Normalization for EKF and PF
10
American Institute of Aeronautics and Astronautics