贡献一个mpu6050的卡尔曼滤波程序
本帖最后由 salahading 于 2014-11-13 20:33 编辑/**********Kalman_Filter *********/
/* 系统状态变量*/
float x = {0,0};
/* 采样时间 */
float Ts = 0.001;
/*误差的协方差矩阵*/
float p = {{1,0},{0,1}};
/* Kalman增益 */
float K = {0,0};
/* 系统状态协方差矩阵对角元素 */
float Q0 = 0.00001,Q1 = 0.00003;
/* 系统输出 高斯噪声方差*/
float R = 0.5;
/*
Zk 倾角仪器或加速度计测量的系统角度Gyro 陀螺仪的角速度输出
x 系统倾角
x 陀螺仪漂移
*/
void Kalman_Filter(float Zk,float Gyro)
{
/*
X(k+1) = AX(k) + B*U
A = {{1,-Ts},{0,1}} B = {Ts,0}
Zk = Hx
Zk系统输出 H = {1 ,0}
计算系统状态估计值
*/
x = x + Ts*(Gyro - x);
x = x;
/*
误差的协方差估计
*/
p = p - Ts*(p+p) + Ts*Ts*p + Q0;
p = p - Ts*p;
p = p - Ts*p;
p = p + Q1;
/*
计算Kalman增益
*/
K = p/(p+R);
K = p/(p+R);
/*
校正先验协方差估计
*/
p = p - K*p;
p = p - K*p;
p = p - K*p;
p = p - K*p;
/*
校正系统状态估计值
*/
x = x + K*(Zk - x);
x = x + K*(Zk - x);
}
有同学说的他人的程序是不是下面的
void Kalman_Filter1(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; //输出值(后验估计)的微分=角速度
}
那这个是不是卡尔曼滤波呢？ 收下了 你应该讲解两句的 什么入口出口的 你确定这是Kalman滤波？ 看看…… Mark.谢谢楼主的分享！ 这是什么卡尔曼滤波？ cpumaker 发表于 2014-11-12 21:39
你确定这是Kalman滤波？
你确定不是吗 qs6361036 发表于 2014-11-12 22:15
这是什么卡尔曼滤波？
离散卡尔曼 有人验证过么？？效果怎么样？ salahading 发表于 2014-11-13 14:43
你确定不是吗
quite sure 像是Kalman filter的形式，但是估计的只是一个scaler而不会vector，没看懂在估计啥？ 之前看过别人写的。你这个应该不是! 单值滤波，算不上卡尔曼，卡尔曼实际上是融合过程，不是滤波过程 nnnkey 发表于 2014-11-13 17:23
单值滤波，算不上卡尔曼，卡尔曼实际上是融合过程，不是滤波过程
看一下第二段程序是不是？ tomy 发表于 2014-11-13 17:11
之前看过别人写的。你这个应该不是!
你说的是不是第二段程序呢？ cpumaker 发表于 2014-11-12 21:39
你确定这是Kalman滤波？
请问第二段程序是不是卡尔曼滤波呢？ cpumaker 发表于 2014-11-12 21:39
你确定这是Kalman滤波？
还请指教啊 tomy 发表于 2014-11-13 17:11
之前看过别人写的。你这个应该不是!
还请指教 salahading 发表于 2014-11-14 21:49
还请指教啊
第二段应该是，你可以假设一组数据然后加入高斯噪声，看看滤波后的效果 cpumaker 发表于 2014-11-14 22:13
第二段应该是，你可以假设一组数据然后加入高斯噪声，看看滤波后的效果 ...
刚看完电影，心情不错，就不刻薄了，只说一句，别不懂装懂 salahading 发表于 2014-11-15 01:27
刚看完电影，心情不错，就不刻薄了，只说一句，别不懂装懂
你的自信从何而来? salahading 发表于 2014-11-15 01:27
刚看完电影，心情不错，就不刻薄了，只说一句，别不懂装懂
第二段估计就是网上抄的一段mpu-6050程序 先收藏着，以后有时间研究下 salahading 发表于 2014-11-13 20:34
你说的是不是第二段程序呢？
卡尔曼滤波算法C语言实现(转2)
http://www.amobbs.com/thread-5571611-1-1.html
(出处: amoBBS 阿莫电子论坛)
这个帖子讲得满详细，有研究的可以参考看看。 salahading 发表于 2014-11-13 20:34
你说的是不是第二段程序呢？
卡尔曼滤波算法C语言实现(转2)
http://www.amobbs.com/thread-5571611-1-1.html
(出处: amoBBS 阿莫电子论坛)
这个帖子讲得满详细，有研究的可以参考看看。 论坛里发这个算法的人很多，你自己收看看，对比下。
别人纠正的不对，只是为你好，并不是想对你怎么样。 tomy 发表于 2014-11-15 12:36
卡尔曼滤波算法C语言实现(转2)
http://www.amobbs.com/thread-5571611-1-1.html
(出处: amoBBS 阿莫电子 ...
楼主写的那段代码是对的，他只是直接将别人写的二维卡尔曼滤波中的C_0=1了，更直观的展现了卡尔曼滤波的五个经典公式 nnnkey 发表于 2014-11-13 17:23
单值滤波，算不上卡尔曼，卡尔曼实际上是融合过程，不是滤波过程
卡尔曼实际是一个观测器
页:
[1]