超声波定高（基于STM32）

基于STM32的超声波定高代码

#include “kalman.h“


/**
  ******************************************************************************
  * @file    void Kalman_Filter_Init（KalmanCountData * Kalman_Struct）
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波计算中间量初始化
  *	
  *
  ******************************************************************************
  * @attention
  *
  * 
  * 
  *
  ******************************************************************************
  */

void Kalman_Filter_Init（KalmanCountData * Kalman_Struct）
{
	Kalman_Struct -> Angle_err		 = 0;
	Kalman_Struct -> Q_bias			 = 0;
	Kalman_Struct -> PCt_0			 = 0;
	Kalman_Struct -> PCt_1			 = 0;
	Kalman_Struct -> E				 = 0;
	Kalman_Struct -> K_0			 = 0;
	Kalman_Struct -> K_1			 = 0;
	Kalman_Struct -> t_0			 = 0;
	Kalman_Struct -> t_1			 = 0;
	Kalman_Struct -> Pdot[0]		 = 0;
	Kalman_Struct -> Pdot[1]		 = 0;
	Kalman_Struct -> Pdot[2]		 = 0;
	Kalman_Struct -> Pdot[3]		 = 0;	
	Kalman_Struct -> PP[0][0]		 = 1;
	Kalman_Struct -> PP[0][1]		 = 0;
	Kalman_Struct -> PP[1][0]		 = 0;
	Kalman_Struct -> PP[1][1]		 = 1;	
	Kalman_Struct -> Angle_Final	 = 0;
	Kalman_Struct -> Gyro_Final		 = 0;

}


/**
  ******************************************************************************
  * @file    void Kalman_Filter（float Accel	float Gyro KalmanCountData * Kalman_Struct）
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波计算
  *	
  *
  ******************************************************************************
  * @attention
  *		Accel:加速度计数据处理后进来的角度值
  *		Gyro :陀螺仪数据处理后进来的角速度值
  *		Kalman_Struct:递推运算所需要的中间变量，由用户定义为全局结构体变量
  *		Kalman_Struct -> Angle_Final  为滤波后角度最优值
  *		Kalman_Struct -> Gyro_Final   为后验角度值
  ******************************************************************************
  */

void Kalman_Filter（float Accel	float Gyro KalmanCountData * Kalman_Structfloat dt）
{
		//陀螺仪积分角度（先验估计）
		Kalman_Struct -> Angle_Final += （Gyro - Kalman_Struct -> Q_bias） * dt; 
		
		//先验估计误差协方差的微分
		Kalman_Struct -> Pdot[0] = Q_angle - Kalman_Struct -> PP[0][1] - Kalman_Struct -> PP[1][0]; 
		Kalman_Struct -> Pdot[1] = - Kalman_Struct -> PP[1][1];
		Kalman_Struct -> Pdot[2] = - Kalman_Struct -> PP[1][1];
		Kalman_Struct -> Pdot[3] = Q_gyro;
		
		//先验估计误差协方差的积分
		Kalman_Struct -> PP[0][0] += Kalman_Struct -> Pdot[0] * dt;   
		Kalman_Struct -> PP[0][1] += Kalman_Struct -> Pdot[1] * dt;   
		Kalman_Struct -> PP[1][0] += Kalman_Struct -> Pdot[2] * dt;
		Kalman_Struct -> PP[1][1] += Kalman_Struct -> Pdot[3] * dt;
		
		//计算角度偏差
		Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;	
		
		//卡尔曼增益计算
		Kalman_Struct -> PCt_0 = C_0 * Kalman_Struct -> PP[0][0];
		Kalman_Struct -> PCt_1 = C_0 * Kalman_Struct -> PP[1][0];
		
		Kalman_Struct -> E = R_angle + C_0 * Kalman_Struct -> PCt_0;
		
		Kalman_Struct -> K_0 = Kalman_Struct -> PCt_0 / Kalman_Struct -> E;
		Kalman_Struct ->

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      17273  2010-06-07 10:25  超声波定高（0731）\CORE\core_cm3.c

     文件      85714  2011-02-09 14:59  超声波定高（0731）\CORE\core_cm3.h

     文件      15503  2011-03-10 10:52  超声波定高（0731）\CORE\startup_stm32f10x_hd.s

     文件       2186  2017-07-31 09:53  超声波定高（0731）\HARDWARE\csb.c

     文件        265  2017-07-31 09:49  超声波定高（0731）\HARDWARE\csb.h

     文件       1500  2017-07-18 15:57  超声波定高（0731）\HARDWARE\deal.c

     文件        258  2017-07-18 15:51  超声波定高（0731）\HARDWARE\deal.h

     文件       1299  2015-03-23 22:25  超声波定高（0731）\HARDWARE\led.c

     文件        638  2015-03-23 22:26  超声波定高（0731）\HARDWARE\led.h

     文件        585  2017-07-18 10:59  超声波定高（0731）\HARDWARE\pxlb.c

     文件         89  2017-07-15 17:20  超声波定高（0731）\HARDWARE\pxlb.h

     文件       1623  2017-07-31 09:25  超声波定高（0731）\HARDWARE\timer.c

     文件        238  2017-07-14 20:32  超声波定高（0731）\HARDWARE\timer.h

     文件       3963  2015-08-08 14:55  超声波定高（0731）\kalman.c

     文件       1935  2015-08-12 16:47  超声波定高（0731）\kalman.h

     文件        399  2011-04-23 10:24  超声波定高（0731）\keilkilll.bat

     文件       3942  2017-07-18 16:27  超声波定高（0731）\OBJ\core_cm3.crf

     文件        104  2017-07-18 16:27  超声波定高（0731）\OBJ\core_cm3.d

     文件      11148  2017-07-18 16:27  超声波定高（0731）\OBJ\core_cm3.o

     文件     355256  2017-07-31 09:53  超声波定高（0731）\OBJ\csb.crf

     文件       1730  2017-07-31 09:53  超声波定高（0731）\OBJ\csb.d

     文件     393472  2017-07-31 09:53  超声波定高（0731）\OBJ\csb.o

     文件     356631  2017-07-18 15:57  超声波定高（0731）\OBJ\deal.crf

     文件       1788  2017-07-18 15:57  超声波定高（0731）\OBJ\deal.d

     文件     388476  2017-07-18 15:57  超声波定高（0731）\OBJ\deal.o

     文件     349955  2017-07-18 16:27  超声波定高（0731）\OBJ\delay.crf

     文件       1669  2017-07-18 16:27  超声波定高（0731）\OBJ\delay.d

     文件     385352  2017-07-18 16:27  超声波定高（0731）\OBJ\delay.o

     文件     349494  2017-07-18 16:27  超声波定高（0731）\OBJ\led.crf

     文件       1593  2017-07-18 16:27  超声波定高（0731）\OBJ\led.d

............此处省略184个文件信息