Codesys 实现卡尔曼滤波器-CODESYS资源下载区-酷德网 - CoDeSys软件教程下载,CoDeSys工控资讯门户 

酷德网

Heesn-HSC6150
查看: 104|回复: 0

Codesys 实现卡尔曼滤波器

[复制链接]
  • TA的每日心情
    擦汗
    2019-8-13 14:13
  • 签到天数: 32 天

    [LV.5]常住居民I

    发表于 2021-1-27 15:03:50 | 显示全部楼层 |阅读模式
    一 卡尔曼滤波基本步骤
    卡尔曼滤波包含两个步骤:
      1 预测
    123.png
    2 更新:
    456.png
    其中,Kk是卡尔曼增益,它是卡尔曼滤波器的精华,定义如下
    789.png
    以上公式采用的符号与维基百科—卡尔曼滤波保持一致。如果你理解上面的公式有困难,那么下面我给出一种简单的解释。
    以机器人定位为例,状态x代表机器人的位置和姿态(x,y平面坐标和朝向角度),控制u代表机器人的控制输入量(例如左右两车轮的转速)。直观地看,第一步的预测其实就是使用机器人的运动方程进行状态的递推。我们都知道,模型都是有误差的,采集的控制量也不可能完全与实际的一样,这就造成了理论计算值与机器人真实状态的偏差。我们用x表示理论计算值,用xt(下标t表示true)表示机器人真实的状态。短时间内,这个偏差是可以接受的,但问题是这个偏差会一直累积,导致我们的理论推测值与真实值越差越多,最后完全不可相信了。所以卡尔曼滤波的第一步就是单纯的里程定位或者航迹推测,这没什么新鲜的。
      那么你可能会问,协方差矩阵P代表了什么呢?协方差矩阵P代表了我们对估计值x相信的程度,我们用它对信任程度进行量化。P越小说明我们对估计x越有信心,反之如果P越大我们就越怀疑x。这就和人一样,有些人位高权重我们就越倾向于不假思索地相信他们说的话,有些人经常胡说八道我们就会怀疑他观点的正确性。在后面的计算中,我们会根据P的大小来分配权重,权重和P的大小成反比。
      既然只依靠第一步不靠谱,我们就引入了传感器来弥补。这就是第二步的更新,用传感器的测量结果(z)来矫正偏差。可是我们知道,传感器测量值也不是百分之百准确的,它也有不确定度(R),我们也不能完全相信它。我们面对两个都没有完全说真话的人,这个时候到底该怎么办呢?是只信任其中一个还是综合考虑两个人的信息呢?肯定不能完全信任一个人,但是怎么综合考虑最好却是门艺术。卡尔曼滤波巧妙的地方就在于,它没有简单粗暴地直接把二者加起来,而是给其中一项前面乘了个系数,这就是卡尔曼增益K。卡尔曼证明了这样设计的增益K是最优的。没错,卡尔曼增益就是前面提到的权重,它综合了两个信息来源,一个是状态估计,一个是传感器的测量。根据卡尔曼增益K的构造方法我们可以猜出来, P越大R越小,那么K就越接近1,这意味着我们就越相信传感器测量,而不信状态的估计值;反之, P越小R越大,那么K就越接近0,这意味着我们就越相信状态的估计值,而不信传感器的测量。
      这样我们就解决了到底该相信谁的问题。
    二 数学基础
      如果你要估计的状态是向量,那么卡尔曼滤波就需要矩阵计算。而且它的计算过程基本上把常用的矩阵计算都占了,包括矩阵的加、减、乘、求逆、转置,矩阵与向量的乘,向量的加、减、内积、外积、标量与向量乘等等。这非常有意思,有点像我们练毛笔字中的字,所有常用笔画集于一身。如果把卡尔曼滤波实现了,那么实现其它的算法就触类旁通了。好了,大学里的线性代数课本该拿出来了。
      所有的矩阵运算中,求逆是最复杂的。文献 [1]使用了伴随矩阵计算逆矩阵,这种方法适合小规模的矩阵(维数<4),对于大规模矩阵效率较低。目前,矩阵求逆最常用而且比较高效的方法是LU分解法,思路是首先将矩阵分解成两个三角矩阵(的乘积),然后再计算两个三角矩阵的逆。由于计算三角矩阵的逆比较容易,所以这样我们能够高效地求矩阵的逆。但是在实际应用LU分解法时有一点要注意,如果矩阵的对角线上出现了0,就会使分解的三角矩阵不可逆,这可以通过对矩阵的行重新排列来解决,这样就多了一个置换矩阵,细节可以看维基百科。
      维基百科提供了LU分解的C语言代码,我直接将C代码翻译成ST代码。在C语言程序中,直接在输入矩阵上做了计算,即将分解结果保存在了输入矩阵A中。如果你想提取出两个三角矩阵(LU),可以按照下式这样分解,其中I是单位矩阵。注意这里的矩阵A是被更新过的矩阵,不再是原始的输入A了哦。当然,从分解的三角矩阵LU可以还原得到最开始的输入A,只需要再乘以一个置换矩阵就行了。

    10.png
    三 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元)购买授权才能使用。
    20200205112946411.png

    3.2 数据类型
      既然我们用PLC语言实现,那就要熟悉PLC语言的特点。首先面对的问题就是如何定义一个矩阵或者向量。codesys提供了ARRAY数组这种数据类型,可以用来实现矩阵或向量的表示。ARRAY最多支持三维,我们只使用一维和二维就够了。定义二维数组的方法如下,其中,0…2表示数组元素索引的上下界,从A[0,0]开始,最后一位是A[2,2]。可以在定义数组时就初始化,注意对于高维数组,在初始化时采用一维拉直的写法,这就需要约定好是按行拉直还是按列拉直。我们规定一下,本文都采用行主序的存储方式,也就是说矩阵按照行拉直。注意MATLAB采用了Column major order(列主序),而C语言一般是Row major order(行主序),我们与C语言一致。
    [AppleScript] 纯文本查看 复制代码
    VAR[/align]	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中。
    [AppleScript] 纯文本查看 复制代码
    FUNCTION Inner : REAL;[/align]VAR
    	i:LINT;
    END_VAR
    VAR_IN_OUT
       v1: ARRAY[*] OF REAL;    //输入向量1
       v2: ARRAY[*] OF REAL;    //输入向量2
    END_VAR
    
    函数主体部分如下。在遍历数组元素时使用了系统自带的LOWER_BOUND和UPPER_BOUND函数来获取数组的上下界。
    [AppleScript] 纯文本查看 复制代码
    Inner:=0;[/align]FOR i:=LOWER_BOUND(v1,1) TO UPPER_BOUND(v1,1) BY 1 DO 
    	Inner:=Inner+v1[i]*v2[i];
    END_FOR;
    
    函数使用的方式如下。该函数由于使用了可变长度的数组,所以不限制输入向量的维数,对于任意维数的数组都可以给出正确的结果。
    [AppleScript] 纯文本查看 复制代码
    result:=Inner(v1,v2);[/align]
    第二个例子是两个矩阵相加,声明部分代码如下:
    [AppleScript] 纯文本查看 复制代码
    FUNCTION MatAdd: BOOL;[/align]VAR_IN_OUT
       A: ARRAY[*,*] OF REAL;   //输入矩阵
       B: ARRAY[*,*] OF REAL;   //输入矩阵
       C: ARRAY[*,*] OF REAL;   //输出矩阵
    END_VAR
    VAR
    	i:LINT;
    	j:LINT;
    END_VAR
    
    函数主体部分如下:
    [AppleScript] 纯文本查看 复制代码
    FOR i:=LOWER_BOUND(A,1) TO UPPER_BOUND(A,1) BY 1 DO [/align]	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数组里保存着我们想要的结果。
    [AppleScript] 纯文本查看 复制代码
    MatAdd(Mat1,Mat2,Mat3);[/align]
    3.3 矩阵求逆
      LU分解法的具体ST代码如下,基本与C语言版本一样,只有一个地方不同:C语言可以使用指针,但是ST语言的指针不太好用,所以我们不得不挨个操作矩阵中的每个元素,导致代码略长。
    [AppleScript] 纯文本查看 复制代码
    	FOR i:=0 TO n-1 BY 1 DO [/align]		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分解后,下面再求逆就容易了,代码如下。
    [AppleScript] 纯文本查看 复制代码
    	FOR j:=0 TO n-1 BY 1 DO [/align]		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;
    
    完整的卡尔曼滤波迭代过程代码如下:
    [AppleScript] 纯文本查看 复制代码
    FOR i:=0 TO 100 BY 1 DO[/align]		//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是没有噪声的(完美的),估计值是有噪音的。
    [AppleScript] 纯文本查看 复制代码
    % Define the system[/align]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



    打赏鼓励一下!
    +10
    回复

    使用道具 举报

    您需要登录后才可以回帖 登录 | 立即注册 新浪微博登陆

    本版积分规则

    
    顶部qrcode底部
    关注酷德网订阅号,获取更多资讯!

    QQ|小黑屋|手机版|Archiver|酷德论坛 ( 苏ICP备16065247号-1 )|网站地图

    GMT+8, 2021-4-18 23:16 , Processed in 0.205680 second(s), 61 queries .

    酷德网

    © 2001-2013 Hicodesys. 技术支持 by 酷德网

    快速回复 返回顶部 返回列表