|
|
发表于 2011-11-23 13:37:38
|
显示全部楼层
% 建模数据；
% 陀螺仪选用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；
load data2 data2;% 陀螺仪读数（电压值），1*2500；
test_velocity_vol = data2;
temp = ones(1,2500);
temp = temp * 2.7;
velocity_vol = test_velocity_vol - temp% 陀螺仪静态输出2.5V；
velocity = velocity_vol / (15 / 1000); % 陀螺仪角速率；
angle(1) =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.5;2]; % 系统状态初始值；
Y(:,1)=Y0;
A=[1,T;0,1]; % 系统状态转移矩阵；
B=[1/2*(T)^2 T]'; % 噪声输入矩阵；
H=[1 1]; % 观测阵；
C0=[0.1,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(6)
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');
下面这个图是滤波效果，有个疑问：陀螺仪是手拿着来回旋转的，为啥积分后的角度输出始终增大呢？
(原文件名:最新.jpg) |
|
楼主: feng_matrix
