Quadrocopter / 2015_04_17_quadro_bez_sterowania

bez sterowania

Dependencies:   FastPWM mbed-src

Fork of 2015_01_29_quadro2 by Quadrocopter

kalman.cpp@5:c3caf8b83e6b, 2014-12-19 (annotated)

Committer:
Michu90
Date:
Fri Dec 19 13:33:21 2014 +0000
Revision:
5:c3caf8b83e6b
a

Who changed what in which revision?

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