You are on page 1of 6

Gyrostellar Estimation

Marc Alomar Payeras


November 16, 2014

Steady state behavior of the


Kalman filter

Lets find which solution verifies both conditions. Taking p1 > 0, p2 > 0,

In this section we are going to solve the steady-state Riccati equation,

R Qa 2 RQb
p
=

p2 = RQb
q

p = Q Q 2RQ 
4
b
a
b

F P + P F T P C T R1 CP + Q = 0
where

F =

0
0

1
0

C=


,

P =


Q=

p1
p2

Qa
0

p2
p4
0
Qb

Applying the properties of the covariance matrix, we


get the conditions

p1 p4 p22 = Qa + RQb> 0
p


R Qb > 0
p1 + p4 = Qa 2 RQb

Once we put the matrices above into the Riccati equation, we obtain


2p2
p4

p4
0

1
R

p21
p1 p2

p1 p2
p22

 
Qa
+
0

0
Qb

However, these expressions dont always hold. Lets


consider another choice, p1 > 0, p2 < 0,


=0
q

p
=
R Qa + 2 RQb

p2 = RQb
q

p =
Qb Qa + 2 RQb
4

The system of equations is

2p2 p1 /R + Qa = 0
p4 p1 p2 /R = 0

2
p2 /R + Qb = 0

Now, the conditions are

where one of the equations appeared twice on the matrices. Solving the system for pi , we find

p2 = pRQb
p1 = R (Qa 2p2 )

p4 = p1 p2 /R

p1 p4 p22 = Qa + RQb >0


p


p1 + p4 = Qa + 2 RQb
R + Qb > 0

Since all the parameters are positive, this condition is


