打印
[应用相关]

卡尔曼滤波C代码分析

[复制链接]
2765|18
手机看帖
扫描二维码
随时随地手机跟帖
跳转到指定楼层
楼主
cztqwa|  楼主 | 2015-11-28 16:40 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
本帖最后由 cztqwa 于 2015-11-28 16:40 编辑










沙发
mmuuss586| | 2015-11-28 17:09 | 只看该作者

为啥不发PDF资料呢

使用特权

评论回复
板凳
cztqwa|  楼主 | 2015-11-28 21:29 | 只看该作者
mmuuss586 发表于 2015-11-28 17:09
为啥不发PDF资料呢

第一次发帖,还不太会弄。

使用特权

评论回复
地板
cztqwa|  楼主 | 2015-11-28 21:33 | 只看该作者
mmuuss586 发表于 2015-11-28 17:09
为啥不发PDF资料呢

好像是权限太低了,以后补上。

使用特权

评论回复
5
奔牛滚滚| | 2015-11-28 23:41 | 只看该作者
请发pdf资料,权限太低,发到百度网盘然后给个链接也可以啊,发到百度文库也可以赚积分,发到论坛的资料库更好,资料库下载也是要积分的

使用特权

评论回复
6
MJM_WSY| | 2015-11-29 16:45 | 只看该作者
求PDF文档。还有代码。   这个可以用来稳定电子称的数值不?

使用特权

评论回复
7
cztqwa|  楼主 | 2015-11-29 20:21 | 只看该作者
奔牛滚滚 发表于 2015-11-28 23:41
请发pdf资料,权限太低,发到百度网盘然后给个链接也可以啊,发到百度文库也可以赚积分,发到论坛的资料库更好, ...

我连发链接的权限都木有,您可以去百度文库搜索,已上传。

使用特权

评论回复
8
cztqwa|  楼主 | 2015-11-29 20:25 | 只看该作者
MJM_WSY 发表于 2015-11-29 16:45
求PDF文档。还有代码。   这个可以用来稳定电子称的数值不?

不怎么了解电子称的原理。

使用特权

评论回复
9
cztqwa|  楼主 | 2015-11-29 20:45 | 只看该作者
MJM_WSY 发表于 2015-11-29 16:45
求PDF文档。还有代码。   这个可以用来稳定电子称的数值不?
float Angle=0.0;                //卡尔曼滤波器的输出值,最优估计的角度
//float Gyro_x=0.0;                //卡尔曼滤波器的输出值,最优估计的角速度
float Q_angle=0.001;        //陀螺仪噪声的协方差(估计过程的误差协方差)
float Q_gyro=0.003;                //陀螺仪漂移噪声的协方差(估计过程的误差协方差)
float R_angle=0.5;                //加速度计测量噪声的协方差
float dt=0.005;                //积分时间,dt为滤波器采样时间(秒)
char C_0 = 1;                //H矩阵的一个数
float Q_bias=0, Angle_err=0;        //Q_bias为陀螺仪漂移
float PCt_0=0, PCt_1=0, E=0;        //中间变量
float K_0=0, K_1=0, t_0=0, t_1=0;        //K是卡尔曼增益,t是中间变量
float Pdot[4] ={0,0,0,0};        //计算P矩阵的中间变量
float PP[2][2] = { { 1, 0 },{ 0, 1 } };                //公式中P矩阵,X的协方差

void Kalman_Filter(float Gyro,float Accel)//Gyro陀螺仪的测量值,Accel加速度计的角度计算值
{   
    Angle += (Gyro - Q_bias)*dt;
//角度测量模型方程,角度估计值=上一次的最优角度+(角速度-上一次的最优零漂)*dt      //就漂移来说认为每次都是相同的Q_bias=Q_bias

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

    PP[0][0] += Pdot[0] * dt;   
    PP[0][1] += Pdot[1] * dt;   
    PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;

    PCt_0 = C_0 * PP[0][0];                //矩阵乘法的中间变量
    PCt_1 = C_0 * PP[1][0];                //C_0=1
    E = R_angle + C_0 * PCt_0;        //分母
    K_0 = PCt_0 / E;        //卡尔曼增益,两个,一个是Angle的,一个是Q_bias的
K_1 = PCt_1 / E;

    Angle_err = Accel - Angle;
    Angle += K_0 * Angle_err;        //计算最优角度   
    Q_bias += K_1 * Angle_err;           //计算最优零漂
//Gyro_x = Gyro - Q_bias;            //计算得最优角速度

    t_0 = PCt_0;                         //矩阵计算中间变量,相当于a
    t_1 = C_0 * PP[0][1];                //矩阵计算中间变量,相当于b
    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;
}

使用特权

评论回复
10
MJM_WSY| | 2015-11-30 07:51 | 只看该作者

