mega16实现卡尔曼滤波计算俯仰角和横滚角程序

zht9961020 · 发表于 2009-3-30 13:27:28

参照“4轴姿态控制算法讨论贴 ”中kb6mcc 大侠提供的代码，用mega16+mma7260+ewts82验证了一下卡尔曼滤波的效果，上位机就会点bcb，波形显示没做出来，只好通过串口观察滤波效果，感觉效果还可以。就是加速度传感器和陀螺的信号直接进mega16的ADC，卡尔曼以后串口输出结果，比较简单
程序：点击此处下载 ourdev_430061.rar(文件大小:3K) (原文件名:kem.rar)

pitolan · 发表于 2009-3-30 16:19:37

好樣的﹐頂下你﹗

pinocchio · 发表于 2009-3-30 16:27:10

学习。

yuzhou211 · 发表于 2009-3-30 20:40:47

楼主能对程序中的滤波算法解释一下吗？

wj198631 · 发表于 2009-3-31 08:12:44

支持了厉害

zht9961020 · 发表于 2009-4-3 17:48:54

对卡尔曼滤波的简单解释：
先给出一个网上的例子：假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。

---------------------------------------------------------------------------------------
对于四轴飞行器的倾角测量，跟上面的例子类似。首先可以由陀螺仪计算出一个倾角，即倾角预测值xhat_est = A * xhat + B * u
其次可以由加速度计算出一个倾角测量值y，究竟应该相信哪个多一点呢？这就需要卡尔曼增益K来决定了，
即更新的倾角xhat = xhat_est + K * Inn; 其中 Inn = y - c * xhat;

个人认为Inn应该=y-c*xhat_est才对，不知道是不是理解错了，有明白的高手请指点一下。

yuzhou211 · 发表于 2009-4-3 20:37:52

感谢楼主精彩的讲评

zhengrob · 发表于 2009-4-3 21:21:54

谢楼主。

xj_1232005 · 发表于 2009-4-4 21:39:23

精彩。叫好！

ksh84222 · 发表于 2009-4-5 10:12:31

精彩的讲评！

icefire · 发表于 2009-4-6 15:40:47

学习学习

pinocchio · 发表于 2009-4-7 11:18:36

zht9961020 讲解的不错，能不能结合四轴再来个例子？

bluemouse · 发表于 2009-4-8 23:56:36

困惑了好久了，终于拨开云雾见云端。

wj198631 · 发表于 2009-4-10 14:20:06

很厉害啊

sunsmile78 · 发表于 2009-4-10 15:35:08

