数据处理（伪）代码：卡尔曼滤波 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<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');