Application of state observers in attitude estimation using low-cost sensors

Transcription

1 Application of state observers in attitude estimation using low-cost sensors Martin Řezáč Czech Technical University in Prague, Czech Republic March 26, 212

2 Introduction motivation for inertial estimation unit stabilized platform employed sensors Kalman vs. Complementary Filter attitude estimation implementation issues correction of translational and centripetal accelerations measured by accelerometer

3 Sensors models 3-axis gyroscope: ω m = ω + η + b

4 Sensors models 3-axis accelerometer: a = v + ω v + R ϕ G + η 3-axis magnetometer: m = R ϕ [M ] T x M y M z + η magnetic field calculator: With neglecting translational acceleration term and centripetal term a = R ϕ [ G z ] T + η

5 Single axis problem The Kalman Filter

6 Single axis problem The Complementary Filter ω m (t) ˆb(t) - k 1 θ m (t) e(t) Complementary Filter ˆθ(t) ˆb(t) ωm(t) k 2 - k 1 θm(t) e(t) Complementary Filter with bias estimation θ(s) = s + k 1 θ(s) = k 1 θ(s) + s θ(s) (1) s + k 1 s + k 1 s + k 1 θ(s) = k 1 s + k 1 θ(s) + 1 s + k 1 ω(s) (2) ˆθ(s) = k 1 s + k 1 θ m(s) + 1 s + k 1 ω m(s) (3) ˆθ(t) ˆθ(s) = k 1 ˆθ + k1 θ m + ω m (4) = ω m + k 1 (θ m ˆθ) }{{} ˆb (5)

7 From 1D to 3D 1D: single axis φ = 1 ω m 3D: Euler angles φ θ = ψ 3D: quaternions q q 1 q 2 q 3 = tan θ sin φ tan θ cos φ cos φ sin φ sin φ/ cos θ cos φ/ cos θ ω x ω y ω z ω x ω z ω y ω y ω z ω x ω z ω y ω x ω x ω y ω z q q 1 q 2 q 3

8 Attitude estimation the principle 3 types of MEMS sensors gyroscopes (ω), accelerometers (a), magnetometers (m) gyros signal is integrated to obtain attitude (ϕ), which is also measured through magnetic and gravitational fields. error signal between measured and predicted sensor outputs is fed back through gain K to attitude derivative. [ ] ω q ˆq Rˆq M R ω (ˆq) [ ˆm ] Rˆq G K [ m a] â

9 Equations for estimation of the attitude [ ] ω q ˆq Rˆq M R ω (ˆq) [ ˆm ] Rˆq G â K [ m a] q q 1 q 2 q 3 = = = = 1 2 ( ωx q 1 ω y q 2 ω z q 3 ) 1 2 (+ωx q + ω z q 2 ω y q 3 ) 1 2 (+ωy q ω z q 1 + ω x q 3 ) 1 2 (+ωz q + ω y q 1 ω x q 2 ) R(q) = ax a y = R(q) [ ] T G z a z mx m y = R(q) [M ] T x M y M z m z (1 2q 2 2 2q2 3 ) 2(q q 3 + q 1 q 2 ) 2(q 1 q 3 q q 2 ) 2(q 1 q 2 q q 3 ) (1 q 2 1 q2 3 ) 2(q q 1 + q 2 q 3 ) 2(q q 2 + q 1 q 3 ) 2(q 2 q 3 q q 1 ) (1 q 2 1 q2 2 )

10 Extended Kalman Filter Compute partial derivative of the state equation and use it to perform time update of the state estimate and estimation-error covariance: A t 1 = f(x, u) x x=ˆx + t 1, u=u t 1 x t = f(x t 1, u t 1 ) + w t 1 y t = h(x t, u t) + v t w t (, Q) v t (, R), P t = A t 1 P + t 1 AT t 1 + Q, ˆx t = f(ˆx + t 1, u t 1 ) System Compute partial derivative of the output equation and use it to perform measurement update of the state estimate and estimation-error covariance: u(t) B D w(t) A x(t) C v(t) y(t) C t = h(x, u) x x=ˆx t, u=u t K t = P t C T t (C t P ) t C T 1 t + R ˆx + t = ˆx t + K t (y t h(ˆx ) t, u t ) P + t = P t P t C T t KT t B D A ˆx(t) K e(t) ŷ(t) C Observer

11 Using feedback linearization to obtain gain K h(ˆq) [ ] ω q ˆq Rˆq M R [ ω (ˆq) ˆm â] Rˆq G q q 1 q 2 q 3 = = = = 1 2 ( ωx q 1 ω y q 2 ω z q 3 ) 1 k 2 (+ωx q + ω z q 2 ω y q 3 ) 1 2 (+ωy q ω z q 1 + ω x q 3 ) 1 2 (+ωz q + ω y q 1 ω x q 2 ) R(q) = q (C T C) 1 C T C = h(q) q e [ m a] ax a y = R(q) [ ] T G z a z mx m y = R(q) [M ] T x M y M z m z (1 2q 2 2 2q2 3 ) 2(q q 3 + q 1 q 2 ) 2(q 1 q 3 q q 2 ) 2(q 1 q 2 q q 3 ) (1 q 2 1 q2 3 ) 2(q q 1 + q 2 q 3 ) 2(q q 2 + q 1 q 3 ) 2(q 2 q 3 q q 1 ) (1 q 2 1 q2 2 )

12 Estimation of the bias in the gyro h(ˆq) [ ] ω q ˆq Rˆq M R [ ˆm ] ω (ˆq) Rˆq G â k 1 ω k 2 (B T B) 1 B T B = R ω (ˆq) ˆq q (C T C) 1 C T C = h(q) q e [ m a] - we are interested in the impact of bias (present in ω) to quaternions q B = R ω(ˆq) ω feedback is done using the inverse of B times some stabilizing constant k 2 ] B # = (B T B) 1 B T = 2 [ q1 q q 3 q 2 q 2 q 3 q q 1 q 3 q 2 q 1 q