这个代码跟以前我下载的差不多,先谢谢你了,还能单独给我发出来。
这个是导入两个参数的。
现在我这采样回来的只有AD的数值,怎么做才能让AD的数值更平滑?这个函数要怎么用?能指点一下不

使用特权

评论回复
11
犹豫的大三| | 2015-11-30 08:44 | 只看该作者
MJM_WSY 发表于 2015-11-30 07:51
这个代码跟以前我下载的差不多,先谢谢你了,还能单独给我发出来。
这个是导入两个参数的。
现在我这采样 ...

你的AD采样速度是多少?太慢的话没有必要用。这个算法滞后太严重了,需要人工改变那两个参数值。

使用特权

评论回复
12
cztqwa|  楼主 | 2015-11-30 09:18 | 只看该作者
MJM_WSY 发表于 2015-11-30 07:51
这个代码跟以前我下载的差不多,先谢谢你了,还能单独给我发出来。
这个是导入两个参数的。
现在我这采样 ...

代码也是从网上下的,主要是文档里对这段代码的实现进行了分析。这段代码一般用于陀螺仪和加速度计融合数据用的。

使用特权

评论回复
13
cztqwa|  楼主 | 2015-11-30 10:40 | 只看该作者
MJM_WSY 发表于 2015-11-30 07:51
这个代码跟以前我下载的差不多,先谢谢你了,还能单独给我发出来。
这个是导入两个参数的。
现在我这采样 ...

如果只是想让AD的数值更平滑,有很多软件滤波的方法可以参考的,比如:限幅滤波法、递推平均滤波法等。您可搜一下“10中软件滤波算法”,里面也许有您需要的。

使用特权

评论回复
14
309030106| | 2015-11-30 20:33 | 只看该作者
主要是文档里对这段代码的实现进行了分析

这个资料应该是不错的,期待上传

使用特权

评论回复
15
MJM_WSY| | 2015-12-2 14:15 | 只看该作者
cztqwa 发表于 2015-11-30 10:40
如果只是想让AD的数值更平滑,有很多软件滤波的方法可以参考的,比如:限幅滤波法、递推平均滤波法等。您 ...

挨个试了一遍。。。都不行。  很不理想

使用特权

评论回复
16
MJM_WSY| | 2015-12-2 14:17 | 只看该作者
犹豫的大三 发表于 2015-11-30 08:44
你的AD采样速度是多少?太慢的话没有必要用。这个算法滞后太严重了,需要人工改变那两个参数值。 ...

要求就是快。所以以前我也套用了卡尔曼,确实是平滑度很高。但是滞后十分严重。

使用特权

评论回复
17
犹豫的大三| | 2015-12-3 08:54 | 只看该作者
MJM_WSY 发表于 2015-12-2 14:17
要求就是快。所以以前我也套用了卡尔曼,确实是平滑度很高。但是滞后十分严重。 ...

如果采样速度很快的话,滞后应该会好点。此外设置一个定值,如果两次采样的值之间的差值小于这个定值,用卡尔曼滤波。如果差值大于这个定值,可以修改卡尔曼滤波的参数,让它迅速到当前值。这样滞后应该就会减小了。你可以试一下看看!我的AD采样速度不快,用卡尔曼滤波效果不怎么好,你的采样速度快的话可以试一下这种方法。

使用特权

评论回复
18
jasonell| | 2015-12-6 07:23 | 只看该作者
学习了。

使用特权

评论回复
19
MJM_WSY| | 2015-12-9 14:44 | 只看该作者
犹豫的大三 发表于 2015-12-3 08:54
如果采样速度很快的话,滞后应该会好点。此外设置一个定值,如果两次采样的值之间的差值小于这个定值,用 ...

我不太了解这个卡尔曼。以下是我以前用过的一个卡尔曼的代码。我不太确定这个是不是卡尔曼。用这个的确是平滑度很高。就是滞后严重。STM32F103ZET6下使用的。

#include "KF.h"


   
double KF_Q = 0.01;                // Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
double KF_R = 0.1;                // R:测量噪声,R增大,动态响应变慢,收敛稳定性变好     



u16 KalmanFilter(u16 v)
{
        static double x_last;
        static double p_last;
        double x_mid;
        double x_now;
        double p_mid;
        double p_now;//协方差
        double kg;   //卡尔曼     
       
       
        x_mid = x_last;                                                 //x_last=x(k-1|k-1), x_mid=x(k|k-1)
        p_mid = p_last + KF_Q;                                 //p_mid=p(k|k-1),p_last=p(k-1|k-1),KF_Q=噪声
        kg = p_mid / (p_mid+KF_R);                 //kg为kalman filter,R为噪声
        x_now = x_mid + kg * (v-x_mid);        //估计出的最优值

        p_now = (1-kg) * p_mid;                                //最优值对应的covariance        

        p_last = p_now; //更新covariance值//协方差
        x_last = x_now; //更新系统状态值              

        return        x_now;
}

//end

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

2

主题

33

帖子

0

粉丝