Banner publicitario de PCBWay

Dudas con Filtro Kalman

P

prometeo01091991

Hola compañeros del foro, como parte de mi tesis, necesito obtener los ángulos de un sensor giroscópico y uno acelerometrico, asi que buscando en la red encontré el MPU6050, y logre obtener la información :cool:.

El unico lio es que es un asco (n), varia mucho la medicion, por lo que buscando en la red, encontre inf. respecto al filtro Kalman. Pero seré sincero con ustedes, no le entiendo nada. Asi que busque mas y encontré un codigo, en ccs, para este sensor con el filtro :D... pero solo saca 2 medidas pitch y roll, y necesito sacar yaw. Por lo que necesito entenderlo para sacarlas.

Alguno de ustedes, me podria explicar de donde saca sus medidas?

libreria kalman
float Q_angle = 0.0004; //0.001 //0.005
float Q_gyro = 0.0002; //0.003 //0.0003
float R_angle = 0.002; //0.03 //0.008

y
float dt = (float)(looptime)/1000;


codigo del pic

set_ticks(0);
accXangle = (atan2 (accY, accZ) + PI) * RAD_TO_DEG;
DOUBLE gyroXrate = (double) gyroX / 131.0;
gyroXangle += gyroXrate * ( (DOUBLE) (get_ticks () - timer) / 1000);
kalAngleX = kalmanCalculate (accXangle, gyroXrate, (get_ticks()-timer));
timer = get_ticks ();
//timer=0;
set_ticks(0);
accYangle = (atan2 (accX, accZ) + PI) * RAD_TO_DEG;
DOUBLE gyroYrate = (double) gyroY / 131.0;
gyroYangle += gyroYrate * ( (DOUBLE) (get_ticks () - timer) / 1000);
kalAngleY = kalmanCalculate (accYangle, gyroYrate, (get_ticks()-timer));
timer = get_ticks ();

el codigo es el siguiente:

En el PIC

Código: 
 #include <18F4550.h>
 #fuses HSPLL,NOWDT,NOPROTECT,NOLVP,NODEBUG,USBDIV,PLL5,CPUDIV1,VREGEN
 #use delay(clock=48000000)

//#use i2c(master, sda=PIN_C4, scl=PIN_C3,FORCE_HW)
//#use i2c(master, sda=PIN_E0, scl=PIN_E1)  
#define MPU_SDA PIN_B0                               
#define MPU_SCL PIN_B1                         
#use I2C(master, sda=MPU_SDA, scl=MPU_SCL)  
#use TIMER(TIMER=1, TICK=1ms, BITS=16, NOISR)       
#define LCD_RS_PIN PIN_D0                                                    
#define LCD_RW_PIN PIN_D1         
#define LCD_ENABLE_PIN PIN_D2                           
#define LCD_DATA4 PIN_D4                                                          
#define LCD_DATA5 PIN_D5                                    
#define LCD_DATA6 PIN_D6                                                                            
#define LCD_DATA7 PIN_D7                                    
#include <lcd.c>  
#include "MPU6050.c"               
#include "math.h"
#include "Kalman.h"
#define RAD_TO_DEG 180/PI  

typedef struct
{
 struct
 {
  signed int16 X;
  signed int16 Y;
  signed int16 Z;
 }Accel;
 signed int16 Temperatura;
 struct
 {
  signed int16 X;
  signed int16 Y;
  signed int16 Z;
 }Gyro;
}MPU6050;  

void MPU6050_Test( MPU6050 *Sensor )
{
 MPU6050_I2C_Start();
 MPU6050_I2C_Wr( MPU6050_ADDRESS );
 MPU6050_I2C_Wr( MPU6050_RA_ACCEL_XOUT_H );
 MPU6050_I2C_Start();                          
 MPU6050_I2C_Wr( MPU6050_ADDRESS | 1 );
 Sensor->Accel.X = ((MPU6050_I2C_Rd(1))<<8 ) | MPU6050_I2C_Rd(1);              
 MPU6050_I2C_Stop();                                                 
                        
}