always held. Therefore, we can conclude that these are
As we can see, there are four different solutions de- the correct values of the covariance matrix. To finish,
pending on the sign of p2 and p1 , but only one of the Kalman gains are
them satisfies the properties of the covariance matrix.
q
(
p
We know that the covariance matrix is positive-definite,
K1 = p1 /R = Qa /R + 2 Qb /R
p
which implies
K2 = p2 /R = Qb /R
(
T r(P ) = p1 + p4 > 0
In the following, we will use K20 K2 , to have a
Q
2
positive Kalman gain.
(eig) = det(P ) = p1 p4 p2 > 0
1

Steady state transfer function of


the Kalman filter

2.1

K1 s + K2
= 2
S
s + K1 s + K2

Frequency space representation

s2
= 2
G
s + K1 s + K2

Lets consider the state-space representation of the gyrostellar estimator,


To characterize them, we can simply look at the limits
s

0 and s ,



= G d + K1 S


d = K2 S

lim
= 1, lim
= 0 Low pass filter
s S
s0 S
If we apply the Laplace transform on the previous system, we get




s = G d + K1 S
lim
= 0, lim
= 1 High pass filter
s G
s0 G


sd = K2 S

2.3

where all the variables are now functions of the parameter s, i.e. S S (s). We have converted the ODE into
an algebraic system, and now we just have to rearrange
the terms to solve
the second equation, we ob it. From


tain d = K2 S /s. Plugging this value into the


first equation, we can express
in terms of S and G ,


(s + K1 ) = G + K2 S /s + K1 S . Regrouping

Noise response

2.3.1

2nd order behavior

The denominator of the transfer functions is a second order polynomial. A second order system can be characterized by the damping factor, , and the natural frequency,
n ,
s2 + 2n s + n2

the terms, we obtain

If we compare this equation to the denominator of the


transfer function, we can identify the parameters
(

n = K 2

To obtain d in terms of G and S , we just have to


= K1 /2 K2

plug the last equation into the expression for d,



To prove that > 0.707 = 1/ 2, we first raise to the
K2 S
power of 2 the expression for , 2 = K12 /4K2 . Now,
K2 (G sS )

d=
= 2
using the values of K1 and K2 found previously,
s
s + K1 s + K2
G s + (K1 s + K2 ) S
=
s2 + K1 s + K2

Now, we can write the equations for and d in matrix


form,


2.2

1
= 2
s + K1 s + K2

K1 s + K2
K2 s

s
K2



S
G

Qa + 2 RQb
Qa
R
K12
4 =
=
=
+2
K2
R
Qb
RQb
2

Lets consider the limit case 2 = 1/2. The above


expression reads
Qa
2=
+2
RQb

Transfer functions

If we use the relation G = G , we obtain sG = G in


The parameters Qa , R and Qb are all positive, meaning
the Laplace space. Putting this expression in the equathat
the above equality cannot be held. Therefore, the

tion for ,
equality implies that must be greater than 0.707.
=

The parameters n and characterize the response of


the system. If the value of is less than 1, the response
will be oscillatory (underdamped system), whereas when
> 1, the response will be overdamped (no oscillations).

s2
K1 s + K2
G + 2
S
s2 + K1 s + K2
s + K1 s + K2

Now, we can express the transfer functions


2

2.3.2

Numeric application

The table below shows the values of K1 , K2 , n , and


for different noise parameters,
SST [arcsec]
Set 1
Set 2

Set 1
Set 2

10
K1
0.045
0.027

a [/s]
104
105

i [/s2 ]

K2

n [rad/s]

3.6104

1.9102

106

1.18
0.713

Bode Diagram
50

3
3.1

Magnitude (dB)

We can see that, for set 1, the system is overdamped


( > 1), whereas for set 2, the system is underdamped
( < 1). In the latter case, the damping value is above
0.707, as we proved before. The Bode diagrams are
shown in figure 1. As we can see, the estimator filters
the low frequency measurements of the gyrometer, and
just keeps the low frequency values of the star tracker.
With regard to the two different sets, we can slightly see
the resonance peak on the second diagram, due to < 1.

SST
GYR

100

Phase (deg)

Simulink Application

90

90
4
10

10

10
Frequency (rad/s)

10

10

Simulink model
Bode Diagram

Magnitude (dB)

20
0
20
40
60
180
SST
GYR
Phase (deg)

The Kalman filter consists of 4 transfer functions (fig.


2, bottom). The expressions simply correspond to the
matrix representation found in 2.3. Notice that we only
not for .
Howhave the transfer functions for and d,

ever, we can calculate from the estimated drift and the

= G d.
gyrometer measurement,

3.2.1

50

150
180

We have created a model of the gyrostellar estimator in


Simulink (fig. 2). The input of the model is the satellite
angular rate, which is set as an harmonic signal of amplitude 0.001 /s and pulsation 0.06 /s. The SST block
models the measurements from the star tracker, and adds
white noise to the satellites attitude. The GYR blocks
does the same for the gyrometer, but also adds a constant
bias of 1 /h.

3.2

90

90
3
10

10

10

10

Frequency (rad/s)

Figure 1: (Top) Bode diagram for the set 1 of parameters.


(Bottom) Bode diagram for the set 2.

Simulation
Temporal response

We first verified that the Kalman estimator converges.


Since we know the real values of the attitude and the
drift (notice that in a real application, we wouldnt have
access to these values), we just have to compare sat to
as well as d to d.
Figure 3 compares the estimated
,
variables to the real ones. As we can see, the system
needs about 400 seconds to converge. After that time,
the filter behaves as expected.
3

0.2
Estimated Angle
Real Angle

0.18
0.16

Angle []

0.14
0.12
0.1
0.08
0.06
0.04
0.02
0

100

200

300
time [s]

400

500

600

x 10

2.5

Estimated Drift
Real Drift

Drift [/s]

1.5

0.5

0.5

Figure 2: Simulink model of the gyrostellar estimator.

100

200

300
time [s]

400

500

Figure 3: Convergence of the Kalman filter.

600

0.35

x 10

0.06 /s
0.6 /s
6 /s

0.8
0.3
0.6
0.4
Attitude error []

Angle []

0.25

0.2

0.15

0.2
0
0.2
0.4

0.1

0.6
0.05

Estimated Angle
Real Angle
0

100

200

300
time [s]

400

500

0.8
1

600

1000

2000

Drift [/s]

3000
time [s]

4000

x 10

1000
4

3.5

6000

Estimated Drift
Real Drift

Attitude estimation performance

5000

0.06 /s

Figure 4: Attitude estimation from the gyrometer. The


value diverges due to the bias drift.
3.2.2

x 10

2000

3000
time [s]
0.6 /s

4000

5000

6000

Drift [/s]

To calculate the performance of the attitude estimator,


3
we have compared the estimated attitude and the real
attitude in steady-state (i.e. after 1000 seconds). The
0
1000
2000
3000
4000
5000
6000
difference between both signals gives the attitude error,
time [s]
6 /s
x 10
which can be quantified with the standard deviation. To
3.5
have a reliable value, we have considered the time pe3
riod 1000 6000 s, with a total of 500000 points. The
standard deviation of the attitude error is 0.15 arcsec. If
0
1000
2000
3000
4000
5000
6000
we compare it to the noise of the star tracker, 10 arcsec,
time [s]
we can observe an important increase of performance.
By combining the gyrometer measurements, we have inFigure 5: Attitude and drift estimation when the gyromcreased the accuracy by two orders of magnitude.
eter has harmonic noise.
Drift [/s]

3.2.3

No SST measurements
havior corresponds to a band-pass filter, centered around
the cross-over frequency.

To check what would happen if only used the gyrometer,


lets set K1 = K2 = 0 (i.e. no star tracker). To obtain
the angle from gryo angular rate, we integrate the measurements of the gyrometer. However, the sensor has a
constant drift. If we integrate the drift, the error will
diverge, as we can see in figure 4. Using a star tracker
allows us to estimate the drift.
3.2.4

When we compare the estimated drift to the real drift,


we can see that for very low pulsations the estimator can
follow the variations of the drift. However, when the drift
changes faster, the estimator cant follow the oscillations
of the drift.

Harmonic drift on gyrometer


3.2.5

Now, lets look at the dynamic behavior of the filter. We


add to the constant drift an harmonic signal of amplitude
0.1 /h. In figure 5 we have plotted the attitude error
(i.e. real satellite attitude minus the estimated attitude)
for different pulsations. We can observe that when the
pulsation is 0.06 /s, the error is very low. The error increases at 0.6 /s, but decreases again at 6 /s. This be-

Harmonic noise on SST

If we consider an harmonic error on the SST measurements, the results are different. With respect to the
attitude, we can see that the low frequency noise induces more errors than the high frequency noise (figure
6). With respect to the drift estimation, we can observe
that, even if the real drift is constant (we are adding har5

monic noise on the star tracker, not on the gyrometer),


the estimator gives an oscillatory estimated drift. The
drift error is band-passed, like when the harmonic noise
was on the gyrometer.
With regard to the attitude error, we can directly link
the results of fig. 6 (top) with the Bode diagram of the
star tracker transfer function, fig. 1 (bottom). When the
pulsation is 0.06 /s, the amplitude of the attitude error
is 5.7103 . This is the same amplitude of the harmonic
noise (100 microrad), and we can deduce that the gain
is 1. This result agrees with the Bode diagram, which
shows a gain of 1 at low frequencies. When the pulsation
is 0.6 /s, the attitude error increases. This is because
the frequency is closer to the resonance peak, where the
gain is bigger than 1. However, for higher frequencies the
gain decreases strongly, which agrees with the low-pass
filter behavior observed on the Bode diagram.
3.2.6

x 10

0.06 /s
0.6 /s
6 /s

Attitude error []

Frequency behavior

2
0
2
4

In the past two sections we have verified the behavior of


the Bode diagram of fig. 1: the Kalman filter behaves
like a low-pass filter for the star tracker, and like a highpass filter for the gyrometer.

6
8

To conclude, we have seen the advantages of using sensor fusion to measure the attitude. Combining both sensors, we can increase the accuracy of one single sensor,
as we could verify in section 3.2.2. At the same time, the
filter allows us to estimate the drift of the gyrometer.
Both sensors have many complementary properties, and
the Kalman filter gets the best from both of them.

1000

2000

3000
time [s]

4000

5000

6000

3.4

x 10

Real Drift
0.06 /s
0.6 /s
0.06 /s

3.2

Drift [/s]

2.8

2.6

2.4

2.2

1000

2000

3000
time [s]

4000

5000

6000

Figure 6: Attitude and drift estimation when the star


tracker has an harmonic error.