Location of accelerometers and Dynamic system

Putting an accelerometer at the point P and ηv is the sensing direction of this accelerometer. Output of the accelerometer can be written as the form (2.3).

( ) ( ( ) )

Because of there are six degrees of freedom, six accelerometers must be required at least. Six outputs of accelerometers can be constructed by matrix form. Formula (2.5) presents those on the body. Conveniently in use, J substitutes the relation between ηv and rv as shown in forming (2.4).

( ) ( ( ) )

Formula (2.6) shows the relations between output signals of each accelerometer and dynamic properties of the body in motion and includes linear acceleration of aircraft, angular rate and angular acceleration that they are all represented in the rotation frame.

There is an important issue with matrix J in Eq.2.6. In order to estimate angular acceleration and linear acceleration from (2.6), matrix J must be invertible. In other words, rank of J must be full rank. Formula (2.7) shows the “governing equation” in our model (IMU). Formula (2.8) presents the system output for observer to use. Formula (2.7) is an unstable system and there is white noise in practical application, so state estimation (observer) must be used to observe the real state without this noise. This question is taken up in the next section.

( ) ( ( ) )

[ ]

In order to ensure matrix J invertible, designing the relation between location and sensing direction becomes an important issue. One way we used is that putting nine accelerometers in the same circular on the flat, Fig. 2 illustrates the location and sensing direction about all accelerometers and Table 1 shows these parameters. Location and sensing direction of the first three and 7^th, 8^th accelerometers sense the tangential acceleration, the 4^th to 6^th accelerometers sense acceleration of Z-axis only and 9^th accelerometer sense the centripetal acceleration. Matrix J can be rewritten as (2.9) and the determinant of matrix J is not zero as shown in formula (2.10).

⎥⎥

M : mth coordinate base.

0

Table 1 Location and Sensing direction of the accelerometers

Location

Sensing direction ]

Accelerometer 1 ⎥

⎦

Accelerometer 3 ⎥

⎦

Accelerometer 4 ⎥⎦

Accelerometer 5 ⎥

⎦

Accelerometer 6 ⎥

⎦

Accelerometer 7 ⎥

⎦

Fig.2.2.1 Location and Sensing direction of the nine accelerometers

According to the above parameters of accelerometers, governing equation (2.7) and output equation (2.8) can be rewritten as (2.11) (2.12).

( )

Formula (2.11) and (2.12) present the dynamic system in the continuous time domain.

In order to program the above system,ω& can be discretize by _j ω_j

(

k+1

)

､ ω_j

( )

k and dtas formula (2.13). Formula (2.14) and (2.15) show the discrete system from (2.11) and (2.12) by (2.13).

Consider the nonlinear system with dynamics

(

k 1

)

f

[

k x

( )

k

]

v

( )

k

x + = + (2.16)

where, for simplicity, it is assumed that there is no control, and the noise is assumed additive, zero mean, and white Gaussian distribution.

[ ]

v

( )

k 0

( ) ( )

⎥⎦⎤=R

( )

k δ_kj

⎢⎣⎡ ′

= E v k v j

E (2.17)

The measurement is

( )

k h

[

k x

( )

k

] ( )

k

z = +ω (2.18)

Where the measurement noise is additive, zero mean, and white

[

ω

( )

k

]

0 ω

( ) ( )

k ω j ⎥⎦⎤=R

( )

k δ_kj

⎢⎣⎡ ′

= E

E (2.19)

Because of this is a nonlinear system, the system need to be linearization.

Formula (2.20) uses Taylor series expansion to linearize system and measurement for obtaining the optima estimation of the system.

( )

ei :i Cartesian basis vector, ^th

State prediction

The predicted state to time k+ from time k is obtained by taking the expectation 1 of (2.20) neglecting HOT.

( ) [ ( ) ] _∑ [ ( ) ]

And prediction covariance is:

( ) ( ) ( ) ( )

Measurement prediction

Similarly, the predicted measurement is, for the second-order filter