MPU6050 Sensor;      
                                                
void main()
{
   SIGNED int16 accX,accY, accZ;
   SIGNED int16 gyroX,gyroY,gyroZ;
   DOUBLE accXangle,accYangle; 
   DOUBLE gyroXangle,gyroYangle;
   DOUBLE kalAngleX,kalAngleY;            
   UNSIGNED int16 timer;
   lcd_init ();
   printf (LCD_PUTC, "\f MuaLinhKien.Vn\nPIC 16/18 Basic Kit");
   delay_ms (1000);
   printf (LCD_PUTC, "\f");
   Mpu6050_Init () ;
   delay_ms (500) ;
   INT8 x;
   x = Mpu6050_Read(MPU6050_RA_WHO_AM_I);
                                                       
   IF (x != 0x68)
   {
      LCD_Gotoxy (2, 0) ;
      printf (LCD_PUTC, "Connection ERR!!!");
      return;
   }                    

   
   WHILE (true)
   {                                                               
      accX = Mpu6050_GetData (MPU6050_RA_ACCEL_XOUT_H);  
      accY = Mpu6050_GetData (MPU6050_RA_ACCEL_YOUT_H);
      accZ = Mpu6050_GetData (MPU6050_RA_ACCEL_ZOUT_H);  
      gyroX = Mpu6050_GetData(MPU6050_RA_GYRO_XOUT_H); 
      gyroY = Mpu6050_GetData(MPU6050_RA_GYRO_YOUT_H);
      gyroZ = Mpu6050_GetData(MPU6050_RA_GYRO_ZOUT_H);
//!      lcd_gotoxy(1,1);                  
//!      printf(LCD_PUTC,"G:X%5lu",gyroX);         
//!      lcd_gotoxy(10,1);
//!      printf(LCD_PUTC,"Y%5lu",gyroY); 
//!      lcd_gotoxy(1,2);                                               
//!      printf(LCD_PUTC,"Z%5lu",gyroZ);             
//!      delay_ms(100);                     
                                                
//!      lcd_gotoxy(1,1);                  
//!      printf(LCD_PUTC,"A:X%5lu",accX);                     
//!      lcd_gotoxy(10,1);                    
//!      printf(LCD_PUTC,"Y%5lu",accY);  
//!      lcd_gotoxy(1,2);                      
//!      printf(LCD_PUTC,"Z%5lu",accZ);                     
//!      Delay_ms( 100 );
      
      set_ticks(0);   
      accXangle = (atan2 (accY, accZ) + PI) * RAD_TO_DEG; 
      DOUBLE gyroXrate = (double) gyroX / 131.0; 
      gyroXangle += gyroXrate * ( (DOUBLE) (get_ticks () - timer)  / 1000);  
      kalAngleX = kalmanCalculate (accXangle, gyroXrate, (get_ticks()-timer)); 
      timer = get_ticks ();
      //timer=0; 
      set_ticks(0);                    
      accYangle = (atan2 (accX, accZ) + PI) * RAD_TO_DEG;
      DOUBLE gyroYrate = (double) gyroY / 131.0;                                                   
      gyroYangle += gyroYrate * ( (DOUBLE) (get_ticks () - timer)  / 1000);
      kalAngleY = kalmanCalculate (accYangle, gyroYrate, (get_ticks()-timer));
      timer = get_ticks ();
       
      LCD_Gotoxy (1, 1) ;                                
      printf (LCD_PUTC, "X=%f", kalAngleX);
      
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "       " ); 
      if(kalAngleX>200)
      { 
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "back   " );   
      }                                    
      if((150<kalAngleX)&&(kalAngleX<170))          
      { 
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "forward" );   
      } 
      if(kalAngleX<150)        
      {                            
      LCD_Gotoxy (10, 1) ;
      printf (LCD_PUTC, "nitro  " );
      }
                                         
       LCD_Gotoxy (1, 2) ;                
      printf (LCD_PUTC, "Y=%f", kalAngleY); 
       LCD_Gotoxy (10, 2) ; 
      printf (LCD_PUTC, "       " ); 
      if(kalAngleY>200)
      { 
      LCD_Gotoxy (10, 2) ;       
      printf (LCD_PUTC, "left  " );  
      } 
      if(kalAngleY<160)                          
      { 
      LCD_Gotoxy (10, 2) ; 
      printf (LCD_PUTC, "right " );
      }
      delay_ms(100);
     }                 
}

