一 卡尔曼滤波基本步骤

  卡尔曼滤波包含两个步骤:
  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=(LI)+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