13 Designed hardware I I I I I I output: CAN bus, RS232, possible USB ARM7 microcontroller LPC2368 Analog Devices sensor ADIS 164 (3-axis gyro, accelerometer and magnetometer), pressure sensor, GPS module 1Hz codes written in C using GnuARM compiler EKF algorithm computational speed 2 Hz (CF)algorithm computational speed 2 Hz

14 Implementation issues need to implement matrix operations (EKF) inversion of covariance matrix (better than gauss elimination is to use of matrix factorization) is it necessary to compute the inverse in itself? (CF) implementation of the pseudo-inverse paying attention on proper antialiasing filtering

15 Realtime experiments with inertial unit

16 Experiments

17 Troubles Magnetometer data mx my mz Gyroscope data gx gy gz Magnetic field [mgauss] Time [s] Acceleration [scaled] Original (measured) accelerometer ax ay az Angular rate [rad/s] Time [s] Angle [deg] Euler angles, K 1 =.5 pith roll yaw Time [s] Time [s]

18 GPS derivative 1.5 Original (measured) accelerometer ax ay az 1.5 Accelerometer after correction ax ay az Acceleration [scaled].5 1 Acceleration [scaled] Acceleration [ms 2 ] Time [s] 5 5 Measured accelerometer vs. filtered second derivative of GPS accelerometer GPS sec. der. Angle [deg] Time [s] Euler angles, K 1 =.5 pith roll yaw Time [s] Time [s]

19 Lowering feedback gain 15 1 Euler angles, K 1 =.5 pith roll yaw 15 1 Euler angles, K 1 =.1 pith roll yaw Angle [deg] 5 Angle [deg] Time [s] Time [s]

20 Implementation of GPS derivative using filtfilt in matlab (requires all data) central vs. backward finite difference filtering of the derivative noncausal = zero phase filter filter must be FIR!!! the order is DelayTime/SamplingTime.15 Coeficients of the FIR filter 5 Derivative filter bode Coef. value Magnitude [db] (db) Phase (deg) Filter shift z i FRequency [Hz] (Hz)

21 Obtaining quaternion in actual time - integrate the whole gyro buffer (by using euler discretization with period T ) q (k + 1) q 1 (k + 1) q 2 (k + 1) q 3 (k + 1) = 1 T /2 ω x T /2 ω y T /2 ω z T /2 ω x 1 T /2 ω z T /2 ω y T /2 ω y T /2 ω z 1 T /2 ω x T /2 ω z T /2 ω y T /2 ω x 1 } {{ } A(ω(k)) which although requires a lot of computer time. q(k + 1) = A(ω k )q(k) q(k + 2) = A(ω k+1 )q(k + 1) = A(ω k+1 ) A(ω k )q(k) q(k + 5) = A(ω k+499 ) A(ω k+498 ) A(ω k ) q(k) }{{} A 1 5 adding the new sample means: q(k + 51) = A(ω k+5 ) A 1 5 A(ω k ) 1 q(k + 1) q (k) q 1 (k) q 2 (k) q 3 (k) troubles with quaternion normalization? Introduction matrix normalization using Frobenius norm.

22 Conclusion possible extensions introducing not only rotational model for estimation but also translational double integrator for accelerometer compared with GPS. other possible schemes used for quaternion estimation show the experiment of bias estimation Martin Řezáč