CHAPTER 4 TEST OF THE IMU DESIGN
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 104 nine accelerometer output with onlyrotation 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 104 nine accelerometer output with onlyrotation 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
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 0.1 0.2 0.3
0 5 10 15 20
rad/s
wphi 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 rotation frame differentiate encoder
Fig.4.2.9 Euler’s angular displacement (
(
φ+ϕ)
in the third sub-figure)0 0.1 0.2 0.3
0.5 1 1.5 2
rad/s
w1 in Inertial frame
0 0.05 0.1 0.15 0.2 0.25
-1 -0.5 0
rad/s
w2 in Inertial frame
-1 -0.5 0 0.5 1 1.5
rad/s
w3 in Inertial frame
inertial frame differentiate encoder subtract delay time
0 0.2 0.4 0.6 0.8 0
0.1 0.2 0.3 0.4 0.5
rad
angle1 in Inertial frame
0 0.2 0.4 0.6 0.8
-0.08 -0.06 -0.04 -0.02 0 0.02
rad
angle2 in Inertial frame
0 0.1 0.2 0.3 0.4
-0.1 -0.05 0 0.05
rad
angle3 in Inertial frame
inertial frame encodeer subtract delay time
Fig.4.2.11 Angular displacement in the inertial frame
We rewrite Eq.2.6 as Eq.4.6 to obtain linear acceleration in the rotation frame and use Euler＇s transform (Eq.2.36) to transfer it in the rotation frame into it in the inertial frame Fig.4.2.12 and Fig.4.2.13 indicate the result. With accumulation of the integration error of Euler＇s angle, acceleration diverges in the Z-axis in the inertial frame. Here, data of encoder is regard as Euler’s angle and put it into Eq.2.37 ; and we can get convergent acceleration in the inertial frame by using Euler’s angle. Fig.4.2.14 shows the convergent acceleration in the inertial frame.
0.05 0.1 0.15 0.2 0.25
subtract delay time
Fig.4.2.12 Linear acceleration in the rotation frame
[ ]
0 0.05 0.1 0.15 0.2 0.25 0.3 -2000
-1000 0 1000 2000
mm/s2
F1 in Inertial frame
0 0.1 0.2 0.3
-3000 -2000 -1000 0 1000 2000 3000
mm/s2
F2 in Inertial frame
0.05 0.1 0.15 0.2 0.25 0.3
-1000 -500 0 500 1000
mm/s2
F3 in Inertial frame
estimate real
subtract delay time
Fig.4.2.13 Linear acceleration in the inertial frame
0.05 0.1 0.15 0.2 0.25 -2000
-1000 0 1000 2000
F1 in Inertial frame
mm/s2
0.05 0.1 0.15 0.2 0.25 -3000
-2000 -1000 0 1000 2000 3000
F2 in Inertial frame
mm/s2
0 0.05 0.1 0.15 0.2 0.25 -40
-20 0 20
F3 in Inertial frame
mm/s2
estimate real
Fig.4.2.14 Linear acceleration in the inertial frame (with encoder information)
4.3 Z-axis rotation with biaxial linear acceleration and non-zero initial condition
Fig4.3.1 Experimental set up
The difference between this experiment and the above experiment is initial condition of angle of Y-axis and Z-axis as shown in Fig.4.3.1, angle of Y axis is 90 degree and Z axis is -90 degree as shown in Fig.4.3.1. Fig.4.3.1 presents the set up of this experiment.
Because initial condition in Y axis is not zero, equ.2.35 can not be rewritten as equ.4.5.
Quantities of Y axis and Z axis in the rotation frame are similar to -X axis and -Y axis in the inertial frame respectively when ϕ is still changed small enough. Quantities of X axis in the rotation frame are equal to Z axis in the inertial frame. The influence of
0 0.1 0.2 0.3 0.4 0
0.5 1 1.5 2
rad/s
w1 in rotated frame
0 0.1 0.2 0.3 0.4
-1.2 -1 -0.8 -0.6 -0.4 -0.2 0
rad/s
w2 in rotated frame
0 0.1 0.2 0.3 0.4
-1 0 1 2
rad/s
w3 in rotated frame
rotation frame differentiate encoder subtract delay time
Fig4.3.2 Angular rate in the rotation frame
0 0.1 0.2 0.3 0.4
0 0.2 0.4 0.6 0.8 1 1.2
wfife in euler anlge
rad/s
0 0.1 0.2 0.3
-2 -1.5 -1 -0.5 0
wtheta in euler angle
rad/s
0 0.1 0.2 0.3 0.4
-1 0 1 2
wphi in euler angle
rad/s
eular frame differentiate encoder subtract delay time
Fig4.3.3 Euler’s angular rate
Algorithm flow (Fig.4.2.2) indicated that the influence of integral error of Euler’s angle on linear acceleration in the inertial frame is the most serious. Because linear acceleration in the rotation frame is obtained by Eq.4.6 and it carry about a lot of white noise, it is inaccuracy when we transfer it into linear acceleration in the inertial frame with Euler’s angle. There is the same circumstance when we transfer the angular rate in the rotation frame into it in the inertial frame. The angular rate in the rotation frame is obtained by Iterated Extended Kalman filter without white noise, so it is more accuracy than linear acceleration. In this experiment, because the angular rate in the rotation frame converges after 0.3 second, the angular rate in the inertial frame is divergent shown as Fig4.3.4 and Fig.4.3.6. If accuracy Euler’s angle as encoder information is being substituted Euler’s angle calculated by Eq.2.37 shown as Fig.4.3.5 and Fig.4.3.7, the angular rate will not be divergence in X-axis in the inertial frame. Fig.4.3.4 and Fig.4.3.5 show the angular rate between 0 and 0.4 second, Fig.4.3.6 and Fig.4.3.7 show the angular rate between 0 and 4 second.
Linear acceleration in the inertial frame is also divergence as the above experiment.
Encoder information is being substituted Euler’s angle shown to obtained linear acceleration in the inertial frame as Fig.4.3.10. Fig.4.3.8 presents linear acceleration in the rotation frame and Fig.4.3.9 shows linear acceleration transferred by Euler’s angle which is calculated by Eq.2.37 in the rotation frame and it will be divergence.
0 0.1 0.2 0.3 0.4 -1
0 1 2
rad/s
w1 in Inertial frame
0 0.1 0.2 0.3 0.4
-2 -1.5 -1 -0.5 0 0.5
rad/s
w2 in Inertial frame
0 0.1 0.2 0.3 0.4
0 0.5 1
rad/s
w3 in Inertial frame
inertial frame differentiate encoder subtract delay time
Fig4.3.4 Angular rate in the inertial frame
0 0.1 0.2 0.3 0.4
-1 -0.5 0 0.5 1 1.5 2
rad/s
w1 in Inertial frame
0 0.1 0.2 0.3 0.4
0 0.5 1 1.5
rad/s
w2 in Inertial frame
0 0.1 0.2 0.3 0.4
-1 -0.5 0
rad/s
w3 in Inertial frame
estimate real
subtract delay time
Fig4.3.5 Angular rate in the inertial frame (with encoder information)
-2 0 2 4 6 -2
-1 0 1 2 3
rad/s
w1 in Inertial frame
inertial frame differentiate encoder subtract delay time
0 1 2 3 4 5
-2 -1.5 -1 -0.5 0 0.5
rad/s
w2 in Inertial frame
0 1 2 3 4 5
-0.5 0 0.5 1 1.5
rad/s
w3 in Inertial frame
Fig4.3.6 Angular rate in the inertial frame (with encoder information)
-2 0 2 4 6
-2 -1 0 1 2 3
rad/s
w1 in Inertial frame
-2 0 2 4 6
0 0.5 1 1.5 2
rad/s
w2 in Inertial frame
-2 -1.5 -1 -0.5 0
rad/s
w3 in Inertial frame estimate real
subtract delay time
inertial frame differentiate encoder
0 0.1 0.2 0.3 0.4 -4000
-2000 0 2000 4000
Fr1
mm/s2
0 0.1 0.2 0.3 0.4
-10200 -10100 -10000 -9900 -9800 -9700 -9600
Fr2
mm/s2
0 0.1 0.2 0.3 0.4
-3000 -2000 -1000 0 1000 2000 3000
Fr3
mm/s2
estimate real
subtract delay time
Fig4.3.8 Linear acceleration in the rotation frame
0 0.1 0.2 0.3 0.4
-6000 -4000 -2000 0 2000
F1 in Inertial frame
mm/s2
0.1 0.2 0.3 0.4
-4000 -2000 0 2000 4000
F2 in Inertial frame
mm/s2
0 0.1 0.2 0.3 0.4
8500 9000 9500 10000
F3 in Inertial frame
mm/s2
estimate real
subtract delay time
Fig4.3.9 Linear acceleration in the inertial frame
0 0.1 0.2 0.3 -3000
-2000 -1000 0 1000 2000 3000
F1 in Inertial frame
mm/s2
0 0.1 0.2 0.3 0.4
-4000 -2000 0 2000 4000
F2 in Inertial frame
mm/s2
0 0.1 0.2 0.3 0.4
9600 9700 9800 9900 10000 10100
F3 in Inertial frame
mm/s2
estimate real
subtract delay time
Fig4.3.10 Linear acceleration in the inertial frame (with encoder information)