Gyro+Accelerometer+kalman=>Tilt angle

coolhe · 发表于 2008-7-28 21:08:56

大家好!
现正从事Gyro+Accelerometer应用于动态Tilt angle检测的项目,资源如下:
Gyro:ENC-03R
Accelerometer:MMA6270
Algorithm:Kalman
MCU:Mega168
有没有朋友做过或对此兴趣,可以一起讨论学习.
邮箱随时开通.

zhcool_521 · 发表于 2008-7-29 08:14:41

我在苦恼的过程中！！！

Gyro: ENC-03R /ADXRS150/EWTS82
Accelerometer: LIS3L02A
Algorithm: Kalman 或 +其他
MCU: 51 （资源紧缺，可能得换）

你邮箱随时开，但没注明邮箱地址。邮箱交流不如论坛，或者qq/msn

luzhengmao · 发表于 2008-7-29 14:15:46

我觉得cpu 还是用avr32 or arm 比较合适。

zhcool_521 · 发表于 2008-7-29 14:23:37

有同感，DSP更好！

coolhe · 发表于 2008-7-29 19:24:39

不好意思，邮箱设置晚了一步，现在开通了。
对于MCU当然是ARM/DSP越轻松，但项目还受成本的控制，另外，对于这样的IMU检测动态tilt angle并进行控制，MCU资源应当是够了的！

coolhe · 发表于 2008-7-29 19:27:25

To:zhcool_521
你的“苦恼”何在？说说一起分享。
或邮件到Eengineerhe@163.com

tryagain · 发表于 2008-7-29 20:10:06

有人用过lis302dl吗？为了使其在有加速度新数据时能在INT1产生信号，我做了一下设置：
1、write 47H to ctrl_reg1
2、write 0H to ctrl_reg2
3、write 04H to ctrl_reg3
不知道为什么INT1（PIN#8）不会产生信号？

zhcool_521 · 发表于 2008-7-30 08:19:20

苦恼1： 51 资源不够用！
苦恼2：上位机作的 Kalman 还有问题。

(原文件名:ourdev_358271.JPG)
如图标号1 ，剧烈震动时滤波后毛刺还是较高。
如图标号2 ，急速增大或减小角度时滤波后的角度没有跟上角度的变化。

coolhe · 发表于 2008-7-30 19:59:34

你的检测更新周期是多少ms?是不是因为计算周期太长？
如果你的算法结构完全正确的话，就在于参数配置了。
我的效果要比这个好一些，但还是不理想，仍存在一些细节问题。

zhcool_521 · 发表于 2008-7-31 08:10:42

更新周期为 20ms ；算法结构不能保证完全正确，第一次做这种数学题，没什么经验！留个qq 或者msn 晚上找时间聊聊！

zhcool_521 · 发表于 2008-7-31 11:53:41

(原文件名:互补.JPG)

1 所指是k值过小先验估计比重过大也就是角速度比重过大。

2 所指k值过大先验估计比重过小也就是加速度比重过大。

两者不能兼容！如何是好？

coolhe · 发表于 2008-7-31 21:30:47

To:zhcool_521
能否详细说说你的开发思路？传感器除了用Accelerometer外还有其它吗？你的算法是在ＰＣ机完成？如果是在ＰＣ上完成，那更新周期是否再调短？我想周期越短可能会好一些(我是说＂可能＂)．
　我现在采用Gyro与Accelerometer作为信息感应部分，因为两者都有自已的不足，才加入kalman对其熔合，欲得到准确的倾斜动态角度，我的更新周期在7~8ms，由于检测手段不足，仍在开发_论证中．
　Ｐ．Ｓ．：不知你的波形是通过什么方式显示的？(即＂双通道＂显示)
(我的ＱＱ：912722059，不过常在办公室不方便用,不好意思)

feng_matrix · 发表于 2008-7-31 22:00:01

这个是我做的效果，白色线是三轴加速度计算的X轴倾角，红色是经过kalman滤波后的
测量协方差、预测协方差参数还不怎么会选择，白色红色基本吻合，滤波效果不大
下方两个圆是用来直观显示陀螺仪硬件倾角的，从网上视频学来的
PC程序用DIRECT X SDK做的

(原文件名:CIMG0215.JPG)

zhcool_521 · 发表于 2008-8-1 10:14:34

To:coolhe
我的资源
Gyro: ENC-03R /ADXRS150/EWTS82
Accelerometer: LIS3L02A
Algorithm: Kalman 或 +其他
MCU: 51 （资源紧缺，可能得换）

单片机采样（20ms）通过串口传pc， pc作Kalman 处理然后画出来的！采样周期有限制（传感器的响应不够）。

