谁懂卡尔曼滤波算法?由于参数太多,看不懂,求指教!
//******卡尔曼参数************float code Q_angle=0.001;
float code Q_gyro=0.003;
float code R_angle=0.5;
float code dt=0.01; //dt为kalman滤波器采样时间;
charcode 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 ={0,0,0,0};
float xdata PP = { { 1, 0 },{ 0, 1 } };
//*********************************************************
// 卡尔曼滤波
//*********************************************************
//Kalman滤波,20MHz的处理时间约0.77ms;
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot=Q_angle - PP - PP; // Pk-先验估计误差协方差的微分
Pdot=- PP;
Pdot=- PP;
Pdot=Q_gyro;
PP += Pdot * dt; // Pk-先验估计误差协方差微分的积分
PP += Pdot * dt; // =先验估计误差协方差
PP += Pdot * dt;
PP += Pdot * dt;
Angle_err = Accel - Angle; //zk-先验估计
PCt_0 = C_0 * PP;
PCt_1 = C_0 * PP;
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;
PP -= K_0 * t_0; //后验估计误差协方差
PP -= K_0 * t_1;
PP -= K_1 * t_0;
PP -= K_1 * t_1;
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
} 同样不理解的帮顶 帮顶~希望大神共享相关文档~~ 帮顶!带数学的,内心怕 帮顶,学习 帮顶,期待大神出来讲解 mark一下 mark..............
mark一下 暂时还没用到,不过论坛上好像有帖子详细讲解的,还有上位机辅助工具, http://www.amobbs.com/forum.php?mod=viewthread&tid=5571611&highlight=%E5%8D%A1%E5%B0%94%E6%9B%BC
http://www.amobbs.com/forum.php?mod=viewthread&tid=5593903&highlight=%E5%8D%A1%E5%B0%94%E6%9B%BC
这些先去看看呗 Hearthbeats 发表于 2014-10-7 11:10
暂时还没用到,不过论坛上好像有帖子详细讲解的,还有上位机辅助工具, http://www.amobbs.com/forum.php?m ...
谢谢,我去看看!
学习中。。。 。续上次问题,我这里有个比较好的资料,差不多转的。http://blog.csdn.net/lanbing510/article/details/8828109。 学习了...
页:
[1]