kalman.c

00001 #include "kalman.h"
00002  
00003 // Setup the kalman data struct
00004 void kalman_init(kalman_data *data)
00005 {
00006     data->x1 = 0.0f;
00007     data->x2 = 0.0f;
00008     data->x3 = 0.0f;
00009  
00010     // Init P to diagonal matrix with large values since
00011     // the initial state is not known
00012     data->p11 = 1000.0f;
00013     data->p12 = 0.0f;
00014     data->p13 = 0.0f;
00015     data->p21 = 0.0f;
00016     data->p22 = 1000.0f;
00017     data->p23 = 0.0f;
00018     data->p31 = 0.0f;
00019     data->p32 = 0.0f;
00020     data->p33 = 1000.0f;
00021  
00022     data->q1 = Q1;
00023     data->q2 = Q2;
00024     data->q3 = Q3;
00025     data->r1 = R1;
00026     data->r2 = R2;
00027 }
00028  
00029 void kalman_innovate(kalman_data *data, float z1, float z2)
00030 {
00031     float y1, y2;
00032     float a, b, c;
00033     float sDet;
00034     float s11, s12, s21, s22;
00035     float k11, k12, k21, k22, k31, k32;
00036     float p11, p12, p13, p21, p22, p23, p31, p32, p33;
00037  
00038     // Step 1
00039     // x(k) = Fx(k-1) + Bu + w:
00040     data->x1 = data->x1 + DT*data->x2 - DT*data->x3;
00041     //x2 = x2;
00042     //x3 = x3;
00043  
00044     // Step 2
00045     // P = FPF'+Q
00046     a = data->p11 + data->p21*DT - data->p31*DT;
00047     b = data->p12 + data->p22*DT - data->p32*DT;
00048     c = data->p13 + data->p23*DT - data->p33*DT;
00049     data->p11 = a + b*DT - c*DT + data->q1;
00050     data->p12 = b;
00051     data->p13 = c;
00052     data->p21 = data->p21 + data->p22*DT - data->p23*DT;
00053     data->p22 = data->p22 + data->q2;
00054     //p23 = p23;
00055     data->p31 = data->p31 + data->p32*DT - data->p33*DT;
00056     //p32 = p32;
00057     data->p33 = data->p33 + data->q3;
00058  
00059     // Step 3
00060     // y = z(k) - Hx(k)
00061     y1 = z1-data->x1;
00062     y2 = z2-data->x2;
00063  
00064     // Step 4
00065     // S = HPT' + R
00066     s11 = data->p11 + data->r1;
00067     s12 = data->p12;
00068     s21 = data->p21;
00069     s22 = data->p22 + data->r2;
00070  
00071     // Step 5
00072     // K = PH*inv(S)
00073     sDet = 1/(s11*s22 - s12*s21);
00074     k11 = (data->p11*s22 - data->p12*s21)*sDet;
00075     k12 = (data->p12*s11 - data->p11*s12)*sDet;
00076     k21 = (data->p21*s22 - data->p22*s21)*sDet;
00077     k22 = (data->p22*s11 - data->p21*s12)*sDet;
00078     k31 = (data->p31*s22 - data->p32*s21)*sDet;
00079     k32 = (data->p32*s11 - data->p31*s12)*sDet;
00080  
00081     // Step 6
00082     // x = x + Ky
00083     data->x1 = data->x1 + k11*y1 + k12*y2;
00084     data->x2 = data->x2 + k21*y1 + k22*y2;
00085     data->x3 = data->x3 + k31*y1 + k32*y2;
00086  
00087     // Step 7
00088     // P = (I-KH)P
00089     p11 = data->p11*(1.0f - k11) - data->p21*k12;
00090     p12 = data->p12*(1.0f - k11) - data->p22*k12;
00091     p13 = data->p13*(1.0f - k11) - data->p23*k12;
00092     p21 = data->p21*(1.0f - k22) - data->p11*k21;
00093     p22 = data->p22*(1.0f - k22) - data->p12*k21;
00094     p23 = data->p23*(1.0f - k22) - data->p13*k21;
00095     p31 = data->p31 - data->p21*k32 - data->p11*k31;
00096     p32 = data->p32 - data->p22*k32 - data->p12*k31;
00097     p33 = data->p33 - data->p22*k32 - data->p13*k31;
00098     data->p11 = p11; data->p12 = p12; data->p13 = p13;
00099     data->p21 = p21; data->p22 = p22; data->p23 = p23;
00100     data->p31 = p31; data->p32 = p32; data->p33 = p33;
00101 }