作的效果和feng_matrix 差不多！个人觉得只能做到这么好了！下一步时间充裕的话，打算试试扩展kalman

coolhe · 发表于 2008-8-1 18:20:12

To: zhcool_521
如果你的算法是在PC机完成,MCU只是进行AD与UART操作,资源应当够吧(只是猜测,我知道用Mega168是够了的,AD UART kalman 及控制全在上面完成).
按你所述,我们的理想思路是一样的,你的效果不好,我觉得可能是参数的问题了.

coolhe · 发表于 2008-8-1 18:23:32

To: zhcool_521
以资料不知你参考过没,不妨看看,我觉得很有用.
http://tom.pycke.be/mav/92/kalman-demo-application
另外,很多资料都说Q与R参考可以通过数学计算与实验得到,但没有一位说如何得到,其反应的准确物理含义,我为此想了好长时间,也没有彻底搞清楚,暂且通过实验比较效果了,不知谁能指点一下?

zhcool_521 · 发表于 2008-8-2 08:59:56

指教谈不上，我也在摸索。

以上资料我都仔细看过了，看样子你也试验的那位老兄的方法，我觉得他的p 求解有问题！

Q与R 我都使用的常量。

这是我的最佳效果：

(原文件名:更新.JPG)

zhcool_521 · 发表于 2008-8-2 09:00:50

在pc 上作 kalman 只是过渡一下，最终还是要在单片机上实现的

coolhe · 发表于 2008-8-2 12:29:14

你是指以下表达式吗？
公式： P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt?* P(1,1) + Q(0,0）
程序中：filterdata->P_00 += - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
我觉得完整形式应当为：
P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt^2* P(1,1) + Q(0,0).

zhcool_521 · 发表于 2008-8-2 16:21:10

参照 http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data 作的。

(原文件名:1111.JPG)

最后一个公式。把求解 k-1时刻后验估计误差协方差和 k时刻先验估计误差协方差合起来的公式。可能有点问题。

coolhe · 发表于 2008-8-3 09:44:15

以下是我做了几个测试结果：

(原文件名:Wave of Kalman1.jpg)

(原文件名:Wave of Kalman2.JPG)

(原文件名:Wave of Kalman3.jpg)

zhcool_521 · 发表于 2008-8-3 10:07:52

你做的效果也是一般。可能不仅仅是参数的问题！

coolhe · 发表于 2008-8-4 09:59:53

我是这么考虑的：
1、尽量使Gyro积分值与实际的角度值一致，确保Gyro工作及程序正确；
2、测试Acclerometer工作情况，利用其静止时测得值与实际倾斜值一致，以确保其工作正常；
3、Kalman算法结构正确；
=>配置参数观测kalman结果值，并与实际角度值相对照，以达到两者吻合为止。
当然，这里还有一个问题，"实际角度"如何测出来？如果方法不同，工具不一样，所测得的精度也不一样，也就是“基准”不一样了，直接影响系统结果。
zhcool_521：不知这样的思路你是否认同？

zhcool_521 · 发表于 2008-8-4 10:56:23

coolhe:
"1、尽量使Gyro积分值与实际的角度值一致" 我觉得角速度最好不要积分求角度的好，没有什么参考价值，积分误差太大。可以上传你的Gyro积分效果图分析一下。

我做的倾角传感器，不需要精确输出，（只需要和绝对角度成线性比例即可）。

我感觉你做的Kalman输入参数和我有所不同。你的输入参数可能是（我猜的）：先验估计直接用角速度积分得到。过程测量值为加速度测量值。

我做的先验估计=(k-1时刻后验估计) + k-1时刻角速度*采样时间 + w（k）
过程测量值=加速度测量值。

coolhe · 发表于 2008-8-5 19:01:04

积分示意图:

(原文件名:Integral_.jpg)

通过上图是无法判定其精度的,而且,这个精度与所采用的“基准”有关，如果基准很一般的话，那就更难判定了。

coolhe · 发表于 2008-8-5 19:05:56

To:zhcool_521
如你所说，我的先验估计确实是Gyro的积分得到的，但通过你的阵述，你是采用倾角传感器？其输出是反应什么信息？角度还是角速度？如果是反应角度，为什么在你的先验估计中：
先验估计=(k-1时刻后验估计) + k-1时刻角速度*采样时间 + w（k）
还有角速度呢？这个角速度通过什么方式得到？
如果输出是反应角速度，是不是就和上式吻合了？也就是通过Gyro输出积分得到先验估计了？
不知我的理解是否正确？

zhcool_521 · 发表于 2008-8-5 21:40:38

先验估计=(k-1时刻后验估计) + k-1时刻角速度*采样时间 + w（k）

上式: k-1时刻角速度是通过Gyro采样得到.

kalman输出反映的是角度.

coolhe · 发表于 2008-8-6 18:41:54

这样看来思路是一致的,正符合本贴主题:Gyro+Accelerometer+kalman=>Tilt angle.
:)

