CODESYS_ST_最小二乘线性标定FB_含完整源码

原创已于 2026-08-26 09:45:36 修改 · 171 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

于 2026-08-25 17:17:04 首次发布

CODESYS ST 最小二乘线性标定 FB（含完整源码）

1. 功能说明

目标：

根据标定数据：

$R_i,Z_i)$

求：

$Z = K R + B$

输出：

K：比例系数
B：偏移量
RMSE：均方根误差
MaxError：最大误差

2. FB接口

FUNCTION_BLOCK FB_LinearLeastSquare


VAR_INPUT

    //启动，上升沿触发
    Start : BOOL;


    //复位
    Reset : BOOL;


    //数据
    //Data[i,1]=R
    //Data[i,2]=Z

    Data :
    ARRAY[1..36,1..2] OF LREAL;


    //数据数量
    PointNum : INT;


    //允许最大RMSE
    MaxRMSELimit : LREAL := 0.05;


END_VAR



VAR_OUTPUT


    //拟合参数

    K : LREAL;

    B : LREAL;


    //误差

    RMSE : LREAL;

    MaxError : LREAL;



    //状态

    Done : BOOL;

    Error : BOOL;


    //错误代码

    ErrorID : INT;


END_VAR



VAR


    //========================
    // 上升沿触发
    //========================

    start_r : R_TRIG;



    //循环变量

    i : INT;



    //========================
    // 数据变量
    //========================

    R_Value : LREAL;

    Z_Value : LREAL;



    //========================
    // 最小二乘累加
    //========================

    SumR : LREAL;

    SumZ : LREAL;

    SumR2 : LREAL;

    SumRZ : LREAL;



    //行列式

    Den : LREAL;



    //========================
    //误差计算
    //========================

    PredictZ : LREAL;

    ErrValue : LREAL;

    SumError2 : LREAL;


END_VAR


------------------------------------------------------------------------

# 4. 完整ST源码

