请教一下关于卡尔曼多维滤波的C语言程序

加油——蜗牛

下面是我看到别人写的多维卡尔曼滤波程序，实在是看不明白，请高手们详细的分享一下你们的调试经验和关于这段程序的解读。不胜感激
void Kalman_Filter_X(float Accel, float Gyro) //¿¨¶ûÂüº¯Êý
{
        Angle_X_Final += (Gyro - Q_bias_x) * dt; //ÏÈÑé¹À¼Æ

        Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏÈÑé¹À¼ÆÎó²îЭ·½²îµÄ΢·Ö

        Pdot[1] = -PP[1][1];
        Pdot[2] = -PP[1][1];
        Pdot[3] = Q_gyro;

        PP[0][0] += Pdot[0] * dt;   // Pk-ÏÈÑé¹À¼ÆÎó²îЭ·½²î΢·ÖµÄ»ý·Ö
        PP[0][1] += Pdot[1] * dt;   // =ÏÈÑé¹À¼ÆÎó²îЭ·½²î
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;

        Angle_err_x = Accel - Angle_X_Final;        //zk-ÏÈÑé¹À¼Æ

        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;                 //ºóÑé¹À¼ÆÎó²îЭ·½²î
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;

        Angle_X_Final += K_0 * Angle_err_x;         //ºóÑé¹À¼Æ
        Q_bias_x += K_1 * Angle_err_x;         //ºóÑé¹À¼Æ
        Gyro_x = Gyro - Q_bias_x;         //Êä³öÖµ(ºóÑé¹À¼Æ)µÄ΢·Ö=½ÇËÙ¶È
}

void Kalman_Filter_Y(float Accel, float Gyro) //¿¨¶ûÂüº¯Êý               
{
        Angle_Y_Final += (Gyro - Q_bias_y) * dt; //ÏÈÑé¹À¼Æ

        Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏÈÑé¹À¼ÆÎó²îЭ·½²îµÄ΢·Ö

        Pdot[1] = -PP[1][1];
        Pdot[2] = -PP[1][1];
        Pdot[3] = Q_gyro;

        PP[0][0] += Pdot[0] * dt;   // Pk-ÏÈÑé¹À¼ÆÎó²îЭ·½²î΢·ÖµÄ»ý·Ö
        PP[0][1] += Pdot[1] * dt;   // =ÏÈÑé¹À¼ÆÎó²îЭ·½²î
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;

        Angle_err_y = Accel - Angle_Y_Final;        //zk-ÏÈÑé¹À¼Æ

        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;                 //ºóÑé¹À¼ÆÎó²îЭ·½²î
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;

        Angle_Y_Final += K_0 * Angle_err_y;         //ºóÑé¹À¼Æ
        Q_bias_y += K_1 * Angle_err_y;         //ºóÑé¹À¼Æ
        Gyro_y = Gyro - Q_bias_y;         //Êä³öÖµ(ºóÑé¹À¼Æ)µÄ΢·Ö=½ÇËÙ¶È
}

void Kalman_Filter_Z(float Accel, float Gyro) //¿¨¶ûÂüº¯Êý
{
        Angle_Z_Final += (Gyro - Q_bias_z) * dt; //ÏÈÑé¹À¼Æ

        Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏÈÑé¹À¼ÆÎó²îЭ·½²îµÄ΢·Ö

        Pdot[1] = -PP[1][1];
        Pdot[2] = -PP[1][1];
        Pdot[3] = Q_gyro;

        PP[0][0] += Pdot[0] * dt;   // Pk-ÏÈÑé¹À¼ÆÎó²îЭ·½²î΢·ÖµÄ»ý·Ö
        PP[0][1] += Pdot[1] * dt;   // =ÏÈÑé¹À¼ÆÎó²îЭ·½²î
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;

        Angle_err_z = Accel - Angle_Z_Final;        //zk-ÏÈÑé¹À¼Æ

        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;                 //ºóÑé¹À¼ÆÎó²îЭ·½²î
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;

        Angle_Z_Final += K_0 * Angle_err_z;         //ºóÑé¹À¼Æ
        Q_bias_z += K_1 * Angle_err_z;         //ºóÑé¹À¼Æ
        Gyro_z = Gyro - Q_bias_z;         //Êä³öÖµ(ºóÑé¹À¼Æ)µÄ΢·Ö=½ÇËÙ¶È
}

xiaolaba

你这个执行还不够快.
要学习用就先做一个一维的实验一下, 琢磨体会原理和实际表现, 参考下面看看,
https://www.codeproject.com/articles/326657/kalmandemo
以前套垃圾级加速计用了一回, APP比原厂的改善很多, 不过Kalman gain初值设置还是有很大影响.
如果没有十分必要, 用快速的一阶RC滤波也差不多.

sbusr

先学卡尔曼滤波的原理，然后再看代码吧。不然越看越晕。

加油——蜗牛

xiaolaba 发表于 2017-3-14 22:09
你这个执行还不够快.
要学习用就先做一个一维的实验一下, 琢磨体会原理和实际表现, 参考下面看看,
https:// ...

一维的我会用，就是多维的搞不明白，能帮忙讲解一下吗？困扰我很多天了

加油——蜗牛

sbusr 发表于 2017-3-15 20:49
先学卡尔曼滤波的原理，然后再看代码吧。不然越看越晕。

嗯嗯  谢谢了

加油——蜗牛

xiaolaba 发表于 2017-3-14 22:09
你这个执行还不够快.
要学习用就先做一个一维的实验一下, 琢磨体会原理和实际表现, 参考下面看看,
https:// ...

你给的资料好给力啊，好好学习一下，谢谢了，还是外国人写的给力一些

fengyunyu

xiaolaba 发表于 2017-3-14 22:09
你这个执行还不够快.
要学习用就先做一个一维的实验一下, 琢磨体会原理和实际表现, 参考下面看看,
https:// ...

请教一阶RC滤波是否可以滤除基线的漂移，并捕捉突发尖峰信号？

ersha4877

学习下，也在考虑用什么好滤波程序

xiaolaba

fengyunyu 发表于 2017-3-16 06:05
请教一阶RC滤波是否可以滤除基线的漂移，并捕捉突发尖峰信号？

不太明白問題
估計這問號跟濾波的目的相反了

fengyunyu

xiaolaba 发表于 2017-3-16 09:03
不太明白問題
估計這問號跟濾波的目的相反了

比如心电信号

winterw

先标记一下，可能以后会用到，多谢各位

xiaolaba

fengyunyu 发表于 2017-3-16 15:28
比如心电信号

你的訊號做成圖視沒? 上個圖看看
玩具還是做產品的?

fengyunyu

xiaolaba 发表于 2017-3-17 12:05
你的訊號做成圖視沒? 上個圖看看
玩具還是做產品的?

wuquanhui2010

xiaolaba 发表于 2017-3-14 22:09
你这个执行还不够快.
要学习用就先做一个一维的实验一下, 琢磨体会原理和实际表现, 参考下面看看,
https:// ...

楼主网页链接进去程序下载不了啊，注册不了会员

xiaolaba

wuquanhui2010 发表于 2017-3-22 22:41
楼主网页链接进去程序下载不了啊，注册不了会员

email 給出來
幫你註冊一個
收到認證通知信後, 自己改密碼

xiaolaba

fengyunyu 发表于 2017-3-17 12:40

你都能做到這圖了, 不是完成了嗎
還要FILTER幹啥呢

jia_xuan

下到demo code的能共享一下吗，最近也在学习卡尔曼滤波。