请问zht9961020，
我用的是fc v1.2(pitolan），请问是否无需修改kem.c，就能直接调用它运行？
谢谢

lxx_sea_sky · 发表于 2009-4-10 16:37:51

精彩！

zht9961020 · 发表于 2009-4-10 23:12:28

【14楼】 sunsmile78

抱歉，pitolan的程序我没有看过，只看过德国的0.60版飞控程序，很多地方没看懂。
我也不知道mega644跑20M晶振运行卡尔曼滤波够不够，我是想用avr32做。

其实也不一定非用卡尔曼，用哪种算法都无所谓，只要能达到好的控制效果就是好的算法，德国人的程序没有用卡尔曼不是也飞的很稳吗？

我也是最近在论坛里看帖子才知道有卡尔曼滤波，以前连数字滤波器的概念都没有，现在也就是一知半解，程序也就是照猫画虎而已，初始的几个矩阵我都不太清楚是怎么得到的。反正基本也是抄的别人的，拿出来大家一起研究一下，谁搞明白了告诉大家一下，一起学习学习。

robinyuan · 发表于 2009-4-11 00:01:27

这个不错

主要要理解卡尔曼滤波的原理

傅立叶滤波也是不错的选择

happilzz · 发表于 2009-4-11 22:17:59

卡尔曼-好象是根据数据可信度分配权重的东东；
LS的傅立叶-好象是用来把时域数据换算成频域的东东。
貌似是完全不同的东西啊~~~

feng_matrix · 发表于 2009-4-16 14:09:58

顶楼主，不过为何PITCH轴与ROLL轴陀螺仪的灵敏度不同？用两种不同器件？
         roll_rate = gyro_x * 0.002250;
         pitch_rate = gyro_y * 0.0281230;

这句说明有一个轴硬件方向反了，如同我第一次用ENC-03时布的板子也是这样
         roll = atan2(accel_y, accel_z);
         pitch = atan2(-accel_x, accel_z);

你的KALMAN滤波初时化参数比较奇怪，系统多长时间运算一次？

用M32做十位精度的KALMAN滤波还勉强可以，精度再高些比如运算用DOUBLE型，堆栈可能就不够用了
以前我那个ENC-03版KALMAN滤波也是跑在M32上的，现在都改用AVR32了

zht9961020 · 发表于 2009-4-16 17:37:09

【19楼】 feng_matrix

1.为何PITCH轴与ROLL轴陀螺仪的灵敏度不同？

我的硬件只安装了roll轴的陀螺，使用的是EWTS82，25mv/（度/秒），计算后得到roll_rate = gyro_x * 0.002250;
“pitch_rate = gyro_y * 0.0281230;”是原帖子里的，因为没用到，所以忘了改了。

2.          roll = atan2(accel_y, accel_z);
         pitch = atan2(-accel_x, accel_z);
这里是根据mma7260的三轴加速度输出来计算横滚角和俯仰角，与陀螺无关。这里并不是硬件装反了，而是由mma7260的特性决定的。
如果四轴的十字支撑架x轴（前后俯仰轴）和y轴（左右横滚轴）分别平行于mma7260的x轴和y轴，并规定顺时针转角为正，则对应x轴和y轴的同样一个转角，accel_y 和accel_x必然符号相反。

3.你的KALMAN滤波初时化参数比较奇怪，系统多长时间运算一次？

初始化参数我没有修改，还是按照原帖子里的程序设置的

static  float A[2][2] = {{1.0, -0.019968}, {0.0, 1.0}};
static  float B[2][1] = {{0.019968}, {0.0}};

0.019968应该是采样时间间隔，我通过定时器每隔0.02秒运算一次

现在正在学avr32，也准备用它来做

feng_matrix · 发表于 2009-4-16 22:34:39

灵敏度与运放电路倍数有关，直接用原帖中可能会不合适的，KALMAN预测参数等也要调整
必须与自己系统客观实际相匹配，理论值是多少我也不会算，但可以通过实验得到较匹配的参数

zht9961020 · 发表于 2009-4-17 00:29:07

原帖子里的程序：
         // Determine gyro angular rate from raw analog values.
         // Each ADC unit: 3300 / 1024 = 3.222656 mV
         // Gyro measures rate: 2.0 mV/degrees/second ----------------灵敏度
         // Gyro measures rate: 114.591559 mV/radians/second
         // Each ADC unit equals: 3.222656 / 2.0 = 1.611328 degrees/sec
         // Each ADC unit equals: 3.222656 / 114.591559 = 0.0281230 radians/sec
         // Gyro rate: adc * 0.0281230 radians/sec
         roll_rate = gyro_x * 0.0281230; （我只把这给改了）
         pitch_rate = gyro_y * 0.0281230;

roll轴安装的是EWTS82 ，25 mV/degrees/second  ，mega16用外部3.3v参考电压,没有运放，直接进入adc，
与原程序的2.0 mV/degrees/second相比，灵敏度差了12.5倍，所以
roll_rate = gyro_x * 0.0281230/12.5=gyro_x * 0.002250;

feng_matrix · 发表于 2009-4-17 12:34:14

这个是陀螺仪灵敏度参数，比较好算的，难定的是KALMAN滤波参数，要试验才知道最终效果

还有0.25mV/d/s 信号如果不放大，直接用10位精度的M16片内ADC采样，效果可能不会好啊
10位ADC范围只有0－1023，如果3.3的Vref，差太远了吧

zht9961020 · 发表于 2009-4-17 12:57:11

不是0.25mv，是25mv

zht9961020 · 发表于 2009-4-17 12:59:42

这篇文章不错，可以参考一下：卡尔曼滤波器在嵌入式控制系统中的应用
点击此处下载 ourdev_437313.rar(文件大小:101K) (原文件名:卡尔曼滤波器在嵌入式控制系统中的应用.rar)

feng_matrix · 发表于 2009-4-17 13:00:18

哦，25mv，那输出电压应该还可以采样

coolwyc · 发表于 2009-5-4 17:32:37

请问楼主，有没有试过不用卡尔曼滤波，直接对ewts82积分，效果怎么样

moen · 发表于 2009-5-4 18:03:32

hen强~

zht9961020 · 发表于 2009-5-4 19:01:52

【27楼】 coolwyc

没试过，刚把程序在uc3b0256上跑通了，有时间试一下

zht9961020 · 发表于 2009-5-4 20:48:15

卡尔曼滤波效果图：
红线为卡尔曼滤波倾角输出，绿线为加速度计算的倾角

(原文件名:fd667.JPG)

(原文件名:rtyt.JPG)

(原文件名:饿.JPG)

zht9961020 · 发表于 2009-5-4 20:53:08

直接对ewts82积分效果不好，做横滚方向0-20度的往复转动
如图：红线为卡尔曼，绿线为直接积分

(原文件名:tuu.JPG)

harvard · 发表于 2009-5-4 22:36:55

to 【5楼】 zht9961020
倾角预测值:xhat_est
更新的倾角:xhat
xhat = xhat_est + K * Inn;

确实 Inn=y-c*xhat_est才符合逻辑。

zht9961020 · 发表于 2009-5-4 23:27:11

我程序里改成xhat_est了，但实验效果差不多。。。

coolwyc · 发表于 2009-5-5 11:45:24

请问楼上提到的kb6mcc“原帖子里的程序”在哪里下载？
         roll = atan2(accel_y, accel_z);
         roll_rate = gyro_x * 0.002250;
   kalman_output = kalman_update(roll_rate, roll);
如果我只需要单轴稳定，是不是只要一个单轴陀螺传感器和一个双轴加速度传感器就足够了？

zht9961020 · 发表于 2009-5-5 12:33:12

【34楼】 coolwyc

是“4轴姿态控制算法讨论贴”中的程序，你搜一下就能找到

如果我只需要单轴稳定，是不是只要一个单轴陀螺传感器和一个双轴加速度传感器就足够了？

是的

wear778899 · 发表于 2009-5-5 13:18:00

学习

mingyuexin1981 · 发表于 2009-5-5 13:20:11

学习~~

nuaa0303 · 发表于 2009-5-8 11:12:23

请问楼主用来显示波形的是什么软件啊,就是截图上有红线绿线的,用什么东西来显示的呢??

zht9961020 · 发表于 2009-5-8 21:40:39

我用c++ bulider写的小程序，从串口接收数据，然后用TChart控件绘图，水平比较菜，只能先接收一段时间的数据，然后绘图，没做动态波形显示

g921002 · 发表于 2009-5-8 22:21:38

在單晶片中，去做浮點矩陣運算很耗時的。用陀螺和加速規去做姿態估測的話，Kalman可以退化成最小二乘算法。而且因為pitch和roll估測沒有關係，所以不需要用到矩陣算法。這樣算法耗時就可以降低「一點點」。
另外用加速規算pitch和roll的公式是。

pitch_acc = atan((float)Ax/sqrt((float)((Ay*Ay)+(Az*Az))))*57.29578;
roll_acc = atan((float)Ay/sqrt((float)((Ax*Ax)+(Az*Az))))*57.29578;

原來譚上帖子的公式怪怪的。

zht9961020 · 发表于 2009-5-8 22:33:57

原帖子的公式我觉得没错啊，你给的公式sqrt((float)((Ay*Ay)+(Az*Az))部分不知道是什么意思，请给解释一下

g921002 · 发表于 2009-5-8 23:42:17

多說無益,直接看文件比快。
http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf 第四頁。
論壇上的公式在四軸上ok啦！但是用在飛機上會有問題。

lou0908 · 发表于 2010-5-22 22:44:15

牛人，有空研究一下

ottife · 发表于 2010-5-23 01:48:39

mark

fanwt · 发表于 2010-5-23 07:53:26

这个牛，学过数字信号，但是还没实际利用过，学习

ilawp · 发表于 2010-5-23 10:31:20

mark

jetshao · 发表于 2010-5-24 16:28:46

mark

gpzdc986 · 发表于 2010-5-24 20:22:26

mark

timom3 · 发表于 2010-5-25 17:59:17

这个要mark

y2911 · 发表于 2010-6-18 14:37:58

mark ,

hddgf · 发表于 2010-9-19 10:07:48

mark ,

spispispirit · 发表于 2010-9-22 21:07:38

mmmark

avrwoo · 发表于 2010-9-23 08:45:26

学习

chengluoran · 发表于 2010-9-23 09:12:14

mark

tongyf · 发表于 2010-11-15 15:00:58

学习，顶

cuikai12345 · 发表于 2010-11-15 18:51:23

mark

lxw2087290 · 发表于 2010-11-17 12:01:34

MARK，不知道AHRS模块里用的什么算法，是不是kalman算法？

denglu · 发表于 2010-11-17 23:37:48

Mark

tiancegis · 发表于 2010-11-26 15:53:57

不顶没道理

highnose · 发表于 2010-11-26 18:36:20

这算是很高的技术了

bondxie3 · 发表于 2010-11-30 13:39:46

标记学习

danielzhuang · 发表于 2010-11-30 18:16:50

学习了

lichunguang32 · 发表于 2010-12-6 12:30:46

学习，谢谢

lxw2087290 · 发表于 2011-2-4 19:02:42

mark

warmonkey · 发表于 2011-2-4 19:47:38

回复【18楼】happilzz
卡尔曼-好象是根据数据可信度分配权重的东东；
ls的傅立叶-好象是用来把时域数据换算成频域的东东。
貌似是完全不同的东西啊~~~
-----------------------------------------------------------------------

傅立叶滤波指的是频域上的滤波器，也就是我们常说的低通，高通，带通。。。
小波去噪则不算

whyjld · 发表于 2011-2-8 13:35:12

学习

zpwkxg · 发表于 2011-2-20 21:41:44

有借鉴意义！谢谢

zerodml · 发表于 2011-3-3 09:59:07

回复【楼主位】zht9961020
-----------------------------------------------------------------------

         gyro_x -= 732.3;
         gyro_y -= 440.5;

         accel_x -= 496.0;
         accel_y -= 565.0;
         accel_z -= 558.0;
是什么意思？卡尔曼初始化参数是怎么得来的？

zhu1982lin · 发表于 2011-3-4 10:19:14

mark一下,以后学习.

xiaona · 发表于 2011-3-4 14:06:25

请问楼主kalman_update函数中
static float Sz[1][1] = {{17.2}};
static float Sw[2][2] = {{0.005, 0.005}, {0.005, 0.005}};
这两个参数什么意思啊？

xiaona · 发表于 2011-3-4 14:13:35

还有matrix_subtraction((float*) APATSw, (float*) KCPAT, 2, 2, (float*) P);
这一步修改了P有什么作用呢？
谢谢了

blue_helmet · 发表于 2011-3-29 09:48:20

回复【70楼】zerodml
-----------------------------------------------------------------------

同样的问题，我也想知道这是什么意思~希望知道的高手能给了答复~

haoguoying · 发表于 2011-4-18 17:37:58

为什么我下载不了吗？谁能发我一份卡尔曼滤波计算和上面的源程序87867861@qq.com在这里先谢谢大家了

anthole · 发表于 2011-4-24 20:35:29

mark

jmp2002911 · 发表于 2011-4-25 15:40:57

mark 2

blue_helmet · 发表于 2011-5-3 16:35:24

回复【70楼】zerodml
-----------------------------------------------------------------------

我明白了，每次减去初始时刻的陀螺仪和加速度值，得到了变化的变量值。

635656623 · 发表于 2011-5-3 19:14:02

MARK

wcm_e · 发表于 2011-5-3 20:53:55

学习

codenull · 发表于 2011-5-16 14:50:20

学习呵呵研究中

fengleihu · 发表于 2011-6-16 00:48:18

回复【40楼】g921002
-----------------------------------------------------------------------

mark

xiaojia1203 · 发表于 2011-7-25 14:56:31

回复【5楼】zht9961020
-----------------------------------------------------------------------

讲解的非常到位，mark一下

sunicecream · 发表于 2011-7-25 15:15:43

MARK~~

cumt_123456 · 发表于 2011-8-26 14:14:46

感谢楼上的各位，小弟刚看，想请教几个问题
1、我若使用卡尔曼滤波的话，需要调整示例程序的哪些参数？
2、kalman_output = kalman_update(roll_rate, roll); 卡尔曼滤波的输入两个参量分别是什么？输出又是什么？

nter · 发表于 2011-9-6 15:29:37

mark

thisjoy · 发表于 2011-9-16 15:05:49

mark

liumaojun_cn · 发表于 2011-9-16 15:07:07

mark

myhonour · 发表于 2011-9-16 15:15:54

MARK

zjihtt · 发表于 2011-9-18 11:32:49

好贴，zht9961020 你学的好快啊

shaoyidong · 发表于 2011-9-18 16:35:30

mark

260186221 · 发表于 2011-9-28 00:13:46

回复【楼主位】zht9961020
-----------------------------------------------------------------------

楼主能不能把你的上位机也共享出来呢？？

oldbreadman · 发表于 2011-9-28 20:14:51

我来留个名,正准备用六轴(三轴加速度+三轴陀螺)做450直升机用的锁尾陀螺仪和平衡仪.

xiaoding818 · 发表于 2011-10-9 23:08:43

先mark一下，回头再仔细研究卡尔曼滤波。

xiaoding818 · 发表于 2011-10-14 09:27:29

仔细看了看，楼主例子讲解的不错，但是程序没给注释比较让人费解，虽然我没看过程序吧，从别人的提问猜的。to "cumt_123456"，第一问我不知道，因为还没看程序；第二问，kalman_output = kalman_update(roll_rate, roll)，很明显第一个参数是陀螺仪计算的倾角，第二个参数是加速度计计算的倾角，而此函数就是卡尔曼滤波函数，输出也就是最后滤波后的倾角值。
对于40楼g921002说的加速度计计算的倾角公式，其实公式确实是正确的，可是楼主的公式也是正确的，原因很简单，g921002给出的公式是三轴的公式，而楼主给出的是单轴的公式，且楼主测的应该也只是单轴估测而已，不会有三维的旋转。
个人理解，说错无怪哈，大家一起讨论。

xianxian314 · 发表于 2011-10-15 23:06:57

MARK 一下

hangyijun123 · 发表于 2011-10-23 15:44:15

楼主讲的很形象

215661599 · 发表于 2011-11-10 21:40:20

记号

killin · 发表于 2011-11-11 01:20:17

mark

kingboy100 · 发表于 2011-11-11 06:34:34

mark

p4s5j6 · 发表于 2011-11-11 07:59:45

mega16实现卡尔曼滤波计算俯仰角和横滚角程序

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