暴力测试IMU，卡尔曼滤波效果明显（有视频）
IMU用的是M32+ADXL330+ENC03，只做了俯仰和转动两个轴的测量，目前的想法是把这个模块加到小电直上，让其实现自主悬停，现在的情况是加到LAMA V4上，可以实现自平衡，呵呵，没有GPS做定位，所以只能是停在空中不摔机，但在空中位置漂的很厉害。现在实现的办法是，由ADXL330采得的加速度值算出倾角，同时由ENC03的角速率通过四元素法也算出一个转角，然后把这两个角度做卡尔曼滤波，相互修正。视频是用一个烂摄像头拍的，不是很清楚，而且做实验用的机子配置很低，运行软件多了有滞后，实际的情况比视频中的要好，视频中上一个通道显示的是俯仰，第二通道显示的是转动，每个通道中蓝色的线条显示的是卡尔曼滤波后的角度值，红色的线条显示的是加速度测出的角度值（不是很清楚，要仔细看才可以看到），可以看到，不管震动多大（红色的线条可以看出……），但通过卡尔曼滤波后，基本还是反应IMU的实际倾角。
加到LAMA V4上去的模块在测出倾角后，加了一个PID算法，控制俯仰和转动两个通道的舵机，呵呵，在室内的条件下，还是可以实现自主悬停的。下次再拍个视频给大家看看。
视频文件ourdev_375913.rar(文件大小:30.52M) (原文件名:yd.rar) lz牛人。。。。 最近我也在做这个实验， 用lpc2132+enc03+LIS3L02A现在实现的方法：由采得的加速度值算出倾角作为基础值，并由角速率作为补偿进行kalman。效果不是很理想。
由ENC03的角速率通过四元素法也算出一个转角。这个四元素算法有误差补偿吗？ 用的什么补偿方法？ 四元素法不是用来补偿的，而是如果用积分的办法求转角的话，在计算到接近90度的时候会出现退化现象，而且相比其它方法，四元素法的计算量低，四元素法只是算出一个转角，与误差补偿无关。具体理论可以参考科学出版社的一本《惯性导航》 楼主能分享下你的程序吗？
很想看看怎么回事 楼上别难为 lz 了，和lz讨论一些 理论倒是可以。
四元素法 确非误差补偿。我的意思是说， 你用四元数 求角度后，没有再做一些 误差补偿？ 四元数 求角度是有误差积累的 LAMA，楼主是否上5iMX的？ 我留个名 feng_matrix 是否加四元数 试验一下看看效果如何？ 强帖，学习了。 四元素法后，陀螺算出的角度是会漂移的，没有做其它的误差补偿，直接用卡尔曼滤波就可以补偿。 下一版的硬件有个温度传感器，用来做温度补偿，呵呵，这一个版本没有加。
回【7楼】 feng_matrix ，我不是5imx上的那个用LAMA V4做自动控制的人，呵呵，那个家伙也是个牛人啊，真是什么都会。 yuanding31 陀螺算出的角度 和加速度算出的角度谁占权重大些呢？
我看过芬兰的那款 mti感觉她的 陀螺算出的角度 占权重大些，角速度很容易超量程。（超量程输出就乱了）看你的视频 明显角速度超量程。 也来学习一下 我写的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');
图中，绿色的线是滤波后的结果，蓝色是加速度换算后的角度，红色是角速度积分结果。
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_543661.JPG
(原文件名:1.JPG)
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_543662.JPG
(原文件名:未命名.JPG) 回楼上：
Kalman filter的增益在数次迭代后，是趋于常值的。 K值的大小取决于驱动噪声Q和观测噪声R的大小，如果你需要滤波结果更依赖于观测量，那就调小R，增大Q。 反之，调大R，调小Q，这样估计值就取决于系统。 看了影片之後，輸出數據明顯落後。對於角度的估測明顯遲滯。至於對四軸的影響，需要實際飛試。
不過在這使用Kalman是有前提假設的，樓主知道嗎？ Kalman是最优滤波器是建立在系统模型是准确的前提下，而对于六自由度的IMU来讲，其更新的系统模型由于安装的误差，测量的误差等原因，能写进程序的模型并不准确。在后面的进一步的测量中我也发现了这一个问题。后面的AHRS，用了三轴ADXRS610，ADXL330加表，三轴磁强，用KALMAN做航姿估计，就发现，在陀螺过程更新与加表的测量更新误差比较大的时候，在KALMAN更新的时候会出错。而且由于KALMAN是用的是一个状态量，出错后更正要花比较长的时间。后面真正用到四轴上去的AHRS还是用的互补滤波器，现在在上海机场，马上要登机了，回家后再开一帖和大家再继续讨论。 多谢【16楼】 litao8421 的回答，我也是这么想的。
但我不理解，为什么楼主附件里的视频，在剧烈摇晃下，滤波后的结果并没有出现很多的震荡，换言之，就是，楼主的卡尔曼滤波算法能够实现自动调整增益K，从而消除加速度 传感器受外力干扰后的测量换算值不会对卡尔曼滤波最后的结果造成错误影响。就像论坛里的另一个贴，AHRS的演示，视频中，倾角仪被人按在桌面来回推移，这是，角速度应该是基本为0，加速度传感器受干扰后，换算的结果肯定不会显示倾角为0，但视频中电脑显示屏里的结果却相当理想，基本没有受加速度传感器的影响而显示错误的结果（倾角仪倾角不为0）。
在我的matlab仿真中，却做不到这点，滤波程序无法根据角速度为0，来区别加速度的错误。
难道一定要2个以上的轴的信息组成矩阵运算，才能消除加速度受外力干扰的错误吗？ 那倒是不需要，如果是很明显的扰动，可以通过blunder detection的方法，去掉他的影响。 比如上面你的例子里，你的系统方程相当于是静止时的方程，通过检查innovation sequence，可以判断出你的观测量明显和系统的预测量不符（统计意义上的），这是这个观测量就被reject掉，这时系统的估计就不受这个扰动量影响了。
另外大部分的Kalman filter是固定增益的。 除非使用adaptive的方法，调整观测噪声或者驱动噪声，这个时候的Kalman gain是变化的。
但是工程上发现，adaptive的结果不见得比constant gain的好。 而网上大部分的paper都是以仿真为基础的，那个不具备说服力。在实践中，还是constant gain的比较稳定。 litao8421
请问，就我建立的这个方程，要怎么样能达到楼主视频里的那种效果呢？你说的blunder detection要怎么做呢？难道楼主的算法里也是用blunder detection?
是不是我的一维的系统方程就不对？ 发现这么久的帖子还有被翻出来的，想一想做了这么长时间的飞控相关的东西，也很有感触。呵呵，这样吧，程序放上来吧，就是这个帖子所说的这个版本的程序，对加速度和两轴陀螺做了ＫＡＬＭＡＮ，litao8421说的对，是固定增益的。当中也没有使用四元数，因为当初是给固定翼飞控上玩的。后面还有相关的ＰＩＤ控制上的程序。大家看程序时对有的参数从哪设的可能不是很清楚，那是因为，所有的参数都是通过无线数传电台设的，但通信协议上有些问题，因为地面站不是我做的，所以协议还是不公开的好。这也只是一个比较简单的ＫＡＬＭＡＮ应用，后面用到飞控上的要复杂不少。近来在整理算法上的材料，过几天差不多了，重开一个算法上的讨论帖子吧，到时候请feng_matrix 悟和 cnmusic等几个真正自己研究算法的一起讨论一下，要做到工业水平，我们的四轴在算法上怎么构架合适。
程序，编译平台winavrourdev_544174.rar(文件大小:247K) (原文件名:apQuard.rar) 呵呵，几年前做的东西，现在看起来，也很不成熟。大家当笑话看一下就行了，觉得有用的话，就看一下，如果是觉得太烂的话，也请高抬贵手啦，具体更深的研究我想这几天再和大家一起讨论一下。 这个帖子要顶!!! 回21楼，简单一点的方法是，检查Z-Hx，这里的Z-Hx就是innovation sequence了，用这个和你的R比较，超过的话，认为是blunder。当然有稍微复杂的方法。比如根据置信度，虚警，R来确定blunder的门限。那个相对可信度高一些。 各位可以再回去看看視頻，板子轉動時。畫面中角度的曲線明顯延遲。
也就是意味著載具角度產生變化時，uC要經過一段時間才知道角度發生變化。<--不過我不知道這是拍攝的關係還是真的這樣。
如果覺得這樣的Delay可以接受，那就用吧....如果樓主在四軸上測過OK...那就OK囉！
如果載台夠穩定，飛控用PID就綽綽有餘了。 感谢litao8421的指导，但我看了楼主的代码后，发现他并没有采取处理“妖风”的措施啊，不知道楼主视频中的剧烈平移摇晃为何没有影响最后的滤波结果？效果很好啊，我写了个跟楼主给出的代码一样的模型，主要也是参考这里：http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_545395.jpg
(原文件名:QQ截图未命名1.jpg)
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_545396.jpg
(原文件名:QQ截图未命名2.jpg)
但我仿真的时候，就发现那个dt前面的负号如果不去掉的话，就会出现这样的仿真曲线：
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_545397.jpg
(原文件名:QQ截图未命名3.jpg)
去掉dt前的那个负号时，才想那么回事：
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_545398.jpg
(原文件名:QQ截图未命名4.jpg)
以上曲线图中曲线代表滤波后的结果，这个仿真数据是含有“妖风”的仿真数据，理想结果应该是一条直线，因为我仿真的是没有转动的时候，受到“妖风”影响，整个物体平移。 大家牛呀，我最近要做一个6轴的飞行器算法实现，硬件我已经做完了，算法还不会。
哈哈 顶起来，等楼主回答 我认为那个模型没有问题。
我们构建一个系统状态量： x = [ angle, bias ]'，表示某一轴的角度值及其偏差。
其系统状态方程可以表示为:
* x = [ angle, bias ]'
*
* = F x + B u
*
* = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
*
* => angle = angle + dt (dotAngle - bias)
* bias= bias
dotAngle为角速率。可以看出，下一时刻的角度等于当前角度加上去偏差的角速度与两次时间间隔的乘积。这是没有错的。
而下一时刻的角度偏差用这一时刻的偏差来更新也是没有错的。
所以，我认为系统模型没有什么问题。
另外我要讲一点，kalman滤波器并不是万能的，也并不是说，来了一阵持续时间很长的妖风，Kalman仍然可以有效的工作。
Kalman是建立在系统模型准确的情况下才是最优滤波器，而本系统模型在出生之时就被定义了误差是白噪声，均值为0。
如果妖风来的太久，显然就不是均值为0了。这样，滤波出来的值会慢慢的向测量值逼近。
而Kamlan在本例中最本质的还是信息融合。它把陀螺的数据应用于系统的状态方程里，这个更新叫做系统状态更新。
这就是上面的系统状态方程所做的事。
与此同时，它还更新了系统的预测方差矩阵P
* P = F P transpose(F) + Q
*
* = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
*
*P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt* P(1,1) + Q(0,0)
*P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
*P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
*P(1,1) = P(1,1) + Q(1,1)
这个时候，X所预测的值是通过状态方程估计出来的，到底准不准确还不知道。所以，在此，我们引入了一个测量值，
来检测我们的测量是否准确。这个测量值我们用加速度来得到：atan2(ac_x, ac_z)或atan2(ac_y, ac_z)
但我们都知道，加表在运动下的测量值是不准的。这个时候，我们就要用一些先验的知识来对两个值要融合的权值做
一个计算。这个先验知识就是R：测量噪声协方差和Q：系统状态噪声协方差。
令y为测量值与系统状态估计值之间的误差：
y = z - H x （H为测量值到观测量之间的传递矩阵）
而K为测量值对状态量的修正增益：
*S = H P transpose(H) + R
* = [ 1 0 ] P [ 1, 0 ] + R
* = P(0,0) + R
*
*K = P transpose(H) S^-1
* = [ P(0,0), P(1,0) ] / S
对X修正：
*x = x + K y
然后再计算系统的误差协方差矩阵：
*P = (I - K H) P
*
* = ( [ 1 0, [ K(0),
* 0 1 ] - K(1) ] * [ 1 0 ] ) P
*
* = [ P(0,0)-P(0,0)*K(0)P(0,1)-P(0,1)*K(0),
* P(1,0)-P(0,0)*K(1)P(1,1)-P(0,1)*K(1) ]
整个过程中可以看出来，对X的修正增益K与R和Q值都有关系。如果R大Q小，就是说，状态估计值比测量值要可靠，这时，所得出的结果
就是更接近估计值。如果R小Q大，这时，计算出来的结果就会更接近测量值。
实际上，在飞行器的运行中，因为振动和环境影响，加表的值可信度是很低的，而陀螺受振动的影响要小的多。所以，一般Q值会选的比
较小，而R值会选的比较大。但振动以及妖风的影响不可能是长时间向某一个方向来的，总的来说，加表测出来的倾角均值为0.这样，就
会给陀螺提供一个基准，因为陀螺是有积累误差的，这个误差随着时间的推移而变大发散。而加表长时间的基准就是为了对陀螺的积累误差
做一个修正。这样来看的话，可以看做对加表做了个截止频率很低的低通，用以保证基准；而陀螺做了一个截止频率很低的高通，用以保证
响应度。
其实这些工作也可以用互补滤波器来做，德国人的四轴就是用互补来做的。效果也可以。
这些天太忙，有项目要验收，没有来得及回复。抱歉。另外，我也用互补滤波器做了一个，感觉比用这个Kalman还要OK一点。过两天再传
视频。
这是我自己的理解，因为我本身不是控制出身的，这块肯定有不当之处，欢迎拍砖！！ 另外，我刚才看了看你原先仿的那个程序，程序本身没什么问题，只是你对系统的认识有点不对，一般来说，陀螺的方差很小，它更麻烦的是积累误差。所以，把原先你的程序里陀螺的方差改小，把加表的方差改大就行了。我改了一下试了试。
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_546229.jpg
(原文件名:kalmanft.jpg)
这样，滤波的效果就出来了。 mark！ 强烈感谢楼主的回答，真是解了我的心头结。我还有个问题，就是bias是否就是角速度测量值里的白噪声？
那在这个方程里，是固定值，还是要实时更新呢？（我觉得，仿真时还可以实时更新，但实际做的时候，就没办法实时更新了）
* x = [ angle, bias ]'
*
* = F x + B u
*
* = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
其实，我也试很多建模方程来仿真，结果都是无法去除长时间“妖风”的影响。我的士气受到强烈打击。
原来真的如我所怀疑的那样，真是卡尔曼本身的能力有限，没法滤除长时间“妖风”。这下，我可以进行下一步工作：搭建硬件平台了。
希望我能达到楼主的效果，“暴力摇晃也没问题”。
另外，其实我也想过，采用窗口滤波类似的方法，把每一小段时间的测量值进行预处理，动态计算出每个小段时间内的测量值（角速度值以及加表换算后的角度值）的均值，以及方差，这样再通过卡尔曼滤波，可能效果会好些。
我最终是想采用基于ARM平台的东西，所以，存贮空间以及计算能力，应该都不成问题，至少比8为AVR要强很多。
今晚仿真再试试。 今晚，有在原有卡尔曼滤波的基础上，加入了窗口平滑滤波，就是现将z(k)以8个为一组求平均，然后讲平均后的值加入到卡尔曼滤波里，图中黑线便是仿真结果。绿线为没有窗口平滑滤波是的卡尔曼结果，蓝线代表z(k)，红线代表角速度直接积分结果。
clear all;
N=1000;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
;%% u0表示物体真实的转动角速度，
for k=1:N
u0(k)=0
z0(k)=20+u0(k);%% 物体的稳定倾角
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 物体匀速转动
%u0=0.1;
%z0(1)=0;
%for k=2:N
% z0(k)=z0(k-1)+u0;
%end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 物体变速转动
%u0(1)=0;
%for k=2:N
% u0(k)=u0(k-1)+0.0001;
%end
%z0(1)=0;
%for k=2:N
% z0(k)=z0(k-1)+u0(k);
%end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 模拟ENC-03所测得的数据，（角速度）,数据分布在top1*(-0.5~+0.5)之间,
top1=20;
for k=1:N
w(k)=top1*(0.5-rand);
u(k)=u0(k)+w(k);%%u0(k)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 模拟物体稳定在某一个倾角时，加速度传感器所测得的数据，假设稳定倾角为z0,数据分布在top2*(-0.5~+0.5)之间
%% z(k)表示重力加速度在物体的Z轴比X轴，即k时刻倾角的正切值，
%top2=2;
%for k=1:N
% v(k)=top2*(0.5-rand);
% z(k)=atan(tan((z0(k)+v(k))*pi/180))*180/pi;%%z0
%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=1;
az=a*sin(z0*pi/180);
ax=a*cos(z0*pi/180);
top2=5;
for k=1:N
v(k)=top2*(0.5-rand);
z(k)=atan(tan((z0(k)+v(k))*pi/180))*180/pi;%%z0(k)
end
for k=200:350
v(k)=top2*(0.5-rand);
z(k)=v(k)+atan(((gx+ax)/(gz+az)))*180/pi;%%z0(k)
end
for k=500:650
v(k)=top2*(0.5-rand);
z(k)=v(k)+atan(((gx+ax)/(gz+az)))*180/pi;%%z0(k)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q=1e-5%std(u).^2
R=0.1;%std(v).^2
xx(1)=0;
x(1)=20;
%x(2)=0;
%pp(1)=0.01;
p(1)=1;
A=1;
B=0.01;
H=1;
gate=0.01;
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);
change(k)=t(k)*(z(k)-H*xx(k));
x(k)=xx(k)+change(k);
p(k)=(1-t(k)*H)*pp(k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
y(1)=z(1);
for k=8:N
y(k)=(z(k)+z(k-1)+z(k-2)+z(k-3)+z(k-4)+z(k-5)+z(k-6)+z(k-7))/8;
end
for k=2:N
%预测
xx(k)=A*v(k-1)+B*u(k-1);
pp(k)=A*p(k-1)*A+Q;
%更新
t(k)=pp(k)*H/(H*pp(k)*H+R);
change(k)=t(k)*(y(k)-H*xx(k));
v(k)=xx(k)+change(k);
p(k)=(1-t(k)*H)*pp(k);
end
o2(1)=20;
for k=2:N
o1(k)=z(k);
o2(k)=o2(k-1)+B*u(k);
o3(k)=x(k);
end
figure(1);
k=1:N
plot(k,o1,'b',k,o2,'r',k,o3,'g',k,v,'k');%%
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_546444.jpg
(原文件名:QQ截图未命名.jpg) 顶哇，强 请高人指正。
今晚，又有个新想法，仿真了一下，基本思想是把Q，R变成动态的，
每8个z(k)加速度换算的角度值为一组，计算其方差，设为QQ，
每8个u(k)角速度值为一组,计算其方差，设为RR，将QQ，RR乘以系数后替换原卡尔曼滤波公式中的Q,R。
通过多次仿真实验调整系数，最终得到QQ的系数为0.00000001，RR的系数为0.1。
我也不知道这样做的理论依据是什么，请高人指导。
最后得到了如下仿真结果：
绿色是原来的卡尔曼仿真结果，
黑色是现在新的仿真结果，
红色是角速度测量值直接积分结果，
蓝色是加速度计测量值换算得到的角度值。
http://cache.amobbs.com/bbs_upload782111/files_28/ourdev_546648.jpg
(原文件名:QQ截图未命名.jpg)
for k=8:N
y(:,k)=;
s(:,k)=;
end
for k=8:N
E1(k)=mean(s(:,k));
E2(k)=mean(y(:,k));
QQ(k)=std(s(:,k)).^2;
RR(k)=std(y(:,k)).^2;
%预测
vv(k)=A*v(k-1)+B*u(k-1);
pp(k)=A*p(k-1)*A+QQ(k)*0.00000001;
%更新
t(k)=pp(k)*H/(H*pp(k)*H+RR(k)*0.1);
ch(k)=t(k)*(z(k)-H*vv(k));
v(k)=vv(k)+ch(k);
p(k)=(1-t(k)*H)*pp(k);
end 这个应该鼓励一下 顶，做个记号，慢慢学习^_^ 回复【33楼】electronicYH
-----------------------------------------------------------------------
我觉得bias并不是陀螺仪的高斯噪声,而是漂移量,而且正是因为把这个漂移作为一个状态进行迭代才能使陀螺仪积分不发散
比如描述一个汽车做直线运动,跟这里做一个对比,汽车当前位置对应于这里的角度,汽车的速度对应于陀螺仪输出,那么由于道路上的小石头或者坑坑洼洼造成的速度噪声就是高斯噪声,也就是进程噪声,然后我们有一个GPS可以直接得到汽车的位置,但是GPS有误差,那么这个误差也是高斯噪声,也就是测量噪声,此时假设汽车仪表有问题,导致读出来的速度缓慢增大,但是实际上速度没有变快,那么这个变化才是bias,于是把汽车位置和bias作为状态量进行迭代,用来估计汽车的位置和仪表的漂移程度
不知道这么理解对不对呢 回复【39楼】lijieamd
-----------------------------------------------------------------------
谢谢你的回答，但我还没看明白，这个bias如果是漂移的话，那个白噪声是什么关系？ 陀螺仪的噪声是飞行器机体抖动,所以陀螺仪输出的抖动，也就是系统的进程噪声
加速度计的噪声是由于飞行器的运动不确定性造成有运动加速度影响倾角计算,也就是测量噪声
这两个噪声我们都认为是高斯噪声，也就是期望是0
而陀螺仪漂移的期望往往并不是0,所以在这里把它当做状态来估计
我也不知道这么理解是不是正确的,望高手来解答 一般做误差模型的时候，我们说姿态的误差通常有两项或者说近似为两项，一个是有陀螺的宽带噪声引起的，一个是由陀螺的drift引起的。宽带噪声理想情况下，就建模成白噪声。drift一般建模成一阶马尔科夫。 回复【42楼】litao8421
-----------------------------------------------------------------------
您说的drift就是公式中的bias吗？ 加速计的一般叫bias，陀螺的叫drift。 请问楼主：你是用什么方法观测IMU系统输出的啊？我们近期也在自己的IMU中加了一些算法，想验证一下效果，向您请教了，不胜感激。 回复【44楼】litao8421
-----------------------------------------------------------------------
陀螺有bias和drift兩種誤差。 陀螺是有turn-on bias,这类的误差，但是一般被calibrate掉了，并不估算这个东西。 顶起来，等待楼主的互补滤波算法的效果演示 楼主好，按照你的例程进行滤波，发现延时非常严重，请问是啥原因？谢谢。 强帖留名 延时过大应该考虑一下，每次滤波之间的时间间隔是多少，然后你更新时设的时间常数是不是和这个一致；然后就是两个方差的设置，如果方差选的不合适的话，滤波效果就会很差 讨论算法值得肯定！
不过楼主应用在LAMA V4上，不免有些画蛇添足，它已经是非常稳定的飞行器了，它靠平衡锤和自身吊篮效应已经能够自主悬停。还用飞控干嘛？建议楼主换个450控一下。 要好好学习一下！ 留个记号慢慢看 mark mark 强帖留名 mark 做个记号，学习 学习 口水！！ mark 记号 用加速度传感器来进行数据融合，在低动态情况下可行。但是在高动态环境下呢？还有就是卡尔曼滤波虽然滤掉了大家认为是噪声的信息，但是这种信息不一定是无用的。在特定的运动状况下，会滤掉有用信息。 而且大家都知道，对于MEMS的加速度传感器来说，零漂和温漂都很厉害，温漂可以补偿，但是零漂呢？ 做现场标定也会因为，IMU放的不水平，零点标不准，无法提供有效的初始姿态矩阵。
实际我认为，解决这种航模机的IMU的关键问题是，采用更加科学的MEMS加速度传感器现场标定技术。建议用**标定算法，这个可以达到1/100精度转台的水平。 比较难 mark 这个帖子好长，mark一下，等会回来好好看 学习…… mark.以后学会卡尔曼滤波再来仔细看. mark 我是新手，刚接触陀螺仪，用的是NEC03。如果只有陀螺仪没有加速度计，怎么对角度积分和滤波？另外，nec03的采样频率多大比较合适？ 学习了，添加收藏夹，慢慢品味！ 留个记号 mark mark 回复【51楼】yuanding31
-----------------------------------------------------------------------
楼主的帖子貌似很早了，不知现在还关注不？认真细读了一遍，也看过了楼主发的代码，还是有一点不懂。
1.关于bias这个值，我看英文说得是无旋转时陀螺仪的输出。bias是有上一次加速度计输出计算得到的吗？
2.Q既然是系统误差协方差矩阵，那么P_11 +=Q_gyro * dt；P_00 +=- dt * (P_10 + P_01) + Q_angle * dt;
此处的Q_gyro * dt和Q_angle * dt又怎么理解？
3.陀螺的方差很小，它更麻烦的是积累误差。加表的方差大。关于方差的设置是经验值吗？还是使用不同的kalman公式
参数就需要从新设？
4.这个不算问题，很期待楼主的互补滤波算法。 mark~ 卡尔曼33333333333333333333444444444444444444 先留个名记录一下. 虽然不懂,敬仰一下各位高人 学习了。 lz这里的“自主悬停”是 没有 遥控器参与么？ mark 回复【15楼】electronicYH
-----------------------------------------------------------------------
你的kf好像是错的。请看下相关书籍 mark 好贴，牛人 mark 慢慢学习了！！ 高人，mark一下。性能很不错 好贴，向牛人们学习 果断看。
回复【楼主位】yuanding31
----------------------------------------------------------------------- 顶一个 回复【34楼】electronicYH
-----------------------------------------------------------------------
感觉效果是一样的，没有太大变化。 * X = [ angle, dw]'
* X = F X + w
* = [ 1 dt, 0 1 ] [ angle, dw] +
那么预测：
* => angle = angle + dt*dw
* dw= dw (因为实际情况中，短时间里，变化很小)
* Z = H X + v
= [ angle, dw] +
注：dw是采样时间段内的角速率，观测angle和dw
大家说说这个模型会不会更科学些？？？ 再将用四元素法解得的角度与上面所得的角度作信息融合。
式如：
X = '
X = F X + BU + w
= ' + ' angle_q +
Z = H X + v
= '+R_
./emotion/em192.gif 牛人 mark~ mark 这个号犀利啊
页:
[1]
2