```pascal
//================================================
// Reset复位
//================================================

IF Reset THEN


    //输出清零

    K := 0.0;

    B := 0.0;


    RMSE := 0.0;

    MaxError := 0.0;


    Done := FALSE;

    Error := FALSE;

    ErrorID := 0;



    //内部变量清零

    SumR := 0.0;

    SumZ := 0.0;

    SumR2 := 0.0;

    SumRZ := 0.0;


    Den := 0.0;


    RETURN;


END_IF;



//================================================
// Start上升沿检测
//================================================

start_r
(
    CLK := Start
);



//================================================
// Start触发计算
//================================================

IF start_r.Q THEN



    //状态初始化

    Done := FALSE;

    Error := FALSE;

    ErrorID := 0;



    //================================================
    // 数据检查
    //================================================


    IF PointNum < 2 THEN


        Error := TRUE;

        ErrorID := 1;


        RETURN;


    END_IF;



    //================================================
    // 初始化求和
    //================================================


    SumR := 0.0;

    SumZ := 0.0;

    SumR2 := 0.0;

    SumRZ := 0.0;



    //================================================
    // 数据累加
    //
    // ΣR
    // ΣZ
    // ΣR²
    // ΣRZ
    //
    //================================================


    FOR i:=1 TO PointNum DO



        R_Value :=
        Data[i,1];


        Z_Value :=
        Data[i,2];



        SumR :=
        SumR + R_Value;



        SumZ :=
        SumZ + Z_Value;



        SumR2 :=
        SumR2
        +
        R_Value * R_Value;



        SumRZ :=
        SumRZ
        +
        R_Value * Z_Value;



    END_FOR;



    //================================================
    // 判断矩阵是否可逆
    //
    // Den=nΣR²-(ΣR)²
    //
    //================================================


    Den :=
    PointNum * SumR2
    -
    SumR * SumR;



    IF ABS(Den)<1E-12 THEN


        Error := TRUE;

        ErrorID := 2;


        RETURN;


    END_IF;



    //================================================
    // 求K
    //
    // K=
    // (nΣRZ-ΣRΣZ)
    // ----------------
    // (nΣR²-(ΣR)²)
    //
    //================================================


    K :=
    (
        PointNum * SumRZ
        -
        SumR * SumZ
    )
    /
    Den;



    //================================================
    // 求B
    //
    // B=(ΣZ-KΣR)/n
    //
    //================================================


    B :=
    (
        SumZ
        -
        K * SumR
    )
    /
    PointNum;



    //================================================
    // 计算拟合误差
    //================================================


    SumError2 := 0.0;

    MaxError := 0.0;



    FOR i:=1 TO PointNum DO



        PredictZ :=
        K * Data[i,1]
        +
        B;



        ErrValue :=
        Data[i,2]
        -
        PredictZ;



        SumError2 :=
        SumError2
        +
        ErrValue * ErrValue;



        IF ABS(ErrValue)>MaxError THEN


            MaxError :=
            ABS(ErrValue);


        END_IF;



    END_FOR;



    //================================================
    // RMSE
    //================================================


    RMSE :=
    SQRT
    (
        SumError2 / PointNum
    );



    //================================================
    // 精度判断
    //================================================


    IF RMSE > MaxRMSELimit THEN


        Error := TRUE;

        ErrorID := 3;


    ELSE


        Done := TRUE;


    END_IF;



END_IF;

5. 调用示例

PROGRAM PLC_PRG
VAR

    CalibrationFB :FB_LinearLeastSquare;


    CalibrationData :
    ARRAY[1..36,1..2] OF LREAL;
	bstart:BOOL;
	breset:BOOL;

	
	lk:LREAL;
	lb:LREAL;
	lrmse:LREAL;
	lmaxerror:LREAL;
	bdone:BOOL;
	berror:BOOL;
	ierrorid:INT;
END_VAR

主程序：

//==============================
// Z-R 标定数据
// Data[i,1] = R
// Data[i,2] = Z
//==============================


CalibrationData[1,1] := 16666;
CalibrationData[1,2] := 0;


CalibrationData[2,1] := 16404;
CalibrationData[2,2] := 1;


CalibrationData[3,1] := 16085;
CalibrationData[3,2] := 2;


CalibrationData[4,1] := 15748;
CalibrationData[4,2] := 3;


CalibrationData[5,1] := 15425;
CalibrationData[5,2] := 4;


CalibrationData[6,1] := 15093;
CalibrationData[6,2] := 5;


CalibrationData[7,1] := 14756;
CalibrationData[7,2] := 6;


CalibrationData[8,1] := 14433;
CalibrationData[8,2] := 7;


CalibrationData[9,1] := 14100;
CalibrationData[9,2] := 8;


CalibrationData[10,1] := 13765;
CalibrationData[10,2] := 9;


CalibrationData[11,1] := 13430;
CalibrationData[11,2] := 10;


CalibrationData[12,1] := 13100;
CalibrationData[12,2] := 11;


CalibrationData[13,1] := 12760;
CalibrationData[13,2] := 12;


CalibrationData[14,1] := 12440;
CalibrationData[14,2] := 13;


CalibrationData[15,1] := 12108;
CalibrationData[15,2] := 14;


CalibrationData[16,1] := 11770;
CalibrationData[16,2] := 15;


CalibrationData[17,1] := 11435;
CalibrationData[17,2] := 16;


CalibrationData[18,1] := 11110;
CalibrationData[18,2] := 17;


CalibrationData[19,1] := 10770;
CalibrationData[19,2] := 18;


CalibrationData[20,1] := 10435;
CalibrationData[20,2] := 19;


CalibrationData[21,1] := 10112;
CalibrationData[21,2] := 20;


CalibrationData[22,1] := 9790;
CalibrationData[22,2] := 21;


CalibrationData[23,1] := 9445;
CalibrationData[23,2] := 22;


CalibrationData[24,1] := 9115;
CalibrationData[24,2] := 23;


CalibrationData[25,1] := 8780;
CalibrationData[25,2] := 24;


CalibrationData[26,1] := 8440;
CalibrationData[26,2] := 25;


CalibrationData[27,1] := 8115;
CalibrationData[27,2] := 26;


CalibrationData[28,1] := 7790;
CalibrationData[28,2] := 27;


CalibrationData[29,1] := 7440;
CalibrationData[29,2] := 28;


CalibrationData[30,1] := 7115;
CalibrationData[30,2] := 29;


CalibrationData[31,1] := 6785;
CalibrationData[31,2] := 30;


CalibrationData[32,1] := 6450;
CalibrationData[32,2] := 31;


CalibrationData[33,1] := 6120;
CalibrationData[33,2] := 32;


CalibrationData[34,1] := 5795;
CalibrationData[34,2] := 33;


CalibrationData[35,1] := 5455;
CalibrationData[35,2] := 34;


CalibrationData[36,1] := 5125;
CalibrationData[36,2] := 35;



CalibrationFB(
	Start:=bstart , 
	Reset:=breset , 
	Data:=CalibrationData , 
	PointNum:= 36, 
	MaxRMSELimit:= 0.3, 
	K=>lk , 
	B=>lb , 
	RMSE=> lrmse, 
	MaxError=>lmaxerror , 
	Done=>bdone , 
	Error=>berror , 
	ErrorID=>ierrorid );

标签