AHRS: 误差四元数的间接Kalman滤波器

Anteater · 发表于 2012-10-4 03:15:36

本帖最后由 Anteater 于 2012-10-4 03:35 编辑

Matlab 中的算法测试，用生成的加速度、角速度和磁通的数据测试。

数据中的噪音是生成的，因为不知道实际中的测试数据能有多坏或者多好，因此噪音计加的有点大，偏置也不少。

从结果上来看，算法大约在10秒左右开始收敛，因此有一个启动时间

加了点噪声

滤波结果还是可以接受的，虽然噪音也很大，至少可以大致跟上姿态角变化

算法计算的姿态角和 ‘真实值’之间的差距在3-5度之间。考虑的到sensor数据中的噪音水平，kalman的性能还是挺好的。

xiaowenjian · 发表于 2012-10-7 20:36:02

楼主能不能传份实例程序学习学习，matlab的c的都行。

brb2010 · 发表于 2012-10-16 12:25:59

楼主能发一份完整的加速度、角速度和磁通数据么

Anteater · 发表于 2012-10-19 05:00:28

brb2010 发表于 2012-10-16 12:25
楼主能发一份完整的加速度、角速度和磁通数据么

这个倒是没问题，不过我刚刚重装系统，没有新的了，只有以前的数据了，见附件

Anteater · 发表于 2012-10-19 05:22:49

弄到片子上试了一下，看起来还行，参数没仔细调，实际上连程序中有没有bug都还没搞清楚。。。大致看一下觉得还不错，有些抖动，一部分是sensor太次造成噪音太大，一部分是参数没调好

http://v.youku.com/v_show/id_XNDYzNzcyODc2.html

brb2010 · 发表于 2012-10-19 12:28:56

Anteater 发表于 2012-10-19 05:00
这个倒是没问题，不过我刚刚重装系统，没有新的了，只有以前的数据了，见附件 ...

很感谢了

brb2010 · 发表于 2012-10-19 12:33:41

Anteater 发表于 2012-10-19 05:00
这个倒是没问题，不过我刚刚重装系统，没有新的了，只有以前的数据了，见附件 ...

麻烦问下，这数据是角度值？还是什么呢

Anteater · 发表于 2012-10-19 22:15:56

brb2010 发表于 2012-10-19 12:33
麻烦问下，这数据是角度值？还是什么呢

第一个三列应该是gryo数据，第二个三列是加速度计，然后第三个三列是罗盘

扎希德 · 发表于 2012-10-20 10:53:22

mark，留着用

Anteater · 发表于 2012-10-22 12:32:16

Anteater 发表于 2012-10-19 05:22
弄到片子上试了一下，看起来还行，参数没仔细调，实际上连程序中有没有bug都还没搞清楚。。。大致看一下 ...

简单调试了一下，罗盘还没有最后搞定，有挺大的静态偏置

基本上可以实现三轴姿态的哦噪音在1度以内，自我感觉良好，呵呵

试图在传感器输出端接FIR，再进kalman，发现不行，FIR引入的额外的相位滞后会引起kalman中当成偏置除掉，最终造成姿态不准，
在kalman输出端接FIR，可以把输出的三轴姿态噪音降到0.5度以内，但是还是引起太大的相移，输出姿态明显落后，最后还是直接kalman把，仔细挑挑参数，应该还能更好点。

droper · 发表于 2012-10-23 15:09:59

楼主能分享下滤波结构不呢？比如系统方程、观测方程什么的

Anteater · 发表于 2012-10-25 03:53:36

droper 发表于 2012-10-23 15:09
楼主能分享下滤波结构不呢？比如系统方程、观测方程什么的

当然，见附件

tree666 · 发表于 2012-10-25 15:38:59

非常感谢楼主，学习一下

__@ · 发表于 2012-10-25 17:01:50

学习下

droper · 发表于 2012-10-25 18:06:43

Anteater 发表于 2012-10-25 03:53
当然，见附件

非常地感谢楼主

Freezing_ · 发表于 2012-10-26 08:35:29

楼主是单轴的卡尔曼还是姿态矩阵的卡尔曼

Anteater · 发表于 2012-10-27 02:10:11

Freezing_ 发表于 2012-10-26 08:35
楼主是单轴的卡尔曼还是姿态矩阵的卡尔曼

都不是，见上面的paper，

业未央 · 发表于 2013-3-12 16:37:27

Anteater 发表于 2012-10-19 05:22
弄到片子上试了一下，看起来还行，参数没仔细调，实际上连程序中有没有bug都还没搞清楚。。。大致看一下 ...

