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