The measurement prediction covariance or innovation covariance or residual covariance--really MSE matrix—is

( )

( ) ( ) ( )

( ) ( ) ( ) ( )

[

^{h k 1P k 1|k h k 1P k 1|k}

]

^R

⁽

^{k 1}

⁾

tr e 2 e

1

1 k h k

| 1 k P 1 k h

1 k S

x x

n 1 i

n 1 j

j xx i

xx j i

x x

+ + + +

+

′ + +

+ ′ +

+

= +

∑∑

= =

(2.24)

Filter gain

(

k 1

) (

P k 1

) (

h_x k 1

) (

Sk 1

)

^-1

W + = + + ′ + (2.25)

Update

The state estimate updated is

(

k 1|k 1

) (

x k 1|k

)

W

(

k 1

) (

ν k 1

)

x + + =ˆ + + + +

ˆ (2.26)

and state covariance updated is

(

k+1|k+1

) (

=P k+1|k

) (

-W k+1

) (

S k+1

) (

W k+1

)

^′

P (2.27)

The optima estimation will obtain from the above calculating repeatedly.

Using the first-order extended Kalman filter, the higher-order term and the Hessian of f, and h will be neglected. For simplify to use the first order extended Kalman filter, Table 2 is presented the flow chat included state prediction and state update.

W(k+1) U(k)

Evolution of the system

(true state)

Known input (control or

Sensor motion)

Estimation of the state

State covariance computation

State covariance at tk

P(k|k)

Evaluation of Jacobians & Hessian

)

Measurement prediction

)]

State prediction

)]

Measurement residual

( ) ( )

Measurement at tk+1

) 1 k (

z + =

Transition

)

Update state estimate

Filter gain

)1

Residual covariance

)

State prediction covariance

)

State estimate at tk

xˆ^(k|k)

Input at tk

U(k) State at tk

X(k)

2.3.2 The Iterated Extended Kalman Filter

A modified state updating approach can be obtained by an iterative procedure as follows. that is, the same as the first-order (no iterated) EKF.

Overview of the Iteration Sequence

For i=(0:N-1)

(2.28)

(2.29)

(2.30)

For i=N

(2.30)

with N decided either a prior or based on a convergence criterion.

Through the above procedure to calculate angular rate and linear acceleration represented in body frame repeatedly, we can obtain the optima estimation at timek+ . 1

2.4 Orthogonal Transfer Matrix

In order to consider the angular rate and linear represented in inertial frame, the orthogonal matrix should be used to transfer the representation to another base which interests us. Formula (2.31) shows the relation between two different bases.

i

b AX

X =

(2.31)

Where

A : Orthogonal transfer matrix,

X : Representation in inertial frame. i

The Euler anglesθ , φandϕare an orthogonal transform to transfer the quantities in the rotation frame into those in the inertial frame.

Fig.2.4.1 shows the sequence that is started by rotating the initial system of axes, XYZ, by an angleφcounterclockwise about the Z axis, and the resultant coordinate system is labeled theξηζ axes. In the second step, the intermediate axes,ξηζ , are rotated about the η axis counterclockwise by the angle θ to produce another intermediate set, theξ′η′ζ′axes. Finally, the ξ′η′ζ′axes are rotated counterclockwise by an angle ϕ about theζ′ axis to produce the desired X′Y′Z′ system of axes.

Follow the above three separate rotation is called Y-Convention. Formulas (2.32) to (2.34) show the relation like (2.31) in the three rotations. Formula (2.35) shows the result of the three separate rotations, and A is the orthogonal transfer matrix defined in formula (2.31). Formula (2.36) shows inverse A to transfer representation from the body frame to the inertial frame.

DXi

ξ = (2.32)

ξ

ξ′=C (2.33)

ξ′

= B

X^b (2.34)

their time derivatives as (2.37). In consequence of the vector property of infinitesimal rotations, the vector Euler angles time derivatives can be obtained as the sum of the three separate angular rate vectors and ω . The angle θ must avoid being zero carefully, or make the element in the (2.37) singular.