楼主能把你的上位机软件传给我一份吗？yangliyeyly@gmail.com

jade0606 · 发表于 2013-3-15 10:10:19

本帖最后由 jade0606 于 2013-3-15 10:17 编辑

你是不是希望能有什麼辦法可以使得卡爾曼後的輸出可以不要那麼的跳動??希望噪聲再小一點...
如果是的話,你應該知道這個跳動其實是傳感器的噪聲造成的,在標準的卡爾曼濾波數據融合的過程中是避不開的...
你應該要做卡爾曼的變形...,建議你在優化參數後考慮卡爾曼濾波的過程...
我們其實也遇到過這樣的問題,
如果你明白擴展形卡爾曼的過程,我這樣說你應該就懂了,我不直接說答案是希望你能自己發現,這會有趣的多,
而且你可以真正學習到....

monkeynav · 发表于 2013-3-15 10:14:59

kalman不可以随便配合FIR之类的
带限白噪声或者粉红噪声理论上来说，都会让卡尔曼得不到最佳估计

jade0606 · 发表于 2013-3-15 10:37:10

本帖最后由 jade0606 于 2013-3-15 10:43 编辑

monkeynav 发表于 2013-3-15 10:14
kalman不可以随便配合FIR之类的
带限白噪声或者粉红噪声理论上来说，都会让卡尔曼得不到最佳估计 ...

與其他濾波混搭其實是可以的,但要小心...
在統計上有一句話,"所有的模型都是錯的"...
我們也曾經明明知道就算濾波後residual 噪聲不是白的也照樣使用...
只要是無偏差的估計,我們都敢用, (有偏差的也敢...,我們不是使用Σ(Xk –Xav)2/(N-1)的有偏差估計來估計標準差嗎?原因是因為它是most likelyhood)
只是,要很清楚用了會失去什麼東西,通常都是用了以後才發現狀況很慘,哈....
事實上,有時候也不一定要最佳估計的,常常是在某情形下,我們放棄最佳估計....

monkeynav · 发表于 2013-3-15 11:19:07

jade0606 发表于 2013-3-15 10:37
與其他濾波混搭其實是可以的,但要小心...
在統計上有一句話,"所有的模型都是錯的"...
我們也曾經明明知道 ...

FIR绝对不是什么好主意
延迟大会影响后面的控制环路性能
kalman smoother寻找最近n个输入值的最佳估计，有一点效果
再用形态学滤波或者滑动平均也不错。

快速融合出来的数据本身就是抖动很厉害的，这样才能体现出“快速”
如果觉得用不了那么快的更新速度，平均一下也没啥问题

Anteater · 发表于 2013-3-16 03:10:13

jade0606 发表于 2013-3-15 10:10
你是不是希望能有什麼辦法可以使得卡爾曼後的輸出可以不要那麼的跳動??希望噪聲再小一點...
如果是的話,你 ...

是的，的确想输出不那么跳就好了
传感器出的输出噪音已经计入到模型里面了，kalman一直在估计静态偏置和传感器噪音

可能是传感器噪音的covariance估计的不够

在这个帖子中第一次出现技术讨论，很高兴的说

Anteater · 发表于 2013-3-16 03:11:01

monkeynav 发表于 2013-3-15 10:14
kalman不可以随便配合FIR之类的
带限白噪声或者粉红噪声理论上来说，都会让卡尔曼得不到最佳估计 ...

是，加上去了就有很大的延迟

Anteater · 发表于 2013-3-16 03:13:12

jade0606 发表于 2013-3-15 10:37
與其他濾波混搭其實是可以的,但要小心...
在統計上有一句話,"所有的模型都是錯的"...
我們也曾經明明知道 ...

我不是很懂数字信号处理的部分，并且统计实在是功力太差，从数学上我实在是搞不定FIR + Kalman不行的原因，但是从实践角度明确的只知道不行，还是kalman自己的效果好

Anteater · 发表于 2013-3-16 03:14:33

monkeynav 发表于 2013-3-15 11:19
FIR绝对不是什么好主意
延迟大会影响后面的控制环路性能
kalman smoother寻找最近n个输入值的最佳估计， ...

我本来打算用滑动窗口 DB小波滤波了的，后来发现算法太复杂，dsp跑不动，就作罢了

monkeynav · 发表于 2013-3-17 17:19:20

Anteater 发表于 2013-3-16 03:14
我本来打算用滑动窗口 DB小波滤波了的，后来发现算法太复杂，dsp跑不动，就作罢了 ...

