一 卡尔曼滤波基本步骤
卡尔曼滤波包含两个步骤：
1 预测：
2 更新：
其中，K k 是卡尔曼增益，它是卡尔曼滤波器的精华，定义如下
以上公式采用的符号与维基百科—卡尔曼滤波保持一致。
如果你理解上面的公式有困难，那么下面我给出一种简单的解释。
以机器人定位为例，状态x xx代表机器人的位置和姿态（x,y平面坐标和朝向角度），控制u uu代表机器人的控制输入量（例如左右两车轮的转速）。直观地看，第一步的预测其实就是使用机器人的运动方程进行状态的递推。我们都知道，模型都是有误差的，采集的控制量也不可能完全与实际的一样，这就造成了理论计算值与机器人真实状态的偏差。我们用x xx表示理论计算值，用x t（下标t表示true）表示机器人真实的状态。短时间内，这个偏差是可以接受的，但问题是这个偏差会一直累积，导致我们的理论推测值与真实值越差越多，最后完全不可相信了。所以卡尔曼滤波的第一步就是单纯的里程定位或者航迹推测，这没什么新鲜的。
那么你可能会问，协方差矩阵P 代表了什么呢？协方差矩阵P 代表了我们对估计值x 相信的程度，我们用它对信任程度进行量化。P 越小说明我们对估计x 越有信心，反之如果P 越大我们就越怀疑x xx。这就和人一样，有些人位高权重我们就越倾向于不假思索地相信他们说的话，有些人经常胡说八道我们就会怀疑他观点的正确性。在后面的计算中，我们会根据P 的大小来分配权重，权重和P 的大小成反比。
既然只依靠第一步不靠谱，我们就引入了传感器来弥补。这就是第二步的更新，用传感器的测量结果（z zz）来矫正偏差。可是我们知道，传感器测量值也不是百分之百准确的，它也有不确定度（R ），我们也不能完全相信它。我们面对两个都没有完全说真话的人，这个时候到底该怎么办呢？是只信任其中一个还是综合考虑两个人的信息呢？肯定不能完全信任一个人，但是怎么综合考虑最好却是门艺术。卡尔曼滤波巧妙的地方就在于，它没有简单粗暴地直接把二者加起来，而是给其中一项前面乘了个系数，这就是卡尔曼增益K 。卡尔曼证明了这样设计的增益K 是最优的。没错，卡尔曼增益就是前面提到的权重，它综合了两个信息来源，一个是状态估计，一个是传感器的测量。根据卡尔曼增益K 的构造方法我们可以猜出来，P 越大R 越小，那么K 就越接近1，这意味着我们就越相信传感器测量，而不信状态的估计值；反之，P越小R越大，那么K 就越接近0，这意味着我们就越相信状态的估计值，而不信传感器的测量。
这样我们就解决了到底该相信谁的问题。
二 数学基础
如果你要估计的状态是向量，那么卡尔曼滤波就需要矩阵计算。而且它的计算过程基本上把常用的矩阵计算都占了，包括矩阵的加、减、乘、求逆、转置，矩阵与向量的乘，向量的加、减、内积、外积、标量与向量乘等等。这非常有意思，有点像我们练毛笔字中的“永”字，所有常用笔画集于一身。如果把卡尔曼滤波实现了，那么实现其它的算法就触类旁通了。好了，大学里的线性代数课本该拿出来了。
所有的矩阵运算中，求逆是最复杂的。文献[ 1 ] [1][1]使用了伴随矩阵计算逆矩阵，这种方法适合小规模的矩阵（维数<4），对于大规模矩阵效率较低。目前，矩阵求逆最常用而且比较高效的方法是LU分解法，思路是首先将矩阵分解成两个三角矩阵（的乘积），然后再计算两个三角矩阵的逆。由于计算三角矩阵的逆比较容易，所以这样我们能够高效地求矩阵的逆。但是在实际应用LU分解法时有一点要注意，如果矩阵的对角线上出现了0，就会使分解的三角矩阵不可逆，这可以通过对矩阵的行重新排列来解决，这样就多了一个置换矩阵，细节可以看维基百科。
维基百科提供了LU分解的C语言代码，我直接将C代码翻译成ST代码。在C语言程序中，直接在输入矩阵上做了计算，即将分解结果保存在了输入矩阵A中。如果你想提取出两个三角矩阵（L和U），可以按照下式这样分解，其中I II是单位矩阵。注意这里的矩阵A 是被更新过的矩阵，不再是原始的输入A 了哦。当然，从分解的三角矩阵L和U可以还原得到最开始的输入A ，只需要再乘以一个置换矩阵就行了。
A=(L−I)+U
三 PLC具体实现
3.1 PLC编程语言
我们用PLC做做数字的加减乘除很容易，但是要实现矩阵计算就有点复杂了，有过PLC编程经验的同志都知道，PLC一般不提供矩阵计算所需的函数。原因也很简单，早期的PLC主要被设计用于逻辑控制和过程控制，不是用来实现复杂的运动控制和数据处理算法的。这也是在PLC中实现的卡尔曼滤波主要的难点所在，基础支持太少。常用的PLC编程语言有梯形图和ST。其中，梯形图适合实现逻辑流程控制，不适合数值计算，所以我们采用ST语言这种与高级语言比较类似的PLC语言来实现卡尔曼滤波。由于现在的PLC语言基本都遵守IEC 61131-3国际标准，所以我们只在一种PLC环境中实现，移植到其它厂家的PLC产品将非常容易。我们选择codesys软PLC，它的编程软件免费而且使用方便，还支持本地仿真调试，不像西门子一样还要安装庞大而缓慢的博图软件。
codesys官方网店提供矩阵计算库（Matrix Library），可实现常用的矩阵操作，例如相加、乘、逆、行列式，但是你要花250欧元（人民币1932元）购买授权才能使用。
3.2 数据类型
既然我们用PLC语言实现，那就要熟悉PLC语言的特点。首先面对的问题就是如何定义一个矩阵或者向量。codesys提供了ARRAY数组这种数据类型，可以用来实现矩阵或向量的表示。ARRAY最多支持三维，我们只使用一维和二维就够了。定义二维数组的方法如下，其中，0…2表示数组元素索引的上下界，从A[0,0]开始，最后一位是A[2,2]。可以在定义数组时就初始化，注意对于高维数组，在初始化时采用一维拉直的写法，这就需要约定好是按行拉直还是按列拉直。我们规定一下，本文都采用行主序的存储方式，也就是说矩阵按照行拉直。注意MATLAB采用了Column major order（列主序），而C语言一般是Row major order（行主序），我们与C语言一致。
VAR
A:ARRAY[0..2,0..2] OF REAL:=[1,2,3,4,5,6,7,8,9];
END_VAR
如何操作ARRAY数组呢？如果我们想引用数组中的元素，例如读写，使用A[i,j]即可，其中i和j就是矩阵中元素的行列。
如果我们想传递数组该怎么办呢，例如将数组作为一个参数传递给函数块？数组名就像C语言中的指针，可以直接传递数组名，例如A:=FUN(B)。也可以直接用数组名给另一个数组赋值，例如A:=B。
数组的大小可以在定义时设为固定值，在程序执行过程中始终保持不变。这样所有的矩阵运算函数都只接收返回固定大小的数组。但是这样编写的程序没有通用性，遇到一个新的问题或者加入新的条件我们就要修改数组大小，很繁琐。有一个方法是使用可变长度数组[2,3]
（Arrays of variable length）。但是注意，可变长度数组只能用于VAR_IN_OUT声明中，不能用在其它声明中（例如VAR或者VAR_IN），下面我给出两个例子。
第一个例子是计算两个向量的内积，将其定义为函数（FUN），声明部分如下。Inner 函数的返回值是REAL类型，输入是两个可变长度的一维数组：v1和v2，二者长度必须相等，注意v1和v2的定义放在了VAR_IN_OUT中，而不是VAR中。
FUNCTION Inner : REAL;
VAR
i:LINT;
END_VAR
VAR_IN_OUT
v1: ARRAY[*] OF REAL; //输入向量1
v2: ARRAY[*] OF REAL; //输入向量2
END_VAR
函数主体部分如下。在遍历数组元素时使用了系统自带的LOWER_BOUND和UPPER_BOUND函数来获取数组的上下界。
Inner:=0;
FOR i:=LOWER_BOUND(v1,1) TO UPPER_BOUND(v1,1) BY 1 DO
Inner:=Inner+v1[i]*v2[i];
END_FOR;
函数使用的方式如下。该函数由于使用了可变长度的数组，所以不限制输入向量的维数，对于任意维数的数组都可以给出正确的结果。
result:=Inner(v1,v2);
第二个例子是两个矩阵相加，声明部分代码如下：
FUNCTION MatAdd: BOOL;
VAR_IN_OUT
A: ARRAY[*,*] OF REAL; //输入矩阵
B: ARRAY[*,*] OF REAL; //输入矩阵
C: ARRAY[*,*] OF REAL; //输出矩阵
END_VAR
VAR
i:LINT;
j:LINT;
END_VAR
函数主体部分如下：
FOR i:=LOWER_BOUND(A,1) TO UPPER_BOUND(A,1) BY 1 DO
FOR j:=LOWER_BOUND(A,2) TO UPPER_BOUND(A,2) BY 1 DO
C[i,j]:=A[i,j]+B[i,j];
END_FOR;
END_FOR;
该函数的使用方式如下，注意MatAdd前面没有赋值符号（:=），而是直接把想要赋值的数组作为参数传递给了这个函数（的第三个输入参数C），C数组里保存着我们想要的结果。
MatAdd(Mat1,Mat2,Mat3);
3.3 矩阵求逆
LU分解法的具体ST代码如下，基本与C语言版本一样，只有一个地方不同：C语言可以使用指针，但是ST语言的指针不太好用，所以我们不得不挨个操作矩阵中的每个元素，导致代码略长。
FOR i:=0 TO n-1 BY 1 DO
maxA:=0.0;
imax:=i;
FOR k:=i TO n-1 BY 1 DO
absA:=ABS(A[k,i]);
IF absA>maxA THEN
maxA:=absA;
imax:=k;
END_IF
END_FOR;
IF imax<>i THEN
//pivoting P
j:=P[i];
P[i]:=P[imax];
P[imax]:=j;
//pivoting rows of A
FOR k:=0 TO n-1 BY 1 DO
tmp[k]:=A[i,k];
END_FOR;
FOR k:=0 TO n-1 BY 1 DO
A[i,k]:=A[imax,k];
END_FOR;
FOR k:=0 TO n-1 BY 1 DO
A[imax,k]:=tmp[k];
END_FOR;
END_IF
FOR j:=i+1 TO n-1 BY 1 DO
A[j,i]:=A[j,i]/A[i,i];
FOR k:=i+1 TO n-1 BY 1 DO
A[j,k]:=A[j,k]-A[j,i]*A[i,k];
END_FOR;
END_FOR;
END_FOR;
完成LU分解后，下面再求逆就容易了，代码如下。
FOR j:=0 TO n-1 BY 1 DO
FOR i:=0 TO n-1 BY 1 DO
IF P[i]=j THEN
IA[i,j]:=1;
ELSE
IA[i,j]:=0;
END_IF;
FOR k:=0 TO i-1 BY 1 DO
IA[i,j]:=IA[i,j]-A[i,k]*IA[k,j];
END_FOR;
END_FOR;
FOR i:=n-1 TO 0 BY -1 DO
FOR k:=i+1 TO n-1 BY 1 DO
IA[i,j]:=IA[i,j]-A[i,k]*IA[k,j];
END_FOR;
IA[i,j]:=IA[i,j]/A[i,i];
END_FOR;
END_FOR;
完整的卡尔曼滤波迭代过程代码如下：
FOR i:=0 TO 100 BY 1 DO
//Prediction
x:=VecAdd(VecAdd(MatVecMul(F,x),u));
P:=MatAdd(MatMul(MatMul(F,P),MatTranspose(F)),Q);
//Update
K:=VecMul(MatVecMul(P,H),MatInv(KroneckerProduct(H,P),MatTranspose(H))+Rc);
x:=VecAdd(x,VecMul(K,z-Inner(H,x)));
P:=MatMul(MatSub(IdentityMatrix(n),KroneckerProduct(K,H)),P);
END_FOR;
四 具体案例
这里我们用一个例子来检验代码的正确性，例子来自于[4]，其与文献[1]差不多，是一个小球做自由落体运动，估计它的高度和速度。
文献[4]给出了MATLAB代码，如下。当然，这里只是仿真结果，噪声用randn函数生成。因为是仿真，为了对比和计算传感器观测值，还定义了真实的系统状态xt。仿真中，真实状态xt是没有噪声的（完美的），估计值是有噪音的。
% Define the system
N = 100; % number of time steps
dt = 0.01; % Sampling time (s)
t = dt*(1:N); % time vector (s)
F = [1, dt; 0, 1]; % system matrix - state
G = [-1/2*dt^2; -dt]; % system matrix - input
H = [1 0]; % observation matrix
Q = [0, 0; 0, 0]; % process noise covariance
u = 9.80665; % input = acceleration due to gravity (m/s^2)
I = eye(2); % identity matrix
% Define the initial position and velocity
y0 = 100; % m
v0 = 0; % m/s
% Initialize the state vector (true state)
xt = zeros(2, N); % True state vector
xt(:, 1) = [y0; v0]; % True intial state
% Loop through and calculate the true state
for k = 2:N
xt(:, k) = F*xt(:, k-1) + G*u; % Propagate the states through the prediction equations
end
% Generate the noisy measurement from the true state
R = 4; % m^2/s^2
v = sqrt(R)*randn(1, N); % measurement noise
z = H*xt + v; % noisy measurement
%% Perform the Kalman filter estimation
% Initialize the state vector (estimated state)
x = zeros(2, N); % Estimated state vector
x(:, 1) = [105; 0]; % Guess for initial state
% Initialize the covariance matrix
P = [10, 0; 0, 0.01]; % Covariance for initial state error
% Loop through and perform the Kalman filter equations recursively
for k = 2:N
x(:, k) = F*x(:, k-1) + G*u; % Predict the state vector
P = F*P*F' + Q; % Predict the covariance
K = P*H'/(H*P*H' + R); % Calculate the Kalman gain matrix
x(:,k) = x(:,k) + K*(z(k) - H*x(:,k)); % Update the state vector
P = (I - K*H)*P; % Update the covariance
end
%% Plot the results
plot(t, x(1,:), 'b--', 'LineWidth', 2);
hold on; plot(t, xt(1,:), 'r:', 'LineWidth', 1.5)
xlabel('t (s)'); ylabel('x_1 = h (m)'); grid on;
legend('Measured','Estimated','True');
参考资料
[1] 卡尔曼滤波器在PLC系统中的实现，徐承爱，自动化技术与应用, 2014.
[2] https://help.codesys.com/api-content/2/codesys/3.5.12.0/en/_cds_datatype_array/
[3] IEC 61131-3: Arrays with variable length
[4] A Kalman Filtering Tutorial for Undergraduate Students