zhcool_521 · 发表于 2008-8-6 19:45:33

有些不同，你的先验估计是角速度积分！

coolhe · 发表于 2008-8-6 19:49:33

先验估计=(k-1时刻后验估计) + k时刻角速度*采样间隔时间 + w（k）

cuilulu0518 · 发表于 2008-10-28 16:02:44

你么好，看了关于这些讨论姿态算法的帖子，关于那个卡尔曼算法，我有几点不太明白，是否有人可以帮我解答一下，甚为感谢~~~

1.kalman滤波中关于陀螺仪的输入值应该是角速度值还是应该为角速度值积分后得到的角度值，即u的值应该怎样确定？

2.KALMAN滤波更新中的单位究竟是角度还是弧度？

coolhe · 发表于 2008-11-3 11:39:41

To:cuilulu0518,我的理解如下:

  1.在kalman预测估计中,陀螺仪输入角速度与时间,算法运行时将其转化为角度,然后与加速度计所测的角度进行融合.

  2.采用哪种单位没有严格要求,只要一致就行,即要么都采用角度,要么都采用弧度,一般来说,采用角度更精确些.

本贴被 coolhe 编辑过,最后修改时间：2008-11-03,11:40:19.

346675655 · 发表于 2009-4-23 10:21:06

在角速度积分为角度的时候累计误差怎么处理的
匀速过程中积分角度可重复性可以
速度变化太快就会出现误差

1ms采100个点然后取平均*1ms 求得1ms转过角度
然后累加至1s

13590955160 · 发表于 2009-12-5 17:38:39

mark

baiya · 发表于 2010-7-11 16:09:24

学到了很多，受用了

electronicYH · 发表于 2010-7-14 19:45:03

我也研究好久了，不过我一直在做仿真，有个问题我一直疑惑，就是但遇到长时间朝一个方向的外力时，根据卡尔曼滤波的算法，此时的滤波结果或与真实值相差一个比较恒定的值。

这个问题不知道各位是怎么解决的？

下面是我的仿真过程：
http://www.ourdev.cn/bbs/bbs_content.jsp?bbs_sn=1390750&bbs_page_no=1&search_mode=4&search_text=electronicYH&bbs_id=9999###
我写的Matlab卡尔曼滤波仿真

我写了个的利用加速度和角速度测量一维倾角的仿真程序，程序中，另角速度测量值为U，x(k)为最终滤波后的值，
共200个时刻采样值，在第50~100时刻，假设物体角速度为0，倾角为20°，但收到水平方向的力，使得物体利用加速度测量的角度出错，
本想利用卡尔曼滤波算法来纠正结果，但发现有问题。
z(k)代表利用测量的加速度算出的倾角，其中t(k)为卡尔曼滤波中的卡尔曼增益k,我发现在，z(k)突变和角速度任然均值为0的时候，t(k),几乎没有什么变化，即，这个增益值没有在x(k)=xx(k)+t(k)*(z(k)-H*xx(k))这算式中起到衰减突变的作用，
请问是我的程序写做了，还是不应当把角速度当成U来用，还是卡尔曼滤波对突变的抑制作用有限？