小波还可以，不要分解太多次，运算量还可以接受。
简单的组合几个典型的处理环节就可以了，没必要搞得太复杂。
一般来说，我最多做到d4，就不会考虑继续分解了。一般自己做降噪甚至只用d1-d2

jade0606 · 发表于 2013-3-19 16:15:10

本帖最后由 jade0606 于 2013-3-20 15:50 编辑

給你一點建議
1. 為什麼你一定要最佳估計呢?差一點的可以嗎?
2. 併用其他濾波器,不管是在卡爾曼之前先處理sensor的噪聲,或卡爾曼濾波後後又再使用其他濾波器處理噪聲,都不是好的選擇...
3. 差一點的gain值,會有什麼結果你可以試試...

asha · 发表于 2013-3-21 12:47:32

Anteater 发表于 2013-3-16 03:14
我本来打算用滑动窗口 DB小波滤波了的，后来发现算法太复杂，dsp跑不动，就作罢了 ...

FIR有很多人说能用，也有人说工程上使用过，但是这个有点疑问。你提供的论文上的模型我仿真了部分，发现静态下效果非常好，但是角速度一大就完蛋了。仿真不出那个效果，等会用你的数据进行下仿真。看看效果如何

asha · 发表于 2013-3-21 12:50:08

Anteater 发表于 2013-3-16 03:14
我本来打算用滑动窗口 DB小波滤波了的，后来发现算法太复杂，dsp跑不动，就作罢了 ...

还有就是你是用的开环卡尔曼还是闭环卡尔曼？看你的数据好像没有加入外部加速度吧？

flyforyou85 · 发表于 2013-3-22 12:09:34

Anteater 发表于 2012-10-19 05:22
弄到片子上试了一下，看起来还行，参数没仔细调，实际上连程序中有没有bug都还没搞清楚。。。大致看一下 ...

楼主，你的上位机太棒了，感觉跟真飞机上的显控一样，能否分享下！

Anteater · 发表于 2013-3-23 19:32:42

asha 发表于 2013-3-21 12:50
还有就是你是用的开环卡尔曼还是闭环卡尔曼？看你的数据好像没有加入外部加速度吧？ ...

没有估计外部加速度，这玩意不是IMU，是AHRS。

我的理解里面kalman应该都是闭环的吧，开环怎么搞？

asha · 发表于 2013-3-25 09:19:17

Anteater 发表于 2013-3-23 19:32
没有估计外部加速度，这玩意不是IMU，是AHRS。

我的理解里面kalman应该都是闭环的吧，开环怎么搞？ ...

你提供的论文我看了很长时间，实际他的创新点是采用特征值分解对外部加速度的R进行估计，然后扣除这个，滤波方式应该是开环的，闭环会比较麻烦容易估计错误。还有就是想问一下，你所提供的仿真数据共6001个点，时间间隔Ts是多少？陀螺加表磁是按照X Y Z顺序排列的么？加表里面加入外部加速度了么？谢谢

Anteater · 发表于 2013-3-26 02:11:49

asha 发表于 2013-3-25 09:19
你提供的论文我看了很长时间，实际他的创新点是采用特征值分解对外部加速度的R进行估计，然后扣除这个， ...

间隔5ms，XYZ排列，我记得帖子上说过吧

了解了一下开环和闭环解释，这个算法的确是开环的。我实际上不是这行当里面的，没觉得这个创新点有多么的nb。另外写这个特征值的算法费了我不少周折。

asha · 发表于 2013-3-26 15:30:28

Anteater 发表于 2013-3-26 02:11
间隔5ms，XYZ排列，我记得帖子上说过吧

了解了一下开环和闭环解释，这个算法的确是开环的。我实际上不 ...

这个算法好处在动态估计外部加速度的R矩阵，实际我仿真了下，真没有那么好的效果。和一般的没多少区别，不过这个思路真是很不错

Anteater · 发表于 2013-3-27 05:07:08

asha 发表于 2013-3-26 15:30
这个算法好处在动态估计外部加速度的R矩阵，实际我仿真了下，真没有那么好的效果。和一般的没多少区别， ...

我以前放到片子上试了一下，有没有实际上差不多，并且计算量的差距也很小，就放在里面没去掉，聊胜于无么。

asha · 发表于 2013-3-27 09:05:58

Anteater 发表于 2013-3-27 05:07
我以前放到片子上试了一下，有没有实际上差不多，并且计算量的差距也很小，就放在里面没去掉，聊胜于无么 ...

