#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個文件信息