- 1、本文档共9页,可阅读全部内容。
- 2、原创力文档(book118)网站文档一经付费(服务费),不意味着购买了该文档的版权,仅供个人/单位学习、研究之用,不得用于商业用途,未经授权,严禁复制、发行、汇编、翻译或者网络传播等,侵权必究。
- 3、本站所有内容均由合作方或网友上传,本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺!文档内容仅供研究参考,付费前请自行鉴别。如您付费,意味着您自己接受本站规则且自行承担风险,本站不退款、不进行额外附加服务;查看《如何避免下载的几个坑》。如果您已付费下载过本站文档,您可以点击 这里二次下载。
- 4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等,请点击“版权申诉”(推荐),也可以打举报电话:400-050-0827(电话支持时间:9:00-18:30)。
一阶互补
//a=tau/(tau+looptime)
//newAngle=anglemeasuredwithatan2usingtheaccelerometer
//加速度传感器输出值
//newRate=anglemeasuredusingthegyro
//looptime=looptimeinmillis()
floattau=0.075;
floata=0.0;
floatComplementary(floatnewAngle,floatnewRate,intlooptime)
{
floatdtC=float(looptime)/1000.0;
a=tau/(tau+dtC);
//下行代码更改成白色,下载后恢复成其他颜色即可看到
x_angleC=;
returnx_angleC;
}
二阶互补
//newAngle=anglemeasuredwithatan2usingtheaccelerometer
//newRate=anglemeasuredusingthegyro
//looptime=looptimeinmillis()
floatComplementary2(floatnewAngle,floatnewRate,intlooptime)
{
floatk=10;
floatdtc2=float(looptime)/1000.0;
x1=(newAngle-x_angle2C)*k*k;
y1=dtc2*x1+y1;
x2=;//此行代码更改成白色,下载后恢复成其他颜色即可看到
x_angle2C=dtc2*x2+x_angle2C;
returnx_angle2C;
}Heretoowejusthavetosetthe?kand?magically?weget?theangle.
卡尔曼滤波
//KasBotV1-Kalmanfiltermodule
float
float
float
floatx_bias=0;
floatP_00=0,P_01=0,P_10=0,P_11=0;
floaty,S;
floatK_0,K_1;
//newAngle=anglemeasuredwithatan2usingtheaccelerometer
//newRate=anglemeasuredusingthegyro
//looptime=looptimeinmillis()
floatkalmanCalculate(floatnewAngle,floatnewRate,intlooptime)
{
floatdt=float(looptime)/1000;
x_angle+=dt*(newRate-x_bias);
//以下代码更改成白色,下载后恢复成其他颜色即可看到
P_00+=;
P_01+=;
P_10+=;
P_11+=;
y=newAngle-x_angle;
S=P_00+R_angle;
K_0=P_00/S;
K_1=P_10/S;
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;
returnx_angle;
}To?gettheanswer,?youhavetoset?3parameters:?Q_angle,?R_angle,R_gyro.
详细卡尔曼滤波
/*-*-indent-tabs-mode:T;c-basic-offset:8;tab-width:8;-*-vi:setts=8:
*$Id:tilt.c,v1.12003/07/0918:23:29johnExp$
*
*1dimensionaltiltsensorusingadualaxisaccelerometer
*andsingleaxisangularrategyro.Thetwosensorsarefused
*viaatwostateKalmanfilter,withonestatebeingtheangle
*andthe
文档评论(0)