To all PRO Kalman's Filter!

Discussion in 'Programmer's Corner' started by leo123, Sep 7, 2010.

  1. leo123

    Thread Starter New Member

    Sep 7, 2010
    1
    0
    Im using a PIC 16F877A and MPLAB pro v7.61 compiler. Been working on my final year project which is a balancing robot. All the hardware and software seems to work fine but im having problems with the kalman filter. The LCD keeps displaying 0.00degrees. The kalman code that im using is from trammell hudson's 1DOF kalman filter. Help really needed as my project deadline is coming!

    Code ( (Unknown Language)):
    1.  
    2. #include <stdio.h>
    3. #include <htc.h>
    4. #include <math.h>
    5. #include "lcd.h"
    6. #include "delay.h"
    7.  
    8. __CONFIG(0x3f71);
    9.  
    10. static const float   dt   = 0.02;
    11.  
    12. static float      P[2][2] = {
    13.    { 1, 0 },
    14.    { 0, 1 },
    15. };
    16.  
    17. float          angle;
    18. float         q_bias;
    19. float         rate;
    20.  
    21. static const float   R_angle   = 0.3;
    22.  
    23. static const float   Q_angle   = 0.001;
    24. static const float   Q_gyro   = 0.003;
    25.  
    26.  
    27. void
    28. state_update(
    29.    float      q_m   /* Pitch gyro measurement */
    30. )
    31. {
    32.    float      q = q_m - q_bias;
    33.  
    34.    float      Pdot[2 * 2];
    35.  
    36.    Pdot[0] = Q_angle - P[0][1] - P[1][0];   /* 0,0 */
    37.    Pdot[1] = - P[1][1];      /* 0,1 */
    38.    Pdot[2] = - P[1][1];      /* 1,0 */
    39.    Pdot[3] = Q_gyro;            /* 1,1 */
    40.    
    41.    rate = q;
    42.    angle += q * dt;
    43.  
    44.    P[0][0] += Pdot[0] * dt;
    45.    P[0][1] += Pdot[1] * dt;
    46.    P[1][0] += Pdot[2] * dt;
    47.    P[1][1] += Pdot[3] * dt;
    48. }
    49.  
    50. void
    51. kalman_update(
    52.    float      ax_m,   /* X acceleration */
    53.    float      az_m    /* Z acceleration */
    54. )
    55. {
    56.    float      angle_m = atan2( -az_m, ax_m );
    57.    float      angle_err = angle_m - angle;
    58.  
    59.    const float      C_0 = 1;
    60.  
    61.    float      PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */
    62.    float      PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */
    63.      
    64.    float      E = R_angle + C_0 * PCt_0;
    65.  
    66.    float      K_0 = PCt_0 / E;
    67.    float      K_1 = PCt_1 / E;
    68.      
    69.    float      t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */
    70.    float      t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1]  = 0 */
    71.  
    72.    P[0][0] -= K_0 * t_0;
    73.    P[0][1] -= K_0 * t_1;
    74.    P[1][0] -= K_1 * t_0;
    75.    P[1][1] -= K_1 * t_1;
    76.    
    77.    angle   += K_0 * angle_err;
    78.    q_bias   += K_1 * angle_err;
    79. }
    80.  
    81. void main ( void )
    82. {
    83.     char LCDString[ 17 ];
    84.     unsigned int adcValue[7];
    85.    float accDegreeX[ 3 ],accDegreeY[ 3 ],gyroRateX[ 3 ];
    86.    unsigned char tmp;
    87.  
    88.     // Initializations
    89.     lcd_init();
    90.  
    91.     lcd_goto ( 0x00 );//Printing Kalman's Filter on the top LCD
    92.     lcd_puts ( "Kalman's Filter" );
    93.     lcd_goto ( 0x40 );//Printing Degree: on the bottom LCD
    94.     lcd_puts ( "Degree:" );
    95.  
    96.    PORTB = 0x00;
    97.  
    98.    TRISA = 0xff;
    99.    TRISD = 0x00;
    100.    ADCON1 = 0xc8;//Selecting type of ADC pins
    101.  
    102.     while ( 1 )
    103.     {
    104.         // Body
    105.       ADCON0 = 0x81;
    106.       ADGO = 1;
    107.       while ( ADGO )
    108.          continue;
    109.       adcValue [ 0 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA0(gyroRateX)
    110.  
    111.       ADCON0 = 0x89;
    112.       ADGO = 1;
    113.       while ( ADGO )
    114.          continue;
    115.       adcValue [ 1 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA1(gyroRateY)
    116.  
    117.       ADCON0 = 0xa1;
    118.       ADGO = 1;
    119.       while ( ADGO )
    120.          continue;
    121.       adcValue [ 2 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA5
    122.  
    123.       ADCON0 = 0xa9;
    124.       ADGO = 1;
    125.       while ( ADGO )
    126.          continue;
    127.       adcValue [ 3 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE0(accDegreeX)
    128.        
    129.       ADCON0 = 0xb1;
    130.       ADGO = 1;
    131.       while ( ADGO )
    132.          continue;
    133.       adcValue [ 4 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE1(accDegreeY)
    134.  
    135.       ADCON0 = 0xb9;
    136.       ADGO = 1;
    137.       while ( ADGO )
    138.          continue;
    139.       adcValue [ 5 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE2(accDegreeZ)
    140.  
    141.       gyroRateX[ 0 ] = (adcValue[ 0 ]*5.0)/1023;
    142.       gyroRateX[ 1 ] = (gyroRateX[ 0 ]-1.5)/0.002;//Basic calculation to get d/dt for gyroRateX
    143.  
    144.       accDegreeX[ 0 ] = (adcValue[ 3 ]*5.0)/1023;
    145.       accDegreeX[ 1 ] = (accDegreeX[ 0 ]-1.6)/0.3;//Basic calculation to get g for accDegreeX
    146.  
    147.       accDegreeY[ 0 ] = (adcValue[ 4 ]*5.0)/1023;
    148.       accDegreeY[ 1 ] = (accDegreeY[ 0 ]-1.6)/0.3;//Basic calculation to get g for accDegreeY
    149.  
    150.       state_update ( gyroRateX [ 1 ] );
    151.       kalman_update ( accDegreeX [ 1 ], accDegreeY [ 1 ] );
    152.  
    153.         lcd_goto ( 0x48 );
    154.         tmp = ( unsigned char ) angle;
    155.           angle = ( angle - tmp ) * 10;
    156.         lcd_putch ( tmp + '0' );
    157.         lcd_putch ( '.' );
    158.         tmp = ( unsigned char ) angle;
    159.         angle = ( angle - tmp ) * 10;
    160.         lcd_putch ( tmp + '0' );
    161.         tmp = ( unsigned char )    angle;
    162.         angle = (angle - tmp ) * 10;
    163.         lcd_putch ( tmp + '0' );
    164.         lcd_putch ( 0xdf );
    165.     }
    166. }
    167.  
    168.  
     
    Last edited by a moderator: Sep 7, 2010
  2. darenw5

    Active Member

    Feb 2, 2008
    45
    0
    What happens when you comment out most of the innards of kalman_update() to temporarily remove the filtering? Of course you need to keep the lines to do trig to get the angle, and the final lines, and hardcode values for K_0 and K_1. See if you can get the raw signal through from ADC to outputs. If so, or if not, you've narrowed down the problem to one place or another.
     
Loading...