kem.c

卡尔曼滤波（非矩阵）参考程序

#include "main.h"

volatile float QingJiao = 0;
volatile float Gyro_Data = 0;

void kalman_update(void)
{
	 
	  float Q =1,R = 300;
	  static float RealData = 0,RealData_P = 0;
	  float NowData = 0,NowData_P = 0;
	  float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0;
	  float Acc_x = 0,Acc_z = 0, Gyro = 0;
	  static float his_acc = 0.0,his_accx = 0.0,his_accz = 0.0;
	  
	  while(!ATD0STAT0_SCF);   //等待转换完成
        
   	Acc_x = (float)ATD0DR1;
   	Acc_z = (float)ATD0DR2;
   	Gyro  = (float)ATD0DR0;
   	
	  if(Gyro > 4090) Gyro += 1000;
	  else
	  if(Gyro > 4084) Gyro += 500;
	  
	  if(Gyro < 55)   Gyro -= 1000;
  	else
  	if(Gyro < 60)   Gyro -= 500;
      	
   	Acc_x = Acc_x - 2342.0;
   	Acc_z = Acc_z - 2076.0;
   	Gyro  = Gyro  - 2048.0;
    Gyro_Data = Gyro;
    
    OutData[0] = Gyro_Data;

    accelerometer_angle = atan2f(-Acc_x,Acc_z);
    OutData[1] = accelerometer_angle*1000;
 
    gyroscope_rate = Gyro*0.0023;         //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.
    
                                          //(3300/4096)/(0.67*9.1)*(3.14/180) =  0.0023
    
    NowData = RealData + gyroscope_rate*0.01;                 //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
    NowData_P = sqrt(Q*Q+RealData_P*RealData_P);              //2.计算预估计协方差矩阵   P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
    Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R)); //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
    RealData = NowData + Kg*(accelerometer_angle - NowData);  //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
    RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);            //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)
    
    QingJiao =  RealData;  
    
    
    OutData[2] = QingJiao*1000; 
    OutPut_Data();
 
	
}