从加速度数据如何获取位置数据获得IMU的吗?
7视图(30天)
显示旧的评论
我有附加硬件加速和辊,音高,偏航数据缩放工具的一个IMU安装。汽车需要两个在一个椭圆形状的跟踪(基本上一个追随者行)。我想获得为数据从加速度和如果我绘制X的位置与Y位置我应该得到两个重叠椭圆形状的圈子。我已经尝试使用klamna过滤器。随着噪声的均值加速daa不断变化,很难做的卡尔曼滤波。然后我试着低通滤波和带通滤波(没有什么帮助)。到了我试着te代码从以下链接:
https://in.mathworks.com/matlabcentral/fileexchange/97012-acceleration-to-velocity-and-displacement-using-dsp-filters。
现在工作但位置数据不准确。还有其他已经存在的过滤器,可以减少数据的噪声,这样我可以得到位置数据?
清晰的所有;关闭所有;clc;
数据= readmatrix (“export_2_rounds.xlsx”);
acc_x =数据(:,3);% x加速
acc_y =数据(:,4);% y acceleratio
acc_z =数据(:,5);% z加速度
滚=数据(:,6);
距=数据(:7);
偏航=数据(:8);
时间=数据(:1);%这里序列号的数据阅读(我没使用时间戳)
acc_global_x = [];
acc_global_y = [];
acc_global_z = [];
%将辊、音高和偏航角弧度
roll_rad =函数(卷);
pitch_rad =函数(沥青);
yaw_rad =函数(偏航);
len =长度(acc_x);
为我= 1:兰
%计算旋转矩阵
R_x = [1 0 0;
0,cos (roll_rad (i)), sin (roll_rad(我));
0,sin (roll_rad (i)),因为(roll_rad(我)));
R_y = [cos (pitch_rad(我)),0,罪(pitch_rad(我));
0 1 0;
sin (pitch_rad(我)),0,因为(pitch_rad(我)));
R_z = [cos (yaw_rad(我)),sin (yaw_rad(我)),0;
sin (yaw_rad (i)),因为(yaw_rad(我)),0;
0,0,1];
%将旋转矩阵
R = R_z * R_y * R_x;
%建立当地的加速度矢量
acc_local = [acc_x(我),acc_y(我),acc_z(我)';
%将当地加速度矢量全球框架
acc_global = R_z * acc_local;%只对偏航角旋转矩阵
%提取组件的全球加速度
acc_global_x (i) = acc_global (1:);
acc_global_y (i) = acc_global (2:);
acc_global_z (i) = acc_global (3:);
结束
ag_xx = [acc_global_x] ';
ag_yy = [acc_global_y] ';
ag_zz = [acc_global_z] ';
accval_x = ag_xx;% X轴旋转后加速
accval_y = ag_yy;% Y轴旋转后加速
accval_z = ag_zz;% Z轴旋转后加速
cut_off_x = (0.2 - 0.8);%为x轴切断频率——两个带通滤波器的频率
cut_off_y = (0.2 - 0.8);
cut_off_z = (0.02 - 0.4);
α= 0.01;%的比例因子
filtertype =“带通”;%低通|高通|带通
% filtermethod =“fft”;% FFT-fft | FIR-fir
L =大小(数据,1);%长度的信号
Fs = 100;%采样频率
Ts = 1 / f;
% % % %计算频率
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%计算联合国过滤信号的傅里叶变换。
fft_x = fft (accval_x);
fft_y = fft (accval_x);
fft_z = fft (accval_x);
%计算双边频谱P2。然后计算出单面频谱P1
%根据even-valued P2和信号长度。
P_x1 = abs (fft_x);
P_y1 = abs (fft_y / L);
P_z1 = abs (fft_z / L);
P_x = P_x1(1:地板(L / 2 + 1));
P_y = P_y1(1:地板(L / 2 + 1));
P_z = P_z1(1:地板(L / 2 + 1));
P_x (2: end-1) = 2 * P_x (2: end-1);
P_y (2: end-1) = 2 * P_y (2: end-1);
P_z (2: end-1) = 2 * P_z (2: end-1);
f = f * (0: (L / 2)) / L;
图;
次要情节(1,1);
情节(f, P_x“线宽”,1)
网格在
标题(加速度的频谱)
包含(“f (Hz)”)
ylabel (“| P1 (f) |”)
次要情节(3、1、2);
情节(f, P_y“线宽”,1)
网格在
标题(加速度的频谱)
包含(“f (Hz)”)
ylabel (“| P1 (f) |”)
次要情节(3,1,3);
情节(f, P_z“线宽”,1)
网格在
标题(加速度的频谱)
包含(“f (Hz)”)
ylabel (“| P1 (f) |”)
% %计算加速度
[filtered_disp_x, filtered_vel_x filtered_acc_x]…
α= func_acc2disp (accval_x cut_off_x, filtertype);
[filtered_disp_y, filtered_vel_y filtered_acc_y]…
α= func_acc2disp (accval_y cut_off_y, filtertype);
[filtered_disp_z, filtered_vel_z filtered_acc_z] = func_acc2disp (accval_z cut_off_z,α,filtertype);
% %计算频率
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
%计算过滤信号的傅里叶变换。
fft_x = fft (filtered_acc_x);
fft_y = fft (filtered_acc_y);
fft_z = fft (filtered_acc_z);
%计算双边频谱P2。然后计算出单面频谱P1
%根据even-valued P2和信号长度。
P_x1 = abs (fft_x / L);
P_y1 = abs (fft_y / L);
P_z1 = abs (fft_z / L);
P_x = P_x1(1:地板(L / 2 + 1));
P_y = P_y1(1:地板(L / 2 + 1));
P_z = P_z1(1:地板(L / 2 + 1));
P_x (2: end-1) = 2 * P_x (2: end-1);
P_y (2: end-1) = 2 * P_y (2: end-1);
P_z (2: end-1) = 2 * P_z (2: end-1);
f = f * (0: (L / 2)) / L;
图;
次要情节(1,1);
情节(f, P_x“线宽”,1)
网格在
标题(加速度的频谱)
包含(“f (Hz)”)
ylabel (“| P1 (f) |”)
次要情节(3、1、2);
情节(f, P_y“线宽”,1)
网格在
标题(加速度的频谱)
包含(“f (Hz)”)
ylabel (“| P1 (f) |”)
次要情节(3,1,3);
情节(f, P_z“线宽”,1)
网格在
标题(加速度的频谱)
包含(“f (Hz)”)
ylabel (“| P1 (f) |”)
% %情节vs.time加速度
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
图;
次要情节(1,1)
情节(时间、filtered_acc_x“线宽”,1)
网格在
包含(“时间(秒。美元)”,“翻译”,“乳胶”);
ylabel (的加速度($ \压裂{m} {s ^ 2} $)”,“翻译”,“乳胶”);
标题(“加速vs.time(直流偏压删除))
大举裁员=次要情节(3、1、2);
情节(时间、filtered_vel_x“线宽”,1)
网格在
包含(“时间(秒。美元)”,“翻译”,“乳胶”);
ylabel (“速度($ \压裂{m}{年代}$)”,“翻译”,“乳胶”);
标题(“速度vs.time”)
ax5 =情节(3、1,3);
情节(时间、filtered_disp_x“线宽”1);
网格在
包含(“时间(秒。美元)”,“翻译”,“乳胶”);
ylabel (“位移(m美元)”,“翻译”,“乳胶”);
标题(“位移vs.time”)
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
图;
次要情节(1,1)
情节(时间、filtered_acc_y“线宽”,1)
网格在
包含(“时间(秒。美元)”,“翻译”,“乳胶”);
ylabel (的加速度($ \压裂{m} {s ^ 2} $)”,“翻译”,“乳胶”);
标题(“加速vs.time(直流偏压删除))
次要情节(1、2)
情节(时间、filtered_vel_y“线宽”,1)
网格在
包含(“时间(秒。美元)”,“翻译”,“乳胶”);
ylabel (“速度($ \压裂{m}{年代}$)”,“翻译”,“乳胶”);
标题(“速度vs.time”)
次要情节(3、1,3)
情节(时间、filtered_disp_y“线宽”,1)
网格在
包含(“时间(秒。美元)”,“翻译”,“乳胶”);
ylabel (“位移(m美元)”,“翻译”,“乳胶”);
标题(“位移vs.time”)
图;
情节(filtered_disp_x filtered_disp_y);
函数(pos、xd_Time Acc_filt] = func_acc2disp (signalIN截止,α,filtertype)
%的输入
xdd = signalIN;
dt = 0.01;
Fs = 1 / dt;
[Acc_filt, ~] = func_DCclean (xdd截止,α,Fs, filtertype);
velo = dt * cumtrapz (Acc_filt);
[xd_Time, ~] = func_DCclean (velo截止,α,Fs, filtertype);
pos = dt * cumtrapz (xd_Time);
% (pos_fil ~) = func_DCclean (pos、截止、α,Fs, filtertype);
结束
函数[sigflt, sigfrq] =…
func_DCclean(团体、截止、α,fs passtype)
%这个函数FUNC_DCCLEAN删除(低频率的内容
%给截断值)和直流偏置
x =长度(团体);
k =截止* (x / fs);
n =圆(k);
SIG_fft = fft (sig, x);
开关passtype
情况下低通滤波器的
SIG_fft(1) =复杂(α* abs (SIG_fft (n (1))), 0);
SIG_fft (x) =复杂(α* abs (SIG_fft (n (1))), 0);
如果(长度(n) > 1)
错误(“太多的截止频率”)
结束
为我= n:圆形((x / 2) 1)
SIG_fft (i) =α* SIG_fft (n);
SIG_fft (x) =连词(SIG_fft(我));
结束
情况下“高反差保留”
SIG_fft(1) =复杂(α* abs (SIG_fft (n (1))), 0);
如果(长度(n) > 1)
错误(“太多的截止频率”)
结束
为我= 2:n - 1
SIG_fft (i) =α* SIG_fft (n);
(SIG_fft (x) -我2)=连词(SIG_fft(我));
结束
否则
如果(长度(n) > 2)
错误(“太多的截止频率”)
elseif(长度(n) < 2)
错误(“截止频率太少”)
结束
SIG_fft(1) =复杂(α* abs (SIG_fft (n (1))), 0);
SIG_fft (x) =复杂(α* abs (SIG_fft (n (1))), 0);
为我= n(2):圆形((x / 2) 1)
SIG_fft (i) =α* SIG_fft (n (2));
SIG_fft (x) =连词(SIG_fft(我));
结束
为我= n (1): 1:2
SIG_fft (i) =α* SIG_fft (n (1));
SIG_fft (x) =连词(SIG_fft(我));
结束
结束
sigfrq = SIG_fft;
sigflt =实际(传输线(SIG_fft));
结束
