拟合曲线：一元二次方程

代码如下：

FUNCTION_BLOCK _107_01_Fitting_Curve (*  采样5次 一元二次公式 *)
VAR_INPUT
    IN:REAL;  (*   当前速度  *)
    X_In:ARRAY[0..4] OF REAL;  (*   当前速度  *)
    Y_In:ARRAY[0..4] OF REAL;  (*   当前纠偏角 *)
END_VAR
VAR_OUTPUT
    OUT: REAL; (*  输出  *)
END_VAR
    VAR
    ID:INT;
    X:ARRAY[0..20] OF REAL;
    Y:ARRAY[0..20] OF REAL;

    SUMX: REAL:=0;
    SUMY: REAL:=0;
    SUMX2: REAL:=0;
    SUMXY: REAL:=0;
    SUMX3: REAL:=0;
    SUMX2Y: REAL:=0;
    SUMX4: REAL:=0;
    A: REAL;
    B: REAL;
    C: REAL;

    Z1: REAL;
    Z2: REAL;

    Z3: REAL;
    M1: REAL;
    M2: REAL;
    M3: REAL;

END_VAR

(*   初始值   *)
SUMX:=0.0;  SUMY:=0.0;  SUMX2 :=0.0;
SUMXY :=0.0;    SUMX3 :=0.0;     SUMX2Y:=0.0;
SUMX4 :=0.0;
(*   0-n (4)    5次采样     *)
FOR ID:=0 TO 4 DO   (*   总计5次采样   0-4 *)
  X[ID]:=  X_In[ID];    Y[ID]:=  Y_In[ID];
SUMX :=     X[ID] + SUMX ;  SUMY :=     Y[ID] + SUMY ;
SUMX2 := EXPT(X[ID],2 ) + SUMX2 ;  SUMXY := X[ID] * Y[ID]  + SUMXY  ;
SUMX3 := EXPT(X[ID],3 ) + SUMX3 ;  SUMX2Y := EXPT(X[ID],2 ) *Y[ID]+ SUMX2Y ;
SUMX4 := EXPT(X[ID],4 ) + SUMX4 ;

M1:=A;  A:= ( SUMX2Y -SUMX3*B-SUMX2*C)/SUMX4;  Z1:= (A-M1)*(A-M1);
M2:=B;  B:= ( SUMXY -SUMX*C-SUMX3*A)/SUMX2;  Z2:= (B-M2)*(B-M2);
M3:=C;  C:= ( SUMY -SUMX2*A-SUMX*B)/5;  Z3:= (C-M3)*(C-M3);
END_FOR;

(*   二次曲线    *)
OUT:=  M1* EXPT(IN,2 ) + M2 *IN + M3 ;