步骤一、导入csv或txt格式的试验数据
最简洁也是据说读取速度最快的方法是:
pPath = 'C:\data_org\9#-1.txt' % 数据文件
data = importdata(pPath); % 读取 pPath 的结果到 一个数据结构变量 data 中。
pData = data.data; % 提取有效数据数组
data
的数据结构如下:
data
.data % 数组
data
.textdata % cell
data
.name
保存与试验参数相关的一些信息:
pInfo = temp.textdata{1:28};
保存试验数据的变量名称及其单位等相关信息:
index = find(temp.textdata{29}()==';');
index = [0, index];
lenIdx = length(index);
pUnit = temp.textdata(30,1:6);
pVarious = temp.textdata(29,1);
i = 1;
while i<lenIdx
pName{1,i} = pVarious{1}(index(i)+1:index(i+1)-1);
i = i+1;
end
pName{1,i} = pVarious{1}(index(i)+1:end);
clear pVarious temp
步骤二、试验数据预处理
设置需要处理的数据序列起始点s
和长度num
。
Y1 = pData(s:s+num,1); % Tension
Y2 = pData(s:s+num,2); % Torsion
Y3 = pData(s:s+num,3); % Moment_X
Y4 = pData(s:s+num,4); % Moment_Y
Y5 = sqrt(Y3.^2+Y4.^2); % Moment_XY
pTime = pData(s:s+num,5)'; % Time
设置试验数据的采样时间间隔——由试验数据的时间戳可以求出,这里是1600Hz采样频率下的结果,因此有:
dt = 6.25e-04; % 采样时间间隔,1600Hz
估计或者计算试验数据的量测噪声强度——预估或者由试验结果计算。这里假设 Q = 1; R = 500^2。
q = 1; % 估计方差,模型噪声
sd = 500; % 预设方差,量测噪声
设置系统状态、协方差的初始值、系统的状态方程和输出向量等。
A1 = [2 -1; 1 0]; % 离散量的状态方程
Q1 = diag([q*dt 0]); % 模型噪声
M1 = [0;0]; % 初始值 x0
P1 = diag([0.25 2]); % 协方差矩阵
R1 = sd^2; % 量测噪声
H1 = [1 0]; % 输出
分配Kalman滤波后的数据空间,以及对应的协方差矩阵序列
MM1 = zeros(size(M1,1),size(Y1,2));
PP1 = zeros(size(M1,1),size(M1,1),size(Y1,2));
根据数量序列的大小,使用 Kalman Filter
依次求解滤波后的结果。
%
% KF for Tension
%
for k=1:size(Y1,2)
[M1,P1] = kf_predict(M1,P1,A1,Q1); % 使用模型 X_k = A1*x_k-1 + Q1 预测系统状态 M1
[M1,P1] = kf_update(M1,P1,Y1(k),H1,R1); % 由量测值对结果进行修正(更新)
MM1(:,k) = M1; % 保存 修正结果
PP1(:,:,k) = P1; % 保持 协方差
% 使用 `Kalman Filter` 处理数据时的实时结果显示
% figure() % 清理 绘图框内容 clf
if rem(k,1000)==1
plot(pTime,Y1,'k:', ...
pTime(k),M1(1),'ro',...
pTime(1:k),MM1(1,1:k),'r-');
drawnow;
end
end
然后,使用 Kalman smoother
对滤波的结果进行平滑处理。
(需要前面 Kalman Filter
过程中所使用的协方差矩阵序列 PP1
。)
SM1 = rts_smooth(MM1,PP1,A1,Q1);
或者
if size(A1,3)==1
A1 = repmat(A1,[1 1 size(M1,2)]);
end
if size(Q1,3)==1
Q1 = repmat(Q1,[1 1 size(M1,2)]);
end
%
% Run the smoother
%
D1 = zeros(size(M,1),size(M1,1),size(M1,2));
SM1 = M1;
for k=(size(M1,2)-1):-1:1
P_pred = A1(:,:,k) * P1(:,:,k) * A1(:,:,k)' + Q1(:,:,k);
D1(:,:,k) = P1(:,:,k) * A1(:,:,k)' / P_pred;
SM(:,k) = SM1(:,k) + D1(:,:,k) * (SM1(:,k+1) - A1(:,:,k) * SM1(:,k));
P1(:,:,k) = P1(:,:,k) + D1(:,:,k) * (P1(:,:,k+1) - P_pred) * D1(:,:,k)';
end
保存相关结果到 saveFile
文件中:
save(saveFile, 'pData', 'pTime', 'pName', 'pUnit');
save(saveFile, 'ss','se','dt','sd','q','A1','P1','-append');
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)