|
|
点击此处下载 ourdev_697563NP6J4S.rar(文件大小:109K) (原文件名:示波器数据.rar)
% 建模数据；
% 陀螺仪选用ADXRS150，灵敏度为15mv/°/s；
% 采样点个数 N=2500；
% 采样周期 T=0.002s；
% angle_KF 经过滤波后的角度；
% velocity_KF 经过滤波后的角速率；
% Y=[angle_KF(n);velocity_KF(n)]；
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=[0;0.01]; % 系统状态初始值；
Y(:,1)=Y0;
A=[1,T;0,1]; % 系统状态转移矩阵；
B=[1/2*(T)^2 T]'; % 噪声输入矩阵；
H=[1 0]; % 观测阵；
C0=[0,0;0,1]; % cov 初始值；
C=[C0 zeros(2,2*2499)];
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([0 5 -0.1 0.2]);
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');
(原文件名:图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');
(原文件名:图1.jpg)
示波器波形图：
(原文件名:示波器波形图.JPG) |
阿莫论坛20周年了！感谢大家的支持与爱护！！
知道什么是神吗？其实神本来也是人，只不过神做了人做不到的事情 所以才成了神。 （头文字D, 杜汶泽）
|
发表于 2011-11-21 16:57:45
楼主