Libreria del I2C:

Código: 
#define MPU6050_I2C_Wr(value) i2c_write(value) 
#define MPU6050_I2C_Rd(value) i2c_read(value) 
#define MPU6050_I2C_Stop() i2c_stop() 
#define MPU6050_I2C_Start() i2c_start()        


//
#define MPU6050_ADDRESS             0xD0
#define MPU6050_RA_XG_OFFS_TC       0x00 
#define MPU6050_RA_YG_OFFS_TC       0x01 
#define MPU6050_RA_ZG_OFFS_TC       0x02 
#define MPU6050_RA_X_FINE_GAIN      0x03 
#define MPU6050_RA_Y_FINE_GAIN      0x04 
#define MPU6050_RA_Z_FINE_GAIN      0x05 
#define MPU6050_RA_XA_OFFS_H        0x06 
#define MPU6050_RA_XA_OFFS_L_TC     0x07
#define MPU6050_RA_YA_OFFS_H        0x08 
#define MPU6050_RA_YA_OFFS_L_TC     0x09
#define MPU6050_RA_ZA_OFFS_H        0x0A 
#define MPU6050_RA_ZA_OFFS_L_TC     0x0B
#define MPU6050_RA_XG_OFFS_USRH     0x13 
#define MPU6050_RA_XG_OFFS_USRL     0x14
#define MPU6050_RA_YG_OFFS_USRH     0x15 
#define MPU6050_RA_YG_OFFS_USRL     0x16
#define MPU6050_RA_ZG_OFFS_USRH     0x17 
#define MPU6050_RA_ZG_OFFS_USRL     0x18
#define MPU6050_RA_SMPLRT_DIV       0x19
#define MPU6050_RA_CONFIG           0x1A
#define MPU6050_RA_GYRO_CONFIG      0x1B
#define MPU6050_RA_ACCEL_CONFIG     0x1C
#define MPU6050_RA_FF_THR           0x1D
#define MPU6050_RA_FF_DUR           0x1E
#define MPU6050_RA_MOT_THR          0x1F
#define MPU6050_RA_MOT_DUR          0x20
#define MPU6050_RA_ZRMOT_THR        0x21
#define MPU6050_RA_ZRMOT_DUR        0x22
#define MPU6050_RA_FIFO_EN          0x23
#define MPU6050_RA_I2C_MST_CTRL     0x24
#define MPU6050_RA_I2C_SLV0_ADDR    0x25
#define MPU6050_RA_I2C_SLV0_REG     0x26
#define MPU6050_RA_I2C_SLV0_CTRL    0x27
#define MPU6050_RA_I2C_SLV1_ADDR    0x28
#define MPU6050_RA_I2C_SLV1_REG     0x29
#define MPU6050_RA_I2C_SLV1_CTRL    0x2A
#define MPU6050_RA_I2C_SLV2_ADDR    0x2B
#define MPU6050_RA_I2C_SLV2_REG     0x2C
#define MPU6050_RA_I2C_SLV2_CTRL    0x2D
#define MPU6050_RA_I2C_SLV3_ADDR    0x2E
#define MPU6050_RA_I2C_SLV3_REG     0x2F
#define MPU6050_RA_I2C_SLV3_CTRL    0x30
#define MPU6050_RA_I2C_SLV4_ADDR    0x31
#define MPU6050_RA_I2C_SLV4_REG     0x32
#define MPU6050_RA_I2C_SLV4_DO      0x33
#define MPU6050_RA_I2C_SLV4_CTRL    0x34
#define MPU6050_RA_I2C_SLV4_DI      0x35
#define MPU6050_RA_I2C_MST_STATUS   0x36
#define MPU6050_RA_INT_PIN_CFG      0x37
#define MPU6050_RA_INT_ENABLE       0x38
#define MPU6050_RA_DMP_INT_STATUS   0x39
#define MPU6050_RA_INT_STATUS       0x3A
#define MPU6050_RA_ACCEL_XOUT_H     0x3B
#define MPU6050_RA_ACCEL_XOUT_L     0x3C
#define MPU6050_RA_ACCEL_YOUT_H     0x3D
#define MPU6050_RA_ACCEL_YOUT_L     0x3E
#define MPU6050_RA_ACCEL_ZOUT_H     0x3F
#define MPU6050_RA_ACCEL_ZOUT_L     0x40
#define MPU6050_RA_TEMP_OUT_H       0x41
#define MPU6050_RA_TEMP_OUT_L       0x42
#define MPU6050_RA_GYRO_XOUT_H      0x43
#define MPU6050_RA_GYRO_XOUT_L      0x44
#define MPU6050_RA_GYRO_YOUT_H      0x45
#define MPU6050_RA_GYRO_YOUT_L      0x46
#define MPU6050_RA_GYRO_ZOUT_H      0x47
#define MPU6050_RA_GYRO_ZOUT_L      0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
#define MPU6050_RA_MOT_DETECT_STATUS    0x61
#define MPU6050_RA_I2C_SLV0_DO      0x63
#define MPU6050_RA_I2C_SLV1_DO      0x64
#define MPU6050_RA_I2C_SLV2_DO      0x65
#define MPU6050_RA_I2C_SLV3_DO      0x66
#define MPU6050_RA_I2C_MST_DELAY_CTRL   0x67
#define MPU6050_RA_SIGNAL_PATH_RESET    0x68
#define MPU6050_RA_MOT_DETECT_CTRL      0x69
#define MPU6050_RA_USER_CTRL        0x6A
#define MPU6050_RA_PWR_MGMT_1       0x6B
#define MPU6050_RA_PWR_MGMT_2       0x6C
#define MPU6050_RA_BANK_SEL         0x6D
#define MPU6050_RA_MEM_START_ADDR   0x6E
#define MPU6050_RA_MEM_R_W          0x6F
#define MPU6050_RA_DMP_CFG_1        0x70
#define MPU6050_RA_DMP_CFG_2        0x71
#define MPU6050_RA_FIFO_COUNTH      0x72
#define MPU6050_RA_FIFO_COUNTL      0x73
#define MPU6050_RA_FIFO_R_W         0x74
#define MPU6050_RA_WHO_AM_I         0x75