⎥⎥

Fig.2.5.1 shows the procedure how to use these accelerometers to obtain all information in the body frame or inertial frame.

Fig.2.5.1 The flow chart of all algorithms Iterate EKF

φ θ ϕ, , Eular angle

φ

System(algorithm) A1~A6

A7~A9

CHAPTER 3

SIMULATION AND PRELIMINARY TEST

The algorithm introduced in Chapter 2 will be proved by simulating with Matlab package. In this proof, consider about the random noise with accelerometer output will be measured from static-tested of an accelerometer on a stage. In order to make sure that the experiments are accurate, we test the response of one axis with one accelerometer at first. The structure of test will be presented in section 3.2. Section 3.3 shows that how to simulate the motion and define the suitable accelerometer output signal. Section 3.4 shows that preliminary tested from an accelerometer in the above motion we defined.

Section 3.5 shows Matlab simulation to ensure the algorithm we derived in Chapter 2.

3.1 Test structure

In order to get pure sine wave acceleration, the function generator employs to provide PWM servo amplifiers command and those drives the DC motor on the stage. The stage is driven by Dc motor and generates a sinusoidal acceleration which is less than 1g. And the output of accelerometer (ADXL105) is set to a nominal scale factor of 250 mV/g.

However, in order to reduce SNR and increase accuracy, the circuits used to amplify output voltage must be needed.

