YUFEI JIANG / Mbed 2 deprecated CSSE4011_BLE_IMU

CSSE4011_BLE_IMU IMU Seeed Tiny Ble

Dependencies:   BLE_API_Tiny_BLE MPU6050-DMP-Seeed-Tiny-BLE mbed

kalman.cpp@2:44bc61abdf33, 2015-06-10 (annotated)

Committer:
flywind
Date:
Wed Jun 10 12:56:48 2015 +0000
Revision:
2:44bc61abdf33
Parent:
0:f90c3452d779 
milestone working one , yaw pitch roll update in BLE sucessfull

Who changed what in which revision?

UserRevisionLine numberNew contents of line
flywind 0:f90c3452d779 1 #include "kalman.h"
flywind 0:f90c3452d779 2
flywind 0:f90c3452d779 3 // Setup the kalman data struct
flywind 0:f90c3452d779 4 void kalman_init(kalman_data *data)
flywind 0:f90c3452d779 5 {
flywind 0:f90c3452d779 6 data->x1 = 0.0f;
flywind 0:f90c3452d779 7 data->x2 = 0.0f;
flywind 0:f90c3452d779 8 data->x3 = 0.0f;
flywind 0:f90c3452d779 9
flywind 0:f90c3452d779 10 // Init P to diagonal matrix with large values since
flywind 0:f90c3452d779 11 // the initial state is not known
flywind 0:f90c3452d779 12 data->p11 = 1000.0f;
flywind 0:f90c3452d779 13 data->p12 = 0.0f;
flywind 0:f90c3452d779 14 data->p13 = 0.0f;
flywind 0:f90c3452d779 15 data->p21 = 0.0f;
flywind 0:f90c3452d779 16 data->p22 = 1000.0f;
flywind 0:f90c3452d779 17 data->p23 = 0.0f;
flywind 0:f90c3452d779 18 data->p31 = 0.0f;
flywind 0:f90c3452d779 19 data->p32 = 0.0f;
flywind 0:f90c3452d779 20 data->p33 = 1000.0f;
flywind 0:f90c3452d779 21
flywind 0:f90c3452d779 22 data->q1 = Q1;
flywind 0:f90c3452d779 23 data->q2 = Q2;
flywind 0:f90c3452d779 24 data->q3 = Q3;
flywind 0:f90c3452d779 25 data->r1 = R1;
flywind 0:f90c3452d779 26 data->r2 = R2;
flywind 0:f90c3452d779 27 }
flywind 0:f90c3452d779 28
flywind 0:f90c3452d779 29 void kalman_innovate(kalman_data *data, float z1, float z2)
flywind 0:f90c3452d779 30 {
flywind 0:f90c3452d779 31 float y1, y2;
flywind 0:f90c3452d779 32 float a, b, c;
flywind 0:f90c3452d779 33 float sDet;
flywind 0:f90c3452d779 34 float s11, s12, s21, s22;
flywind 0:f90c3452d779 35 float k11, k12, k21, k22, k31, k32;
flywind 0:f90c3452d779 36 float p11, p12, p13, p21, p22, p23, p31, p32, p33;
flywind 0:f90c3452d779 37
flywind 0:f90c3452d779 38 // Step 1
flywind 0:f90c3452d779 39 // x(k) = Fx(k-1) + Bu + w:
flywind 0:f90c3452d779 40 data->x1 = data->x1 + DT*data->x2 - DT*data->x3;
flywind 0:f90c3452d779 41 //x2 = x2;
flywind 0:f90c3452d779 42 //x3 = x3;
flywind 0:f90c3452d779 43
flywind 0:f90c3452d779 44 // Step 2
flywind 0:f90c3452d779 45 // P = FPF'+Q
flywind 0:f90c3452d779 46 a = data->p11 + data->p21*DT - data->p31*DT;
flywind 0:f90c3452d779 47 b = data->p12 + data->p22*DT - data->p32*DT;
flywind 0:f90c3452d779 48 c = data->p13 + data->p23*DT - data->p33*DT;
flywind 0:f90c3452d779 49 data->p11 = a + b*DT - c*DT + data->q1;
flywind 0:f90c3452d779 50 data->p12 = b;
flywind 0:f90c3452d779 51 data->p13 = c;
flywind 0:f90c3452d779 52 data->p21 = data->p21 + data->p22*DT - data->p23*DT;
flywind 0:f90c3452d779 53 data->p22 = data->p22 + data->q2;
flywind 0:f90c3452d779 54 //p23 = p23;
flywind 0:f90c3452d779 55 data->p31 = data->p31 + data->p32*DT - data->p33*DT;
flywind 0:f90c3452d779 56 //p32 = p32;
flywind 0:f90c3452d779 57 data->p33 = data->p33 + data->q3;
flywind 0:f90c3452d779 58
flywind 0:f90c3452d779 59 // Step 3
flywind 0:f90c3452d779 60 // y = z(k) - Hx(k)
flywind 0:f90c3452d779 61 y1 = z1-data->x1;
flywind 0:f90c3452d779 62 y2 = z2-data->x2;
flywind 0:f90c3452d779 63
flywind 0:f90c3452d779 64 // Step 4
flywind 0:f90c3452d779 65 // S = HPT' + R
flywind 0:f90c3452d779 66 s11 = data->p11 + data->r1;
flywind 0:f90c3452d779 67 s12 = data->p12;
flywind 0:f90c3452d779 68 s21 = data->p21;
flywind 0:f90c3452d779 69 s22 = data->p22 + data->r2;
flywind 0:f90c3452d779 70
flywind 0:f90c3452d779 71 // Step 5
flywind 0:f90c3452d779 72 // K = PH*inv(S)
flywind 0:f90c3452d779 73 sDet = 1/(s11*s22 - s12*s21);
flywind 0:f90c3452d779 74 k11 = (data->p11*s22 - data->p12*s21)*sDet;
flywind 0:f90c3452d779 75 k12 = (data->p12*s11 - data->p11*s12)*sDet;
flywind 0:f90c3452d779 76 k21 = (data->p21*s22 - data->p22*s21)*sDet;
flywind 0:f90c3452d779 77 k22 = (data->p22*s11 - data->p21*s12)*sDet;
flywind 0:f90c3452d779 78 k31 = (data->p31*s22 - data->p32*s21)*sDet;
flywind 0:f90c3452d779 79 k32 = (data->p32*s11 - data->p31*s12)*sDet;
flywind 0:f90c3452d779 80
flywind 0:f90c3452d779 81 // Step 6
flywind 0:f90c3452d779 82 // x = x + Ky
flywind 0:f90c3452d779 83 data->x1 = data->x1 + k11*y1 + k12*y2;
flywind 0:f90c3452d779 84 data->x2 = data->x2 + k21*y1 + k22*y2;
flywind 0:f90c3452d779 85 data->x3 = data->x3 + k31*y1 + k32*y2;
flywind 0:f90c3452d779 86
flywind 0:f90c3452d779 87 // Step 7
flywind 0:f90c3452d779 88 // P = (I-KH)P
flywind 0:f90c3452d779 89 p11 = data->p11*(1.0f - k11) - data->p21*k12;
flywind 0:f90c3452d779 90 p12 = data->p12*(1.0f - k11) - data->p22*k12;
flywind 0:f90c3452d779 91 p13 = data->p13*(1.0f - k11) - data->p23*k12;
flywind 0:f90c3452d779 92 p21 = data->p21*(1.0f - k22) - data->p11*k21;
flywind 0:f90c3452d779 93 p22 = data->p22*(1.0f - k22) - data->p12*k21;
flywind 0:f90c3452d779 94 p23 = data->p23*(1.0f - k22) - data->p13*k21;
flywind 0:f90c3452d779 95 p31 = data->p31 - data->p21*k32 - data->p11*k31;
flywind 0:f90c3452d779 96 p32 = data->p32 - data->p22*k32 - data->p12*k31;
flywind 0:f90c3452d779 97 p33 = data->p33 - data->p22*k32 - data->p13*k31;
flywind 0:f90c3452d779 98 data->p11 = p11; data->p12 = p12; data->p13 = p13;
flywind 0:f90c3452d779 99 data->p21 = p21; data->p22 = p22; data->p23 = p23;
flywind 0:f90c3452d779 100 data->p31 = p31; data->p32 = p32; data->p33 = p33;
flywind 0:f90c3452d779 101 }