金牌会员
- 积分
- 2603
- 威望
- 1282
- 贡献
- 807
- 兑换币
- 851
- 注册时间
- 2012-11-2
- 在线时间
- 257 小时
- 毕业学校
- 华电
|
8#
楼主 |
发表于 2013-10-5 20:35:13
|
只看该作者
glacier0 发表于 2013-10-5 19:51
你确定你这是卡尔曼滤波?
连代码都不愿意传上来谁知道你这是啥问题
额 没发过贴呀=。。=、
谢谢啦
用的是这种 输入角度(0~180)和 角加速度
float kf_again(float accelerometer_angle,float gyroscope_rate)
{
Q = 1,R = 6;
NowData = RealData + gyroscope_rate*0.005;
//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)
return RealData;
}
|
|