|
|
% 建模数据；
% 陀螺仪选用ADXRS150，灵敏度为15mv/°/s；
% 采样点个数 N=2500；
% 采样周期 T=0.002s；
% angle_KF 经过滤波后的角度；
% velocity_KF 经过滤波后的角速率；
% t=0.002:0.002:5; % 共5秒时间；
% T=0.002; % 采样周期；
% X(k) = AX(k-1) + Bw(k-1);
% X(k) = [angle_KF(k);velocity_KF(k)];
% Z(k) = HX(k) + v(k);
% X(k) = [angle(k);velocity(k)];
A = [10,0;0,1];
B = [1;0.1];
H = [1,0;0,1];
Q = 0.05;
R = 0.05;
T = 0.002; %采样周期0.002秒
N = 2500; %采样点个数：2500
load data2 data2; % 陀螺仪读数（电压值），1*2500
test_velocity_vol = data2;
velocity_vol = test_velocity_vol - (2.5 * ones(1,N)); %陀螺仪零点电压约为2.65V
velocity(1) = velocity_vol(1) / (15/1000); % 角速率初值
angle(1) = velocity(1)*T; % 角度初值
for n = 2:N;
velocity(n) = velocity_vol(n) / (15 / 1000);
angle(n) = angle(n-1) + velocity(n)*T - 0.02; %这里-0.02和不减差距很大，具体见图片
end
status = [angle;velocity]; % 系统状态，即观测值
X = zeros(2,N);
X(:,1) = [angle(1);velocity(1)];
C0 = [10,0;0,10];
C = [C0 zeros(2,2*(N-1))];
for n = 1:N;
m = (n-1)*2+1;
X(:,n+1) = A*X(:,n); %自己写的卡尔曼算法
C(:,m:m+1) = A*C(:,m:m+1)*A'+B*Q*B';
Kg = C(:,m:m+1)*H'*inv(H*C(:,m:m+1)*H'+R);
X(:,n+1) = X(:,n)+Kg*(status(:,n)-H*X(:,n));
C(:,m+2:m+3) = (eye(2,2)-Kg*H)*C(:,m:m+1);
% Kg = C(:,m:m+1)*H'*inv(H*C(:,m:m+1)*H'+R); 网上程序的卡尔曼算法
% X(:,n) = X(:,n)+Kg*(status(:,n)-H*X(:,n));
% X(:,n+1) = A*X(:,n);
% C(:,m:m+1) = (eye(2,2)-Kg*H)*C(:,m:m+1);
% C(:,m+2:m+3) = A*C(:,m:m+1)*A'+B*Q*B';
end
figure(6);
t = 0.002:0.002:5;
angle_KF = X(1,2:2501);
velocity_KF = X(2,2:2501);
plot(t,angle_KF,'r',t,angle,'b',t,velocity_KF,'m',t,velocity,'g');
legend('angle after KF','angle','velocity after KF','velocity');
grid;
xlabel('time');
ylabel('four status');
title('KF VS NoKF');
程序思维：首先获取2500组ADXRS150的数据，然后算出其角速率和对应的角度变化，并作为卡尔曼滤波的观测值：
load data2 data2; % 陀螺仪读数（电压值），1*2500
test_velocity_vol = data2;
velocity_vol = test_velocity_vol - (2.5 * ones(1,N)); %陀螺仪零点电压约为2.65V
velocity(1) = velocity_vol(1) / (15/1000); % 角速率初值
angle(1) = velocity(1)*T; % 角度初值
for n = 2:N;
velocity(n) = velocity_vol(n) / (15 / 1000);
angle(n) = angle(n-1) + velocity(n)*T - 0.02; %这里-0.02和不减差距很大，具体见图片
end
再就是进行滤波，其初值设置如下：
A = [10,0;0,1];
B = [1;0.1];
H = [1,0;0,1];
Q = 0.05;
R = 0.05;
T = 0.002; %采样周期0.002秒
N = 2500; %采样点个数：2500
图一是减去0.02的滤波效果图：
(原文件名:图1.jpg)
图二是没减0.02的滤波效果图：
(原文件名:图2.jpg)
不知道这样的效果是好是坏，希望有经验的大牛给点意见与建议，以欢迎大家共同讨论，还有那个0.02是否就约等于陀螺仪的零漂？ |
阿莫论坛20周年了！感谢大家的支持与爱护！！
知道什么是神吗？其实神本来也是人，只不过神做了人做不到的事情 所以才成了神。 （头文字D, 杜汶泽）
|
发表于 2011-11-23 20:16:07
楼主
