加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目,私有仓库也完全免费 :)
免费加入
文件
克隆/下载
PTstepcalc.m 3.07 KB
一键复制 编辑 原始数据 按行查看 历史
Brian White 提交于 2020-07-21 09:06 . minor rstep plot fix
function [stepresponse, t, rateHigh] = PTstepcalc(SP, GY, lograte, subsamp, minInput, rateHighThreshold)
%% [stepresponse, t, rateHigh] = PTstepcalc(SP, GY, lograte, subsamp, minInput))
% estimate of step response function using Wiener filter/deconvolution method
% SP = set point (input), GY = filtered gyro (output)
% returns matrix/stack of etimated stepresponse functions, time [t] in ms, and
% rateHigh, where 1 means set point >=500 deg/s, and 0 < 500deg/s
%
%
% ----------------------------------------------------------------------------------
% "THE BEER-WARE LICENSE" (Revision 42):
% <brian.white@queensu.ca> wrote this file. As long as you retain this notice you
% can do whatever you want with this stuff. If we meet some day, and you think
% this stuff is worth it, you can buy me a beer in return. -Brian White
% ----------------------------------------------------------------------------------
segment_length=(lograte*2000); % 2 sec segments
wnd=(lograte*1000) * 0.5; % 500ms step response function, length will depend on lograte
StepRespDuration_ms=500; % max dur of step resp in ms for plotting
subsampleFactor=subsamp;
stepresponse=[];
segment_vector=1:round(segment_length/subsampleFactor):length(SP);
NSegs=max(find((segment_vector+segment_length) < segment_vector(end)));
length(segment_vector)
if NSegs>0
hw = waitbar(0,['generating stack of input-output segments... ']);
clear SPseg GYseg
j=0;
for i=1:NSegs
waitbar(i/length(segment_vector),hw,['generating stack of input-output segments... ']);
if max(abs(SP(segment_vector(i):segment_vector(i)+segment_length))) >= minInput
j=j+1;
SPseg(j,:)=SP(segment_vector(i):segment_vector(i)+segment_length);
GYseg(j,:)=GY(segment_vector(i):segment_vector(i)+segment_length);
else
end
end
clear resp resp2 G H Hcon imp impf a b
j=0; rateHigh=0;
for i=1:size(SPseg,1)
waitbar(i/size(SPseg,1),hw,['computing step response functions... ']);
a=fft(GYseg(i,:).*hamming(length(GYseg(i,:)))',segment_length*2);% output, use hann or hamming taper
b=fft(SPseg(i,:).*hamming(length(SPseg(i,:)))',segment_length*2);% input, use hann or hamming taper
G=a/length(a);
H=b/length(b);
Hcon=conj(H);
imp=real(ifft((G .* Hcon) ./ (H .* Hcon + .0001)))'; % impulse response function, .0001 to avoid divide by 0
resptmp(i,:) = (cumsum(imp));%.^.5;% integrate impulse resp functions
a=stepinfo(resptmp(i,1:wnd)); % gather info about quality of step resp function
if a.SettlingMin>.5 && a.SettlingMin<=1 && a.SettlingMax>1 && a.SettlingMax<2 %Quality control
j=j+1;
stepresponse(j,:)=real(resptmp(i,1:1+wnd));
if max(abs(SPseg(i,:))) >= rateHighThreshold
rateHigh(j,:)=1;
else
rateHigh(j,:)=0;
end
end
t = 0:1/lograte:StepRespDuration_ms;% time in ms
end
end
close(hw)
end
Loading...
马建仓 AI 助手
尝试更多
代码解读
代码找茬
代码优化