clear all;
N=200;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u0=0;%% u0表示物体真实的转动角速度，
z0=20+u0;%% 物体的稳定倾角
%% 模拟ENC-03所测得的数据，（角速度）,数据分布在top1*(-0.5~+0.5)之间,
top1=10;
for k=1:N
w(k)=top1*(0.5-rand);
u(k)=u0+w(k);%%u0(k)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 物体没有旋转运动，但受到外力干扰，整体产生平移，设此时倾角为20°
%% 设k=50~100时刻，收到水平力干扰，此时，Z轴方向加速度为：az+gz,
%% X轴方向的加速度为：ax+gx
%% 原来无水平力干扰时有：tan(z0)=gz/gx;此时，(az+gz)/(ax+gx)不等于tan(z0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g=10;
gz=g*cos(z0*pi/180);
gx=g*sin(z0*pi/180);
a=10;
az=a*sin(z0*pi/180);
ax=a*cos(z0*pi/180);
top2=1;
for k=1:49
v(k)=top2*(0.5-rand);
z(k)=atan(tan((z0+v(k))*pi/180))*180/pi;%%z0(k)
end
for k=50:100
v(k)=top2*(0.5-rand);
z(k)=atan(((gx+ax)/(gz+az))+v(k))*180/pi;%%z0(k)
end
for k=101:N
v(k)=top2*(0.5-rand);
z(k)=atan(tan((z0+v(k))*pi/180))*180/pi;%%z0(k)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q=std(w).^2;%
R=std(v).^2;%
xx(1)=0;
x(1)=0;
pp(1)=10;
p(1)=10;
A=1;
B=1;
H=1;
for k=2:N
%预测
xx(k)=A*x(k-1)+B*u(k-1);
pp(k)=A*p(k-1)*A+Q;
%更新
t(k)=pp(k)*H/(H*pp(k)*H+R);
x(k)=xx(k)+t(k)*(z(k)-H*xx(k));%%z(k)-H*xx(k)
p(k)=(1-t(k)*H)*pp(k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
o2(1)=0;
for k=2:N
o1(k)=z(k);
o2(k)=o2(k-1)+u(k);
o3(k)=x(k);
end
figure(1);
k=1:N
plot(k,o1,'b',k,o2,'g',k,o3,'r');
图中，绿色的线是滤波后的结果，蓝色是加速度换算后的角度，红色是角速度积分结果。

(原文件名:1.JPG)

(原文件名:未命名.JPG)

g921002 · 发表于 2010-7-17 20:53:07

那是因為加速規的協方差你是用定值，沒有對外力項做修正。另外，四軸假設不會受到長期外力。也不需要對外項做修正。
這個假設對99％狀況都試用，沒有必要為了1%的狀況(可能更低)去費這心。但這狀況是有解的。

或是用三軸電羅盤校正吧！

回复【36楼】electronicYH
-----------------------------------------------------------------------

我也研究好久了，不过我一直在做仿真，有个问题我一直疑惑，就是但遇到长时间朝一个方向的外力时，根据卡尔曼滤波的算法，此时的滤波结果或与真实值相差一个比较恒定的值。

这个问题不知道各位是怎么解决的？

jade0606 · 发表于 2011-11-24 12:34:25

回复【12楼】feng_matrix 悟
-----------------------------------------------------------------------

我覺得你有可能是對的,如果過程沒有問題,那麼只能說,你的偵測值與濾波後的結果真的很靠近...
這樣,你的Device準確度真的很高,Noise很小..
或者你做的有問題,不過你的資料很少,判斷不出來

jade0606 · 发表于 2011-11-24 17:27:26

回复【36楼】electronicYH
-----------------------------------------------------------------------

我個人認為你使用的公式有問題...
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
檢查一下...

jade0606 · 发表于 2011-11-24 17:34:07

回复【36楼】electronicYH
-----------------------------------------------------------------------

另外,看到滤波结果的图形,觉得似乎有一些奇怪,我在想是不是使用的模型或对卡尔曼滤波的理论没有弄的很清楚,图形结果显然在对残差做分析就有问题…
卡尔曼滤波是平均残差接近等於0, 怎么会出现那样的现象, 是不是模型有问题…
X(k)=Fk X(k-1)+Bk Uk+wk,
Z(k)=Hk X(k)+Vk,
可以重新检讨

先验或后验估计,基本上都是无偏差估计,
正确的结果,滤波后的图形应该是,有滤波的图形嵌在没有滤波的数据中间,
如果实际测量值在一段長时间内连续大于或小于滤波后的数据,那么你的滤波大概是有问题的

不过由于您提供的数据太少,我并不清楚你的设定是什么, 可能我是错的也不一定

jade0606 · 发表于 2011-12-16 16:54:13

大概長這種圖形是比較對的

(原文件名:KF Sample.JPG)

hjf2002 · 发表于 2011-12-16 22:42:03

很不错的讨论，学习了.

zl6977 · 发表于 2011-12-18 14:54:37

卡尔曼的效果真的好强大啊。。。不知拉卡尔曼滤波代码多不多？不会真像网上的例子里写的那么多吧？

huttu · 发表于 2011-12-20 20:42:01

引用图片【16楼】zhcool_521
-----------------------------------------------------------------------

(原文件名:更新.JPG)

这个效果做得还不错的呀

jiajiajia · 发表于 2011-12-30 23:24:32

mark!

knight200300 · 发表于 2011-12-31 01:33:22

不错的，卡尔曼资料

nomady · 发表于 2012-1-5 12:56:55

请问kalman以后的结果所表现出的变化趋势是领先于实际测量值还是滞后的啊？？

thxl_kis · 发表于 2012-3-5 22:40:34

mark

Gyro+Accelerometer+kalman=&gt;Tilt angle

阿莫论坛20周年了！感谢大家的支持与爱护！！

Gyro+Accelerometer+kalman=>Tilt angle