unsigned char MPU6050_Read(unsigned char address)
   {
   int8 Data;     
   MPU6050_I2C_Start();           
   MPU6050_I2C_Wr( MPU6050_ADDRESS );      
   MPU6050_I2C_Wr(address);    
   MPU6050_I2C_Start();          
   MPU6050_I2C_Wr( MPU6050_ADDRESS | 1 );  
   Data=MPU6050_I2C_Rd(0);        
    MPU6050_I2C_Stop();              
   return Data;   
   }  
void Mpu6050_Write(unsigned char address,unsigned char Data)
   {
   MPU6050_I2C_Start();               
   MPU6050_I2C_Wr( MPU6050_ADDRESS );    
   MPU6050_I2C_Wr( address);   
   MPU6050_I2C_Wr( Data);       
   MPU6050_I2C_Stop();     
   }

int16 Mpu6050_GetData(unsigned char address)
{
   int16 H=0,L=0;    
   MPU6050_I2C_Start();       
   MPU6050_I2C_Wr( MPU6050_ADDRESS );      
   MPU6050_I2C_Wr(address);    
   MPU6050_I2C_Start();         
   MPU6050_I2C_Wr( MPU6050_ADDRESS |1);   
   H=i2C_read();           
   L=i2C_read(0);         
   MPU6050_I2C_Stop();          
   return (H<<8)|L;
}                                              
   





