极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 62306|回复: 5

卡尔曼滤波过程噪声和测量噪声方差如何选取

[复制链接]
发表于 2012-12-9 20:19:41 | 显示全部楼层 |阅读模式
大家好,小弟最近刚开始弄MPU6050,想使用卡尔曼作为我的滤波方法
现在碰到一个问题就是:过程噪声的方差Q和测量噪声的方差R如何选取的问题

之前看过一个入门帖,温度的例子
http://www.geek-workshop.com/for ... ;pre_pos=1&ext=
这里Q=1e-6,R=1e-1但也没有提如何选取。网上收了下是根据经验选取{:soso_e103:}

然后在坛子里找到个帖子
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=681
角度的方差噪声,给了0.01,不太清楚如何给的

小弟现在已经有MPU6050二十分钟的静态数据,
请问
1、我该如何从该组数据中提取测量方差R?
2、过程噪声Q又如如何确定?

谢谢各位了{:soso_e154:}

提供下温度模拟示例Matlab代码

  1. clear
  2.   N=116;
  3.   %w(1)=0;
  4.   %w=randn(1,N);
  5.   w=0;                  %系统控制矩阵
  6.   x(1)=0;
  7.   a=1;                  %温度模拟A为1
  8.   V=randn(1,N);
  9.   q1=std(V);
  10.   %Rvv=q1.^2;
  11.   Rvv=0.1;              %测量过程协方差 温度模拟R为1e-1
  12.   q2=std(x);
  13.   Rxx=q2.^2;
  14.   q3=std(w);
  15.   %Rww=q3.^2;
  16.   %Rww=0.000001         %温度模拟Q为1e-6
  17.   Rww = 0.000001;       %静态数据的方差
  18.   %c=0.6;
  19.   c=1;                  %温度模拟H为1

  20.   for k=1:N;
  21.   Y(k)=25+sqrt(0.1)*randn(1);  %温度模拟平均温度为25度 方差(协方差)为0.1的温度输入 测量方程,其中V为测量系统的噪声,c为测量系统的参数
  22.   end

  23.   p(1)=10;              %协方差 初始值
  24.   s(1)=1;               %最优估计 初始值
  25.   for t=2:N;
  26.   s(t)=a*s(t-1)+w;                 %先验估计 求当前时刻的估计值  
  27.   p1(t)=a.^2*p(t-1)+Rww;           %协方差估计 求当前时刻的估计值的偏差,a为系统参数,没有控制量,所以没有参数b,Rww为噪声
  28.   b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);   %求Kg,b(t)为Kg,即Kalman增量
  29.   s(t)=a*s(t)+b(t)*(Y(t)-a*c*s(t));%后验估计 求t时刻的最优值,即当前时刻的最优值
  30.   p(t)=p1(t)-c*b(t)*p1(t);         %后验协方差 求当前最状态最优值的偏差,即式子5:(1-c*b(t))*p1(t)
  31.   end

  32.   figure(1);
  33.   plot(Y,'g--');hold on;
  34.   plot(s,'r--');hold on;
复制代码



还有MPU6050的静态数据



本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

发表于 2012-12-12 11:09:44 | 显示全部楼层
本帖最后由 johnsonzzd 于 2012-12-12 11:17 编辑

你的数据,9列各是什么东西?
数学模型:角速度积分得出角度。角速度传感器的零漂等对应模型噪声Q,这个值要小。模型不准,啥都白搭。
测量:加速度值测出来的角度。测量值的方差就是测量方差R。这个值大小不重要,估计一下就行了。

你的温度例子有点复杂,给你写一个简单的便于理解
  1. clc; clear
  2. N=200;
  3. for t=1:N
  4.     z(t)=25+3*randn;     %测量值
  5. end
  6. R=std(z)^2             %测量噪声

  7. V=0.1*randn(1,N);
  8. Q=std(V)^2             %模型噪声

  9. p(1)=10;
  10. x(1)=0;
  11. for t=2:N;
  12.     x1(t)=x(t-1);                      %预测状态
  13.     p1(t)=p(t-1)+Q;                 %预测估计协方差
  14.    
  15.     k(t)=p1(t)/(p1(t)+R);           %最优卡尔曼增益
  16.     x(t)=x1(t)+k(t)*(z(t)-x1(t));   %更新状态估计
  17.     p(t)=(1-k(t))*p1(t);                %更新协方差估计
  18. end

  19. t=1:N;
  20. plot([0,N],[25,25],'g',t,z,'b',t,x,'r');
  21. legend('真实值','测量值','估计值');
复制代码




最后,MPU6050内部自己有算法,能够直接取出角度,不需要自己再写卡尔曼之类的程序了。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

发表于 2013-4-25 02:11:34 | 显示全部楼层
johnsonzzd 发表于 2012-12-12 11:09
你的数据,9列各是什么东西?
数学模型:角速度积分得出角度。角速度传感器的零漂等对应模型噪声Q,这个值 ...

http://www.geek-workshop.com/for ... thread&tid=2747
这里面的角度置信度,角速度数据置信度指的是方差吗?都是通过数据测量计算出来的吗?这个值在滤波算法里是不是越小越好?
回复 支持 反对

使用道具 举报

发表于 2014-7-15 23:00:06 | 显示全部楼层
mark      
回复 支持 反对

使用道具 举报

发表于 2015-6-30 09:22:15 | 显示全部楼层
那个不是ARDUINO代码吧,ARDUINO应该是C/C++代码
回复 支持 反对

使用道具 举报

发表于 2016-12-5 18:20:21 | 显示全部楼层
johnsonzzd 发表于 2012-12-12 11:09
你的数据,9列各是什么东西?
数学模型:角速度积分得出角度。角速度传感器的零漂等对应模型噪声Q,这个值 ...

大神,你好,想请教您一个问题,我现在有加速度计和陀螺仪的数据,怎么实现实现卡尔曼滤波呢?得出一个图像,有一条加速度计的曲线,一条陀螺仪的曲线,一条融合之后的曲线,关键这个图怎么得到呢,我基础比较差。
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-28 17:29 , Processed in 0.046608 second(s), 24 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表