数据处理（伪）代码：卡尔曼滤波 vs. 卡尔曼平滑
步骤一、导入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<lenIdxpName{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() % 清理 绘图框内容 clfif rem(k,1000)==1plot(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)==1A1 = repmat(A1,[1 1 size(M1,2)]);endif size(Q1,3)==1Q1 = 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:1P_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');