Information of encoder of the DC motor is measured by motion card (ADLINK PCI-8133) and the accelerometer output signals are measured by DAQ card (ADLINK

Fig.3.2.1 Structure of uni-axis test

3.2 Generate Motion

Pure sine wave assists us to analysis the signal easily. It is different between the input commands of the DC motor from the output of accelerometer, because it loses the energy on stage. In order to make sure that the signal is the pure sin wave, tuning the frequency of the function generator until the differential data of encoder twice to be like the pure sinusoidal response. Fig.3.3.1 to Fig.3.3.3 shows the result of these procedures.

Fig.3.3.1 1HZ sin wave of function generator Motion card (ADLINK PCI-8133) Function generator

PWM servo amplifiers

Stage (ILS100CC)

Compare position error Accelerometer (ADXL105)

output signal

DAQ card (ADLINK PCI-9114)

∫∫

^dtdt

Fig.3.3.2 11HZ sin wave of function generator

Fig.3.3.3 18HZ sin wave of function generator

We can obtain the result that we increase the frequency of the function generator and the acceleration becomes pure sine wave. If we increase the frequency of the function generator over than about 20HZ, we will get the smaller amplitude of the acceleration than

In the above condition, the acceleration output will be measured by DAQ card.

Fig.3.4.1 shows the acceleration output, the data which is obtained by integrating the output of accelerometer once or twice, and we compare those with the information of the encoder. The accumulation of integral error can be indicated clearly as shown in Fig.3.4.1. The source of this error is the drift of the accelerometer output and the vibration of optical table. This question is taken up in the chapter.5.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

-5 0 5 10

mm

compare encoder and measurement

1 1.1 1.2 1.3 1.4 1.5 1.6 1.7 1.8 1.9 2

0 0.5 1 1.5

mm

1 1.1 1.2 1.3 1.4 1.5 1.6 1.7 1.8 1.9 2

-20 0 20

mm/s

1 1.1 1.2 1.3 1.4 1.5 1.6 1.7 1.8 1.9 2

-2000 0 2000

mm/s2

encoder accelerometer

encoder accelerometer

velocity from accelerometer

acceleration from accelerometer

Fig.3.4.1 Compare encoder and accelerometer output

The sampling rate of encoder is 500 HZ and the sampling rate of DAQ card is 2000 HZ. When the DC motor is static, the standard deviation is 21.9416 mm/s². The amplify scale is 1000/39, the operator voltage is 0V and 5V and the accelerometer output scale is 250 mV/g, so acceleration must be smaller than 0.39g (3900mm/ s²) with

these conditions. Formula 3.1 shows the result.

(5V/2)*(39/1000)/(0.25)=0.39 (3.1)

3.4 Simulation and Resolution

We define the path of the angular acceleration of the Euler’s angles and linear accelerations in the inertial frame as formula(3.2). We assume that sampling rate of DAQ card is 1000HZ, l=50mm and the standard deviation is 25mm/ s².

( )

Fig.3.5.1 shows the state of the algorithm we derived and Fig.3.5.2 shows the angular rate in the inertial frame.

0 1 2 3 4 5

w1 in rotated frame

0 1 2 3 4 5

w2 in rotated frame

0 1 2 3 4 5

w3 in rotated frame measured

Fig.3.5.1 angular rate representation in Body frame

0 1 2 3 4 5

w1 in Inertial frame

0 1 2 3 4 5

w2 in Inertial frame

0 1 2 3 4 5

w3 in Inertial frame measured

Fig.3.5.2 angular rate representation in the Inertial frame

Fig.3.5.3 shows the Euler’s angular rate and Fig.3.5.4 shows the Euler’s angle.

There is pulse at 1.5, 3 and 4 seconds, because the matrix in formula (2.37) is singular.

0 1 2 3 4 5 -8000

-6000 -4000 -2000 0 2000 4000

rad/s

w_fife in euler anlge

0 1 2 3 4 5

-3 -2 -1 0 1 2 3

rad/s

w_theta in euler angle

0 1 2 3 4 5

-8000 -6000 -4000 -2000 0 2000 4000

rad/s

w_phi in euler angle measured real

measured real

measured real

Fig.3.5.3 angular rate of Euler angles

0 1 2 3 4 5

-10 -5 0 5 10 15

rad

fife in euler anlge

0 1 2 3 4 5

0 2 4 6 8 10 12

rad

theta in euler angle

0 1 2 3 4 5

-10 -5 0 5 10 15

rad

phi in euler angle measured real

measured real

measured real

Fig.3.5.4 Euler angles

The iterative time is 40 for initial to 0.1 second and 20 for 0.1 second to 1 second.

Fig.3.5.5 shows the result of iteration.

0 10 20 30 40 50 -2.7375

-2.7374 -2.7373 -2.7372 -2.7371

w1

0 10 20 30 40 50

0.5322 0.5324 0.5326 0.5328 0.533 0.5332 0.5334

w2

0 10 20 30 40 50

1.0432 1.0433 1.0434 1.0435 1.0436 1.0437 1.0438

w3

Iterative convergence Iterative convergence

Iterative convergence

Fig.3.5.5 Iterative convergence of angular rate representation in body frame

Fig.3.5.6 shows the linear accelerations in the body frame and Fig.3.5.7 shows the linear accelerations in the inertial frame.

0 1 2 3 4 5

-3000 -2000 -1000 0 1000 2000 3000

mm/s2

Fr1

0 1 2 3 4 5

-4000 -2000 0 2000 4000

mm/s2

Fr2

0 1 2 3 4 5

-4000 -2000 0 2000 4000

mm/s2

Fr3

measured real

measured real

measured real

Fig.3.5.6 Linear acceleration representation in Body frame

1 1.2 1.4 1.6 1.8 2 -2000

-1000 0 1000 2000

mm/s2

F1 in Inertial frame

1 1.2 1.4 1.6 1.8 2

-2000 -1000 0 1000 2000

mm/s2

F2 in Inertial frame

1 1.2 1.4 1.6 1.8 2

-2000 -1000 0 1000 2000

mm/s2

F3 in Inertial frame measured real

measured real

measured real

Fig.3.5.7 Linear acceleration representation in Inertial frame

Resolution

0 50 100 150

-0.06 -0.04 -0.02 0 0.02 0.04

rad/s

w1 in rotated frame

measured real

0 50 100 150

-0.04 -0.02 0 0.02 0.04 0.06

rad/s

w2 in rotated frame

measured real

0 50 100 150

-0.02 0 0.02 0.04 0.06 0.08

rad/s

w3 in rotated frame

measured real

acceleration and linear acceleration is zero, and rewrite Eq.2.3 as Eq.3.3. We can find out the relationship between resolution of accelerometer and angular rate of our algorithm shown as Eq.3.4.

CHAPTER 4

TEST OF THE IMU DESIGN

To verify the feasibility and reliability of the method for computing angular rate, it is necessary to perform a two-step validation procedure, i.e., the method has to yield consistent and accurate results while acquiring data both from hypothetical and experimental systems.

To check if our method is feasible for any arbitrary motion, some experiments with different configurations should be done. According to the type of Euler＇s angle (ZYZ convention), we design three different experiments and simulations. First, Z axis in the body frame is parallel to the axis of rotation and the other two linear motions are parallel to the inertial frame. Secondly, initial condition is changed in order to verify the observer method is practical when acceleration of gravity effect on the output of accelerometer.

Thirdly, Y axis in body frame is parallel to the axis of rotation and the other two linear motions are parallel to the inertial frame.

Section 4.1 describes the procedures for these experiments with the designed motion, analog amplifier, and Butterworth filter. Section4.2, 4.3 and 4.4 describes the first, second and third experiment and simulation, respectively.

4.1 Experimental procedure

Basic procedure of experiment

Three different function generators output sinusoidal voltage to three different PWM servo amplifiers. These servo amplifiers output sinusoidal current to three different stages. Sinusoidal current excite sinusoidal acceleration on the stage and we put our IMU on the stage to sense the sinusoidal acceleration and read the encoder information by motion card (ADLINK PCI-8133). System noise or disturbance will be generated when three stages create the sinusoidal. And the acceleration output of the ADXL105 is nominally 250mV/g. This scale factor is not appreciated for our applications. An amplifier is need to set an appreciate scale ratio and a low-pass filter is needed to filter accelerometer’s internal or circuit high-frequency noise, so analog 1-pole low-pass filter will be employed to filter those between accelerometer and DAQ card (ADLINK PCI-9114). But other high-frequency noise will be excited when data of acceleration are transferred between the output of 1-pole low-pass filter and DAQ card. The digital filter (4^th order Butterworth filter) must be needed to filter this noise by computer. Let the result of digital filter be as acceleration output (A1~A9) shown as in algorithm, and physical quantities can be calculated by our observer base IMU. To compare these quantities and encoder’s information, and we will know our algorithm be practical or not.

We will discuss every physical quantities which interest us to be practical or not in the following sections. Fig.4.1.1 and Fig.4.1.2 show the process of experiment and the flow chart of algorithm respectively.

Fig.4.1.1 Experimental procedure

DAQ card (ADLINK PCI-9114) Motion card (ADLINK PCI-8133) Function generator

PWM servo amplifiers

Stage(ILS100CC & RV80CC)

Accelerometer

(ADXL105) output signal

Analog amplifier

Butterworth

A1~A9

A1

A2

A3

A4

A5

A6

A7

A8

ω ^v

b

[ ]

^{e b}

e

θ ω

θ

& = ^v

ω ^v

e

System

f

b

v ^{f v}

ⁱ

⁼ [ ] ^w

^e

^{f v}

^b

^{f v}

ⁱ

^{∫∫ p}

system

Route of motion

Because sinusoidal current excited sinusoidal acceleration on the stage, Eq.4.1 describes the relationship about displacement, velocity and acceleration. Fig.3.4.1 shows tendency of Eq.4.1.

( )

( )

( ) ( )

t

ft f f

ft f f

ft

π π π

π π π

π

2 2 1

2 sin nt 1

Displaceme

2 2 1

2 cos Velocity 1

2 sin on Accelarati

2 +

−

=

+

−

=

=

(4.1)

Low-pass analog amplifier (single pole)

Fig.4.1.3 Circuit of low-pass analog amplifier (single pole)

Fig.4.1.3 presents the circuit of low-pass filter and Fig.4.1.4 shows the Bode plot of low-pass filter. Eq.4.2 describes the gain is 5.1282 and Eq.4.3 shows the transfer function when the cutoff frequency is 117.0257 HZ, and.

+ -

R1 R2

C

IN OUT

VMID

F 10 8 . 6 C

K 39 R2 K 200 R1

R2 GAIN R1

2ππCR f 1

9 3

−

−

×

=

Ω

= Ω

=

−

=

dB =

(4.2)

1 00136 . 0

128 . T 5

= +

s (4.3)

-30 -20 -10 0 10 20

Magnitude (dB)

10¹ 10² 10³ 10⁴ 10⁵

-90 -45 0

Phase (deg)

analog low pass filter

Frequency (rad/sec)

Fig.4.1.4 Low-pass analog amplifier (single pole)

Butterworth filter

needed, an elliptic or Chebyshev filter can generally provide steeper roll-off characteristics with a lower filter order. Eq.4.4 shows the Z-transform and Fig.4.1.5 shows frequency response when data sampled at 2000 Hz and design a 4th-order low-pass Butterworth filter with cutoff frequency of 50 Hz.

( )

⁵ ₁^{4 1} ₂ ^{4 2} ₃ ^{4 3} _{1 4} ^{5 4}

Frequency (Hz)

Phase (degrees)

0 100 200 300 400 500 600 700 800 900 1000

Frequency (Hz)

Magnitude (dB)

Butterworth

Fig.4.1.5 Butterworth low-pass filter (fourth order)

There is invariably a time delay between a demodulated signal and the original received signal. The Butterworth filter parameters directly affect the length of this delay.

4.2 Z-axis rotation with biaxial linear acceleration

In this experiment, Z axis in body frame is parallel to the axis of rotation and the other two linear motions are parallel to X axis and Y axis in the inertial frame respectively.

Fig.4.2.1 shows this experiment setup. Eq.4.5 presents Euler＇s transform in these condition. Generally speaking, whether the quantity of Z-axis in the rotation frame or in the inertial frame must be equal to the Euler＇s angle

(

φ+ϕ

)

, and the quantities of X-axis, Y-axis and Euler＇s angleθ must be zero. But that is not exact correct in practical experiment. We shall now look more carefully into Eq.4.5, the quantity of Z-axis must be equal to the Euler＇s angle

(

φ+ϕ

)

when the disturbance or noise in the Euler＇s angleθ is close to zero, and the quantity of Z-axis is equal to ϕ when the disturbance or noise in the Euler＇s angleθ is larger then a value which is over than 0.06 radian after 0.2 seconds in this experiment. Compared Fig.4.2.7 with Fig.4.2.8, and we can clearly understand this question. Fig.4.2.2 shows accelerations which are through analog low-pass filter and Fig.4.2.3 shows accelerations which are through analog low-pass filter and then are through digital Butterworth low-pass filter. The curve in Fig.4.2.3 is smoother then in Fig.4.2.2 and in Fig.4.2.3; there is invariably a time delay which is introduced in the above section. In Fig.4.2.4 and Fig.4.2.6~4.2.13, the information of encoder shown by solid line, result of estimated shown by dotted line and subtracting time delay shown by dash-dot line. The rule holds in the flowing two sections.

Fig.4.2.1 Experimental set up

0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2 -1

-0.5 0 0.5 1

x 10⁴ nine accelerometer output with only_rotation motion

mm/s2

sec

NO.1 NO.2 NO.3 NO.4 NO.5 NO.6 NO.7 NO.8 NO.9

Fig.4.2.2 Nine accelerometers output

-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x 10⁴ nine accelerometer output with only_rotation motion after filtering

mm/s2

NO.1 NO.2 NO.3 NO.4 NO.5 NO.6 NO.7 NO.8 NO.9

Fig.4.2.4 shows the angular rate which is estimated from observer and obtained from differentiating the data of encoder in the rotation frame, Fig.4.2.10 shows that those are in the inertial frame and Fig.4.2.7 shows the Euler＇s angular rate. These quantities in X-axis and Y-axis should converge to zero, and in Z-axis should be the sinusoidal wave which is according Eq.4.1. But zero is smaller then resolution in our algorithm, these two curves will not converge to zero. We simulate the same condition as shown in Fig.4.2.5, and angular rate don’t converge to zero in X-axis and Y-axis. Because quantity in Z-axis in the rotation frame is equal to it in the inertial frame and

(

φ+ϕ

)

, angular displacement can be obtained by integrating from angular rate directly whether it is in the rotation or inertial frame as shown in Fig.4.2.6 and Fig.4.2.11 respectively. Euler＇s angular displacement is obtained from solving differential equation (Eq.2.37) directly and shown in Fig.4.2.9. The integral method will accumulate the error and make serious mistake when using Euler＇s transform as shown in Fig.4.2.13.

0 0.1 0.2 0.3

0 0.5 1 1.5

rad/s

w1 in rotated frame

0 0.1 0.2 0.3

-1 -0.5 0

rad/s

w2 in rotated frame

0 0.05 0.1 0.15 0.2 0.25 0.3

-1 -0.5 0 0.5 1 1.5

rad/s

w3 in rotated frame

rotation frame differentiate encoder subtract delay time

Fig.4.2.4 Angular rate in the rotation frame

0 0.05 0.1 0.15 0.2 0.25 -0.05

0 0.05 0.1 0.15

rad/s

w1 in rotated frame

0 0.05 0.1 0.15 0.2 0.25

0.1 0.2 0.3 0.4

rad/s

w2 in rotated frame

0 0.05 0.1 0.15 0.2 0.25

0 0.2 0.4 0.6 0.8 1

rad/s

w3 in rotated frame

measured real

Fig.4.2.5 Angular rate in the rotation frame (simulation)

0.05 0.1 0.15 0.2 0.25 0.3 0

0.05 0.1 0.15 0.2 0.25

rad

angle1 in rotation frame

0 0.1 0.2 0.3

-0.06 -0.04 -0.02 0

rad

angle2 in rotation frame

0 0.05 0.1 0.15 0.2 0.25 0.3

-0.04 -0.02 0 0.02

rad

angle3 in rotation frame

rotation frame encodeer subtract delay time

0.05 0.1 0.15 0.2 0.25 -20

-15 -10 -5 0

rad/s

w_fife in euler anlge

0.05 0.1 0.15 0.2 0.25 -0.5

0 0.5 1

rad/s

w_theta in euler angle

0 0.1 0.2 0.3

0 5 10 15 20

rad/s

w_phi in euler angle

eular frame differentiate encoder subtract delay time

Fig.4.2.7 Euler’s angular rate

0 0.1 0.2 0.3

-20 -15 -10 -5 0

rad/s

wfife in euler anlge

0.05 0.1 0.15 0.2 0.25 -0.5

0 0.5 1

rad/s

wtheta in euler angle

0.05 0.1 0.15 0.2 0.25 0.3 -1

-0.5 0 0.5 1 1.5

rad/s

wfife+w

theta in euler angle substrate offset eular frame rotation frame differentiate encoder

Fig.4.2.8 Euler’s angular rate (

(

ωφ +ωϕ

)

in the third sub-figure)

0.05 0.1 0.15 0.2 0.25 0.3 -1.5

-1 -0.5 0

fife in euler anlge

0 0.1 0.2 0.3

0 0.05 0.1 0.15 0.2 0.25

theta in euler angle

0 0.05 0.1 0.15 0.2 0.25 0.3 -0.06

-0.04 -0.02 0 0.02 0.04

fife+phi in euler angle

eular frame

eular frame

在文檔中 觀測器基礎純加速規平面式慣性量測單元 (頁 21-0)