Kalman Filter ( Gyro& Accelerometer)

chesley88

i. 你要想做什么？ 你的应用是什么？我是做2轮平衡机器, 用的是PIC 16F877A.




ii. 你想要拿到什么效果
   我现在想要用gyro和accelerometer来平衡,我在网上找了很多关于kalman filter的资料，
   但不知道要怎样把gyro x,y output value 和accelerometer x,y,z output value放进kalman filter的formula里面。



   
iii. 你做了什么
    我之前用Wii Motion Plus 的gyro 来平衡,因为它的output是I2C格式,我不会用PIC来debug，
    所以我跟朋友借Arduino来debug。
    debug过后，我遇到的问题是，没有accelerometer的话，gyro的angle value不准确. 有时震动时，value会跑调~
    而且，Wii motion Plus 的gyro有auto-zero function，
    也就是如果调一个angle 过后，保持着不动，gyro会自动回去零度。

    下面是我之前debug的结果，但不是很清楚。我的wii motion plus已经借我朋友用，
    (如果各位大大需要清楚的video ，那我星期二upload比较清楚的video.)
    Video links:http://www.youtube.com/watch?v=cnH61QlE9iA

  但我已经不想用Wii Motion Plus,现在想用sgbotic(新加玻）的accelerometer      
  和gyro.
  以下是我选中的accelerometer和gyro.


   Triple Axis Accelerometer Breakout - MMA7260Q


This is a breakout board for Freescale's triple-axis MMA7260QT accelerometer. With a low power shut-down mode, high sensitivity output with selectable ranges (±1.5, 2, 4, and 6g), this was one of the very first sensors to market with three accelerometers built onto a single IC!  Board comes fully assembled and tested with external filters installed.

The MMA7260QT is a 3.3V part and outputs an analog voltage for each of the three outputs. This voltage is in ratio to the measured acceleration and to the supply voltage (ratiometric). You will need some extra hardware to convert this analog signal to a usable digital one. Luckily, many uCs have a built in Analog to Digital converter

accelerometer links: http://www.sgbotic.com/index.php?dispatch=products.view&product_id=107

Gyro Breakout Board - LPR530AL Dual 300°/s

This is a breakout board for the ST's dual-axis LPR530AL gyro.  The LPR530AL measures angular velocity along the pitch and roll axes with a full scale of ±300°/s. Two different analog outputs are provided for both the x- and y- axes - one 1x amplified and the other 4x amplified。A regulated voltage between 2.7 and 3.6VDC should be supplied to the power pins. We have the filtering circuits all set up; you'll just need to connect the outputs to an ADC, and you're ready to go.

This breakout board includes the gyro and all necessary filtering capacitors as shown. The 1x and 4x amplified outputs of both axes are connected to the 0.1" pitch headers, along with the power-down, self-test, high-pass filter reset, and power pins.
Features:

• 2.7 to 3.6VDC power supply
• Dual axis, pitch and roll sensing
• 1x and 4x amplified outputs for each axis
• Low power consumption
• All necessary filtering components included
• Access to power-down, self-test, and high-pass filter reset pins
  

gyro links: http://www.sgbotic.com/index.php?dispatch=products.view&product_id=528

---------------------------------------------------------------------------------------------------------------------------------

iv. 你遇到了什么问题。

    我现在遇到的问题是不明白kalman filter的formula怎样用， 要怎样把gyro output value和accelerometer output value apply进
    kalman filter的formula里面。
    这是我在Wikipedia找到的formula.


Predict

Predicted (a priori) state estimate
Predicted (a priori) estimate covariance

[edit]Update

Innovation or measurement residual
Innovation (or residual) covariance
Optimal Kalman gain
Updated (a posteriori) state estimate
Updated (a posteriori) estimate covariance

v. 你用什么MCU？
   PIC,但用arduino来debug，考虑着用Arduino，用了Arduino，觉得Arduino比较容易~




vi. 你用什么语言？ ASM /C/PICBasic/Keil？Compiler 版本?
   C language




vii. 有源码吗？ 如是抄来的请注明出处， 请放链接。
    没有，只是想明白要怎样apply output value进formula里面。




viii. 有电路图吗？
     没有




ix. 你google 了吗？ 你的搜索关键字是什么？
    google 了,关键字是kalman filter




x. 有照片证明你做的东西吗？还是只是概念性的空谈？
   我还没买gyro和accelerometer，想知道如何用kalman filter的equation来算准确的angle，之后我会买来试，
   如果有进展，我会上载最新的照片给大家分享。





xi. 是你自己的问题？ 还是你帮人问的? 请说明。
    我自己的问题~不是帮别人问的~









对不起，有些用英文字，因为有些是从网站copy的，请各位大大不要见怪~
有人会用kalman filter的formula吗？ 可以解释吗 ？我想了解~

chesley88

这是我的2wheels balancing inventor概念图~

waiweng83

楼主可以参考 Robot Head to Toe 的 magazine，第四期。里面有讲关于两轮平衡机器人的东西。Sample source code 可以在 http://www.robothead2toe.com.my/userpage.php?id=download 下载。

chesley88

回复 3# waiweng83

谢谢waiweng83大大，我已经下载了。我想问下，有些Gyro有Auto-Zero的feature，这功能会改变到source code的写法吗？还是一样的？
（对不起啊，对Gyro的Auto-Zero的真正用途不是很了解。）

waiweng83

回复 4# chesley88


    我看了 LPR530AL 的 datasheet，没有找到 Auto-Zero 的东西。请问你所说的 auto-zero 是什么？

chesley88

回复 5# waiweng83

我之前用的Wii Motion Plus里的Gyro有Auto-Zero feature，所以debug时，Angle乱乱跑的。 我也看过cytron的IMU 5 DOF board, 用的Gyro 是 IDG500，有Auto-Zero feature，由于Cytron的IMU有这Auto-Zero function，我又不懂有没有影响到，所以我才选新加玻的LPR530AL。

IMU Gyro的datasheet links:  IDG500

这Auto-Zero Function会影响到Output吗？

waiweng83

回复 6# chesley88


    IDG500 的 auto-zero function，在2轮平衡机器人里是没有用到的。Kalman Filter 会自动把 gyro 的 bias 消除。

chesley88

回复 7# waiweng83

哦~那我就方便多了，不用麻烦到银行去进钱买新加玻的Gyro，我买Cytron的回来试，有问题的话，我再请教你~ 谢谢你~

chesley88

回复 7# waiweng83


   waiweng83大大，有些问题想请教你。


   1）请问那个accelerometer z-axis的output有用到的吗？为什么在pic connection有accelerometer z-axis的input? 但我在source code里却没看到它的存在。


   2）kalman 里的covariance R and Q value全是fixed的吗？如果不是固定的value，要怎样定这covariance的value？



   3) #define TRISB_VALUE
