卡尔曼算法，ADXRS150陀螺仪数据处理，以及滤波前后波形对比
点击此处下载 ourdev_697563NP6J4S.rar(文件大小:109K) (原文件名:示波器数据.rar)% 建模数据；
% 陀螺仪选用ADXRS150，灵敏度为15mv/°/s；
% 采样点个数 N=2500；
% 采样周期 T=0.002s；
% angle_KF 经过滤波后的角度；
% velocity_KF 经过滤波后的角速率；
% Y=；
t=0.002:0.002:5; % 共5秒时间；
T=0.002; % 采样周期；
load test_velocity_vol test_velocity_vol;% 陀螺仪读数（电压值），1*2500；
temp = ones(1,2500);
temp = temp * 2.5;
velocity_vol = test_velocity_vol - temp; % 陀螺仪静态输出2.5V；
velocity = velocity_vol * (15 / 1000); % 陀螺仪角速率；
angle(1) = 0.05; % 陀螺仪初始位置，旋转角度假设为0；
for n=2:2500;
angle(n) = angle(n-1) + velocity(n)*T;% 在迭代过程中实际角度
end
save angle angle
save velocity velocity
load angle angle; % 陀螺仪积分得到的角度，未滤波；
load velocity velocity % 陀螺角速率，未滤波；
T = 0.002; % 采样周期；
Y=zeros(2,2500);% 初值为0；
Y0=; % 系统状态初始值；
Y(:,1)=Y0;
A=; % 系统状态转移矩阵；
B='; % 噪声输入矩阵；
H=; % 观测阵；
C0=; % cov 初始值；
C=;
Q=(0.25)^2; % 过程噪声；
R=(0.25)^2; % 观测噪声；
% 卡尔曼算法；
for n=1:2500
i=(n-1)*2+1;
K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
Y(:,n)=Y(:,n)+K*(angle(:,n)-H*Y(:,n));
Y(:,n+1)=A*Y(:,n);
C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end
figure(1)
t=0.002:0.002:5;
angle_KF=Y(1,2:2501);
velocity_KF = Y(2,2:2501);
axis();
plot(t,angle_KF,'r',t,angle,'b',t,velocity,'g');
legend('angle after KF','angle','velocity');
grid;
xlabel('time');
ylabel('four status');
title('KF VS NOKF');
http://cache.amobbs.com/bbs_upload782111/files_47/ourdev_697565SPXPKQ.jpg
(原文件名:图2.jpg)
figure(2)
t=0.002:0.002:5;
plot(t,angle_KF,'r',t,angle,'b',t,velocity_KF,'c',t,velocity,'g');
legend('angle after KF','angle','velocity after KF','velocity');
grid;
xlabel('time');
ylabel('four status');
title('KF VS NOKF');
http://cache.amobbs.com/bbs_upload782111/files_47/ourdev_697564R54LL8.jpg
(原文件名:图1.jpg)
示波器波形图：
http://cache.amobbs.com/bbs_upload782111/files_47/ourdev_697566EHLTZP.JPG
(原文件名:示波器波形图.JPG) 在示波器图片中，紫色的为陀螺仪数据波形，绿色为磁场传感器一轴的波形，由于硬件还未完全做完，所以没能将加速度波形传上来。各位大侠，给点意见哈，诚心讨论学习！ 上面程序中陀螺仪零漂设的为0.05，若设置为0时，波形如下：
http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_697578O7X34V.jpg
(原文件名:陀螺仪零漂为0时波形.jpg) 在图2和2楼的图片，对比情况下：当陀螺仪的零漂不为0时，其输出经过积分后大约为0.08度，但是当其零漂继续增大时：
http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_697581EGJUHY.jpg
(原文件名:陀螺仪零漂为1时波形1.jpg)
http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_697582QJEZSD.jpg
(原文件名:陀螺仪零漂为1时波形2.jpg) 其积分误差明显增大，不过最终，实测角度和滤波后的值有向相同大小变化的趋势，若能确定陀螺仪的零漂，在算法中加以补偿，效果应该还是可以的。 Mark一下 这里有个问题：在假定陀螺仪零漂时，我是直接用输出角度对其量化的，不知道这样会不会有问题哈。 回复【5楼】rqbh
mark一下
-----------------------------------------------------------------------
./emotion/em065.gif./emotion/em065.gif./emotion/em065.gif./emotion/em065.gif./emotion/em065.gif./emotion/em065.gif./emotion/em065.gif mark mark MARK 哈，才发现有个错误哦！
http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_698263COZWJI.jpg
(原文件名:错误.jpg)
应该是这样的哈：velocity = velocity_vol / (15 / 1000); % 陀螺仪角速率；就说老感觉有点不对劲撒！ 还有陀螺仪的零点大约为2.7V，不是2.5V哦 http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_698264EOP6CT.jpg
(原文件名:最新.jpg)
这个才是，不知道怎么样，好像积分后输出的角度并没有正负变化，而是一直往正或者负增加呢！！！ 大牛们，给点意见啊，是不是陀螺的零漂还是没抵消完的原因？ MARK mark~ mark，学习中~ 我覺得就是陀螺儀零點漂移或是其他加速度運動造成陀螺儀干擾的問題 学习一下 你的速度估計可能存在著極小的偏移,可能是零點的問題,如果沒有修正,積分的結果就是那樣,一次多項式,往正或往負,直線上升 mmmmmmmmm MARK 恩，仿真的效果比较好，不过在实际的过程中，难点在于Q（激励干扰信号方差的）测量；不良的初值可通过增大迭代次数的来纠正，但方差值设置错误，最后结果可能一直不理想。 这两天正在整陀螺仪，看到希望了！！ 请问你的虚拟示波器是什么软件？能分享我用下吗? 回复【25楼】newhand1991
请问你的虚拟示波器是什么软件？能分享我用下吗?
-----------------------------------------------------------------------
同问 回复【26楼】3466756555
回复【25楼】newhand1991
请问你的虚拟示波器是什么软件？能分享我用下吗?
-----------------------------------------------------------------------
同问
-----------------------------------------------------------------------
看样子这不是虚拟示波器，是MATLAB仿真的波形。 回复【楼主位】zerok
-----------------------------------------------------------------------
正好需要，顶
页:
[1]