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!

#include <stdio.h>
#include <htc.h>
#include <math.h>
#include "lcd.h"
#include "delay.h"

__CONFIG(0x3f71);

static const float   dt   = 0.02;

static float      P[2][2] = {
   { 1, 0 },
   { 0, 1 },
};

float          angle;
float         q_bias;
float         rate;

static const float   R_angle   = 0.3;

static const float   Q_angle   = 0.001;
static const float   Q_gyro   = 0.003;


void
state_update(
   float      q_m   /* Pitch gyro measurement */
)
{
   float      q = q_m - q_bias;

   float      Pdot[2 * 2];

   Pdot[0] = Q_angle - P[0][1] - P[1][0];   /* 0,0 */
   Pdot[1] = - P[1][1];      /* 0,1 */
   Pdot[2] = - P[1][1];      /* 1,0 */
   Pdot[3] = Q_gyro;            /* 1,1 */
   
   rate = q;
   angle += q * dt;

   P[0][0] += Pdot[0] * dt;
   P[0][1] += Pdot[1] * dt;
   P[1][0] += Pdot[2] * dt;
   P[1][1] += Pdot[3] * dt;
}

void
kalman_update(
   float      ax_m,   /* X acceleration */
   float      az_m    /* Z acceleration */
)
{
   float      angle_m = atan2( -az_m, ax_m );
   float      angle_err = angle_m - angle;

   const float      C_0 = 1;

   float      PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */
   float      PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */
      
   float      E = R_angle + C_0 * PCt_0;

   float      K_0 = PCt_0 / E;
   float      K_1 = PCt_1 / E;
      
   float      t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */
   float      t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1]  = 0 */

   P[0][0] -= K_0 * t_0;
   P[0][1] -= K_0 * t_1;
   P[1][0] -= K_1 * t_0;
   P[1][1] -= K_1 * t_1;
   
   angle   += K_0 * angle_err;
   q_bias   += K_1 * angle_err;
}

void main ( void )
{
    char LCDString[ 17 ];
    unsigned int adcValue[7];
   float accDegreeX[ 3 ],accDegreeY[ 3 ],gyroRateX[ 3 ];
   unsigned char tmp;

    // Initializations
    lcd_init();

    lcd_goto ( 0x00 );//Printing Kalman's Filter on the top LCD
    lcd_puts ( "Kalman's Filter" );
    lcd_goto ( 0x40 );//Printing Degree: on the bottom LCD
    lcd_puts ( "Degree:" );

   PORTB = 0x00;

   TRISA = 0xff;
   TRISD = 0x00;
   ADCON1 = 0xc8;//Selecting type of ADC pins

    while ( 1 )
    {
        // Body
      ADCON0 = 0x81;
      ADGO = 1;
      while ( ADGO )
         continue;
      adcValue [ 0 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA0(gyroRateX)

      ADCON0 = 0x89;
      ADGO = 1;
      while ( ADGO )
         continue;
      adcValue [ 1 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA1(gyroRateY)

      ADCON0 = 0xa1;
      ADGO = 1;
      while ( ADGO )
         continue;
      adcValue [ 2 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RA5

      ADCON0 = 0xa9;
      ADGO = 1;
      while ( ADGO )
         continue;
      adcValue [ 3 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE0(accDegreeX)
        
      ADCON0 = 0xb1;
      ADGO = 1;
      while ( ADGO )
         continue;
      adcValue [ 4 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE1(accDegreeY)

      ADCON0 = 0xb9;
      ADGO = 1;
      while ( ADGO )
         continue;
      adcValue [ 5 ] = ( ADRESH << 8 ) | ADRESL;//Getting ADC from RE2(accDegreeZ)

      gyroRateX[ 0 ] = (adcValue[ 0 ]*5.0)/1023;
      gyroRateX[ 1 ] = (gyroRateX[ 0 ]-1.5)/0.002;//Basic calculation to get d/dt for gyroRateX

      accDegreeX[ 0 ] = (adcValue[ 3 ]*5.0)/1023;
      accDegreeX[ 1 ] = (accDegreeX[ 0 ]-1.6)/0.3;//Basic calculation to get g for accDegreeX

      accDegreeY[ 0 ] = (adcValue[ 4 ]*5.0)/1023;
      accDegreeY[ 1 ] = (accDegreeY[ 0 ]-1.6)/0.3;//Basic calculation to get g for accDegreeY

      state_update ( gyroRateX [ 1 ] );
      kalman_update ( accDegreeX [ 1 ], accDegreeY [ 1 ] );

        lcd_goto ( 0x48 );
        tmp = ( unsigned char ) angle;
          angle = ( angle - tmp ) * 10;
        lcd_putch ( tmp + '0' );
        lcd_putch ( '.' );
        tmp = ( unsigned char ) angle;
        angle = ( angle - tmp ) * 10;
        lcd_putch ( tmp + '0' );
        tmp = ( unsigned char )    angle;
        angle = (angle - tmp ) * 10;
        lcd_putch ( tmp + '0' );
        lcd_putch ( 0xdf );
    }
}

 

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.