void Mpu6050_Init()
   {

   Mpu6050_Write(MPU6050_RA_PWR_MGMT_1,0X80); 
   delay_ms(5);
   Mpu6050_Write(MPU6050_RA_PWR_MGMT_1, 0x00); 
   Mpu6050_Write(MPU6050_RA_SMPLRT_DIV 0x07);  //Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
   Mpu6050_Write(MPU6050_RA_CONFIG, 0x00);      
                                      
   Mpu6050_Write(MPU6050_RA_GYRO_CONFIG, 0x00); 
                                      
   Mpu6050_Write(MPU6050_RA_ACCEL_CONFIG, 0x00); 
                                     
   Mpu6050_Write(MPU6050_RA_USER_CTRL, 0x00);   
   Mpu6050_Write(MPU6050_RA_PWR_MGMT_1, 0x01);                         
   delay_ms(10);
   }

Libreria del Kalman:

Código: 
#ifndef _Kalman_h
#define _Kalman_h

    float Q_angle  =  0.0004; //0.001    //0.005
    float Q_gyro   =  0.0002;  //0.003  //0.0003
    float R_angle  =  0.002;  //0.03     //0.008

    float x_bias = 0;
    float x_angle;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float  y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate, int16 looptime)
  {
    float dt = (float)(looptime)/1000;
    x_angle += dt * (newRate - x_bias);

//!    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_00 += dt * (dt*P_11 - P_01 - P_10 + Q_angle);
//!    P_01 +=  - dt * P_11;
    P_01 -= dt * P_11;    
//!    P_10 +=  - dt * P_11;
    P_10 -= dt * P_11;
//!    P_11 +=  + Q_gyro * dt;
    P_11 += Q_gyro * dt;

    S = P_00 + R_angle;
    
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    
    y = newAngle - x_angle;
    
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;

    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    
    return x_angle;
  }

#endif
 
Hola,

Lamento no poder ayudarte en tu duda,pues yo también recién estoy iniciándome en lo que es filtro de kalman y se ha convertido en mi pesadilla jajaja.

Lo que si te agradecería ahora que ya supongo tienes mas experiencia me puedas ayudar, para entender el funcionamiento de este filtro así como el código que aquí pusiste.

En el código en la parte:

gyroYangle += gyroYrate * ( (DOUBLE) (get_ticks () - timer) / 1000);
kalAngleY = kalmanCalculate (accYangle, gyroYrate, (get_ticks()-timer));

No entiendo porque calcula el angulo con el gyroscopio si al final no lo usa como entrada para la función Kalman.

Agradecido de antemano por tu respuesta.
 

Temas similares

yon123
Datos_teclado_pic18f887A
Respuestas
5
Visitas
2K
yon123
yon123
Alexisbjj
Como hacer un menu en pic con un teclado matricial?
Respuestas
1
Visitas
2K
Lord Chango
Lord Chango
K
Programación del PIC 24FJ en pcb real con CCS.
Respuestas
2
Visitas
2K
D@rkbytes
D@rkbytes
marmoleda
Midiendo Temperatura con max6675 y pic16f887 pulsos desincronizados
Respuestas
5
Visitas
3K
marmoleda
marmoleda
S
Interrupcion externa y conversion ADC no me funcionan juntos CCS
Respuestas
10
Visitas
2K
J2C
J2C
Atrás
Arriba