0xfcff


       0xfcff是hex number,有16bit,但PIC的port B只有10个port,我可不可以直接define TRISB=%001111111 ?这样define对吗?

waiweng83

回复  waiweng83
1）请问那个accelerometer z-axis的output有用到的吗？为什么在pic connection有accelerometer z-axis的input? 但我在source code里却没看到它的存在。
chesley88 发表于 22-11-2010 12:00 PM

有啊。在 adc.c 里面。

回复  waiweng83
kalman 里的covariance R and Q value全是fixed的吗？如果不是固定的value，要怎样定这covariance的value？
chesley88 发表于 22-11-2010 12:00 PM

这个不是固定的，而是要 tune 的。至于要怎样调，那我也不是很懂 我只是随便 hentam 一个 value，可以用就没有在去管它了。
如果这是你的 FYP，那最好还是去了解 Kalman Filter 的原理和这两个 variable 的关系。不然你 present 的时候就会被射了。

回复  waiweng83
#define TRISB_VALUE 0xfcff

0xfcff是hex number,有16bit,但PIC的port B只有10个port,我可不可以直接define TRISB=%001111111 ?这样define对吗?
chesley88 发表于 22-11-2010 12:00 PM

可以。因为 bit 15:10 是 unimplemented，你放什么进去都无所谓。

不过，binary 是：0b0011111111