嗯，这个算法的优点是在动态情况下加速度传感器为管测量的时候不至于发散的太离谱，但是他最后有用磁观测了一次，这个算法意义就不大了。

Anteater · 发表于 2013-3-29 10:33:37

asha 发表于 2013-3-27 09:05
嗯，这个算法的优点是在动态情况下加速度传感器为管测量的时候不至于发散的太离谱，但是他最后有用磁观测 ...

罗盘修正的只是方向，但是加速度传感器的瞬间干扰可能是任何轴的，

ms外加速停止之后算法应该收敛，不应该发散才对

asha · 发表于 2013-3-29 13:04:00

Anteater 发表于 2013-3-29 10:33
罗盘修正的只是方向，但是加速度传感器的瞬间干扰可能是任何轴的，

ms外加速停止之后算法应该收敛，不应 ...

不是的，外部加速度就算停止了，收敛会很长时间。在你提供的论文上面磁只传播到航向

Anteater · 发表于 2013-3-30 01:06:31

asha 发表于 2013-3-29 13:04
不是的，外部加速度就算停止了，收敛会很长时间。在你提供的论文上面磁只传播到航向 ...

对的，算法在设计的时候就说了只用罗盘修正航向，这样的话剩下两轴主要靠加速度计提供参考。收敛时间看covariance matrix的值了，

本来文章上就是说AHRS，能考虑去除外加速度已经不错了，这玩意又不是IMU，从器件的角度根本应付不了长时间外加速度的干扰

chushibinsaobao · 发表于 2013-12-29 11:26:21

感谢楼主分享

sunsetTH · 发表于 2014-3-22 22:52:24

你好能否分享一下你的仿真程序啊非常想学习谢谢

Anteater · 发表于 2014-3-23 09:04:42

sunsetTH 发表于 2014-3-22 22:52
你好能否分享一下你的仿真程序啊非常想学习谢谢

参见帖子里面的论文附件，算法是核心，程序神马的自己写写就可以了，

sunsetTH · 发表于 2014-3-23 16:01:55

Anteater 发表于 2014-3-23 09:04
参见帖子里面的论文附件，算法是核心，程序神马的自己写写就可以了，

嗯谢谢可是matlab只会很简单的应用所以想学习您的代码

Anteater · 发表于 2014-3-23 22:12:33

sunsetTH 发表于 2014-3-23 16:01
嗯谢谢可是matlab只会很简单的应用所以想学习您的代码

I would recommend "MATLAB for Engineers" (3rd Edition). It is very textbook like.

chushibinsaobao · 发表于 2014-3-24 12:53:58

搂主辛苦了！学习中!最近也在学习卡尔曼滤波，想使用误差四元数作为卡尔曼的估计值，想找一些这方面的参考程序，不知道楼主有没有？

silence2455 · 发表于 2014-4-3 15:29:41

Anteater 发表于 2014-3-23 22:12
I would recommend "MATLAB for Engineers" (3rd Edition). It is very textbook like.

楼主大哥，我现在在用AHRS这个算法做姿态解算，用的是9150模块。
现在Pitch和Roll都没问题，但是Yaw很不正常（Yaw变化不均匀，而且水平转动时在某个角度Pitch和Roll会有很大变化）。
初步确定是磁力计出现问题。磁力计轴向旋转到和陀螺仪加计一致，校准可能有问题，原始数据也可能有问题。
请问楼主，磁力计的原始数据应该符合哪些规律？恳请赐教！！！

Anteater · 发表于 2014-4-4 06:45:44

silence2455 发表于 2014-4-3 15:29
楼主大哥，我现在在用AHRS这个算法做姿态解算，用的是9150模块。
现在Pitch和Roll都没问题，但是Yaw很不 ...

恩，差不多，就是看看soft/hard iron correction就行了

SMC · 发表于 2014-12-13 18:28:54

Anteater 发表于 2012-10-22 12:32
简单调试了一下，罗盘还没有最后搞定，有挺大的静态偏置

基本上可以实现三轴姿态的哦噪音在1度以内，自 ...

要接滤波器也是应该接IIR，这个滤波效果好，滞后小。

xianghaisha · 发表于 2014-12-13 18:33:03

好东西就学习一下。可能之后要做飞控的小程序

nust5042 · 发表于 2015-8-8 21:15:10

我打算对9轴传感器做kalman滤波，使用的是姿态四元数，麻烦楼主共享源码，或者发我邮箱，多谢 122807637@qq.com

AHRS: 误差四元数的间接Kalman滤波器

本帖子中包含更多资源

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

本帖子中包含更多资源

本帖子中包含更多资源