#include?
#include?“mpu6050.h“


//******卡爾曼濾波***********
float?xdata?Q_angle=0.001;//0.001??
float?xdata?Q_gyro=0.03;//0.03
float?xdata?R_angle=0.5;//0.5
float?xdata?dt=0.01;//0.1	??????????????????//dt為kalman濾波器采樣時(shí)間;
char??xdata?C_0?=?1;
float?xdata?Q_bias?Angle_err;
float?xdata?PCt_0?PCt_1?E;
float?xdata?K_0?K_1?t_0?t_1;
float?xdata?Pdot[4]?={0000};
float?xdata?PP[2][2]?=?{?{?1?0?}{?0?1?}?};
//float?Angle_EndGyro_End;			//小車最終傾斜角度，及角速度

/*卡爾曼函數(shù)*/
void?Kalman_Filter（float?Accelfloat?Gyro）		
{
	Angle_End?+=（Gyro?-?Q_bias）?*?dt;?//先驗(yàn)估計(jì)

	
	Pdot[0]=Q_angle?-?PP[0][1]?-?PP[1][0];?//?Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分

	Pdot[1]=-?PP[1][1];
	Pdot[2]=-?PP[1][1];
	Pdot[3]=Q_gyro;
	
	PP[0][0]?+=?Pdot[0]?*?dt;???//?Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
	PP[0][1]?+=?Pdot[1]?*?dt;???//?=先驗(yàn)估計(jì)誤差協(xié)方差
	PP[1][0]?+=?Pdot[2]?*?dt;
	PP[1][1]?+=?Pdot[3]?*?dt;
		
	Angle_err?=?Accel?-?Angle_End;	//zk-先驗(yàn)估計(jì)
	
	PCt_0?=?C_0?*?PP[0][0];
	PCt_1?=?C_0?*?PP[1][0];
	
	E?=?R_angle?+?C_0?*?PCt_0;
	
	K_0?=?PCt_0?/?E;
	K_1?=?PCt_1?/?E;
	
	t_0?=?PCt_0;
	t_1?=?C_0?*?PP[0][1];

	PP[0][0]?-=?K_0?*?t_0;		?//后驗(yàn)估計(jì)誤差協(xié)方差
	PP[0][1]?-=?K_0?*?t_1;
	PP[1][0]?-=?K_1?*?t_0;
	PP[1][1]?-=?K_1?*?t_1;
		
	Angle_End	+=?K_0?*?Angle_err;	?//后驗(yàn)估計(jì)
	Q_bias	+=?K_1?*?Angle_err;	?//后驗(yàn)估計(jì)
??Gyro_End?=?Gyro?-?Q_bias;	?//輸出值（后驗(yàn)估計(jì)）的微分=角速度

}

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2015-05-29?10:27??小車平衡穩(wěn)定\
?????文件????????1446??2015-04-28?10:29??小車平衡穩(wěn)定\Kalman.c
?????文件?????????103??2015-03-28?02:25??小車平衡穩(wěn)定\Kalman.h
?????目錄???????????0??2015-05-29?10:27??小車平衡穩(wěn)定\Listings\
?????文件????????3266??2015-04-28?10:30??小車平衡穩(wěn)定\Listings\Kalman.lst
?????文件????????2746??2015-04-28?10:32??小車平衡穩(wěn)定\Listings\Spe_Pos.lst
?????文件???????39337??2015-05-29?10:27??小車平衡穩(wěn)定\Listings\chengxu.m51
?????文件????????6183??2015-03-30?21:47??小車平衡穩(wěn)定\Listings\lcd1602.lst
?????文件????????5798??2015-05-29?10:27??小車平衡穩(wěn)定\Listings\main.lst
?????文件????????9081??2015-03-30?21:47??小車平衡穩(wěn)定\Listings\mpu6050.lst
?????文件????????1670??2015-04-28?10:31??小車平衡穩(wěn)定\Listings\pid_own.lst
?????文件????????4794??2015-04-28?10:30??小車平衡穩(wěn)定\Listings\pwm_motor.lst
?????目錄???????????0??2015-05-29?10:27??小車平衡穩(wěn)定\objects\
?????文件?????????122??2015-04-28?10:30??小車平衡穩(wěn)定\objects\Kalman.__i
?????文件???????17328??2015-04-28?10:30??小車平衡穩(wěn)定\objects\Kalman.obj
?????文件?????????125??2015-04-28?10:32??小車平衡穩(wěn)定\objects\Spe_Pos.__i
?????文件???????12596??2015-04-28?10:32??小車平衡穩(wěn)定\objects\Spe_Pos.obj
?????文件??????109213??2015-05-29?10:27??小車平衡穩(wěn)定\objects\chengxu
?????文件???????12145??2015-03-19?00:22??小車平衡穩(wěn)定\objects\chengxu.LIB
?????文件?????????138??2015-03-19?00:22??小車平衡穩(wěn)定\objects\chengxu.__b
?????文件?????????987??2015-05-29?10:27??小車平衡穩(wěn)定\objects\chengxu.build_log.htm
?????文件???????15843??2015-05-29?10:27??小車平衡穩(wěn)定\objects\chengxu.hex
?????文件?????????252??2015-05-29?10:27??小車平衡穩(wěn)定\objects\chengxu.lnp
?????文件???????12758??2015-03-30?21:47??小車平衡穩(wěn)定\objects\lcd1602.obj
?????文件???????24956??2015-05-29?10:27??小車平衡穩(wěn)定\objects\main.obj
?????文件???????22037??2015-03-30?21:47??小車平衡穩(wěn)定\objects\mpu6050.obj
?????文件?????????125??2015-04-28?10:31??小車平衡穩(wěn)定\objects\pid_own.__i
?????文件???????12096??2015-04-28?10:31??小車平衡穩(wěn)定\objects\pid_own.obj
?????文件?????????131??2015-04-28?10:30??小車平衡穩(wěn)定\objects\pwm_motor.__i
?????文件???????14441??2015-04-28?10:30??小車平衡穩(wěn)定\objects\pwm_motor.obj
?????文件???????46221??2010-07-27?17:31??小車平衡穩(wěn)定\STC12C5A.h
............此處省略15個(gè)文件信息