PCS7: Kalman filter - KalMod (two autogenerated sources examples)
Äàòà: 2020-06-08
Äîáàâëåíî: komatic
Òåìà: SCL
Process model including Jacobi matrices and filter parameters, created in the configurator.
Huge formulas in code you ever saw.
Sources taken from : SIMATIC PCS 7 Kalman Configurator V9.0
// @(#) $Header:: $
//Copyright (C) Siemens AG 2016. All Rights Reserved.
(*---------------------------------------------------------------------------
KalMod: Process model and Jacobians for the state estimation algorithm
based on time discrete extended kalman filters (EKF).
Version including information from former EKF user data block
individual FC for process model 1 only,
to be created automatically by Kalman Configurator.
03/2016: vectors declared as Vector15 instead of Matrix15
-------------------------------------------------------------------------- *)
//Author: Kalman Configurator Date: 21-Nov-2017 08:27:43 Version: 9.0
//
//Library SIMATIC PCS 7 Kalman
//Function: process model for state estimation algorithm
FUNCTION KalMod3:VOID
TITLE= 'Kalman-Funktionen'
AUTHOR: KalConfi
NAME: KalMod3
VERSION: '9.0'
FAMILY: Kalman
//KNOW_HOW_PROTECT
// constants
// **********************************************************************************************
CONST
CONST_RESTART := -2; // restart the function
CONST_ANLAUF := -1; // startup OB100
CONST_ALGO_H_FUNC := 1; // calculation: estimation of output vectors
CONST_ALGO_F_FUNC := 2; // calculation:extrapolation of estimated state values
CONST_ALGO_JAKOBI_H := 3; // calculation: Jacobian of function h for value x_dach_minus_k
CONST_ALGO_JAKOBI_F := 4; // calculation: Jacobian of function f for value x_dach_plus_k,u_k
CONST_ALGO_JAKOBI_H2 := 5; // calculation: Jacobian of function h2 for value x_dach_minus_n
CONST_ALGO_H_FUNC2 := 6; // calculation: estimation of laboratory values for x_dach_mins_n
END_CONST
// ##############################################################################################
// ##############################################################################################
//
// **********************************************************************************************
// !!! DO NOT CHANGE THE THE INTERFACE OF THE FUNCTION !!!
// **********************************************************************************************
// **********************************************************************************************
//
// Input parameters
// **********************************************************************************************
VAR_INPUT
SampleTime :REAL; // Sampling Time [s]
CalcFunc :INT; // chosen function
NumberX :INT; // maximal number of projected estimated states
NumberY :INT; // maximal number of projected outputs
NumberYn :INT; // maximal number of sporadic analysed states
NumberZ :INT; // maximal number of projected not measurable outputs
StcX_MinusK :StcVector15; // a priori estimated states (x_dach_minus_k)
X_MinusK AT StcX_MinusK :Vector15;
StcX_PlusK :StcVector15; // a posteriori estimated states (x_dach_plus_k)
X_PlusK AT StcX_PlusK :Vector15;
StcU :StcVector15; // Input vector
U AT StcU :Vector15;
END_VAR
//
// Output parameters
// **********************************************************************************************
VAR_OUTPUT
StcX_MinK1 :StcVector15; // a posteriori estimated states (x_dach_minus_kplus1)
X_MinK1 AT StcX_MinK1 :Vector15;
StcY_MinusK :StcVector15; // estimated output vectors (y_dach_minus_k)
Y_MinusK AT StcY_MinusK :Vector15;
StcZ :StcVector15; // calculated additonal output vector (z)
Z AT StcZ :Vector15;
StcH :StcMatrix15; // Jacobian matrix of h
H AT StcH :Matrix15;
StcPHI :StcMatrix15; // Jacobian matrix of f
PHI AT StcPHI :Matrix15;
StcKalDB :StcKal_DB; // contents of former user data block generated by Kalman-Configurator
KalDB AT StcKalDB :Kal_DB;
Error :BOOL; // 1=Error
END_VAR
// **********************************************************************************************
VAR_TEMP
i, j :INT;
END_VAR
// **********************************************************************************************
// ##############################################################################################
// ##############################################################################################
// procedure body
// **********************************************************************************************
// check input parameters
Error := FALSE; // ok
IF (SampleTime = 0 OR NumberX = 0 OR NumberY = 0
OR X_MinusK.zeilenzahl = 0
OR X_PlusK.zeilenzahl = 0
OR U.zeilenzahl = 0 )
THEN
Error := TRUE; // error
END_IF;
IF (NOT Error)
THEN
// Process model 3:
CASE CalcFunc OF
// -------------------------------------------------------------------
// startup
// Restart of superordinated block
CONST_RESTART:
KalDB.MatrixSize := 15;
KalDB.NumberX := 7;
KalDB.NumberU := 7;
KalDB.NumberY := 2;
KalDB.NumberZ := 2;
KalDB.SampleTime := 2.000000e+00;
KalDB.SampleEn := 0;
KalDB.NumberYn := 0;
KalDB.Q_Diag.zeilenzahl := KalDB.NumberX;
KalDB.Q_Diag.elemente[1] := 0.000000e+00;
KalDB.Q_Diag.elemente[2] := 0.000000e+00;
KalDB.Q_Diag.elemente[3] := 0.000000e+00;
KalDB.Q_Diag.elemente[4] := 0.000000e+00;
KalDB.Q_Diag.elemente[5] := 0.000000e+00;
KalDB.Q_Diag.elemente[6] := 0.000000e+00;
KalDB.Q_Diag.elemente[7] := 0.000000e+00;
KalDB.R_Diag.zeilenzahl := KalDB.NumberY;
KalDB.R_Diag.elemente[1] := 1.077028e-02;
KalDB.R_Diag.elemente[2] := 1.245689e-02;
KalDB.X_Start.zeilenzahl := KalDB.NumberX;
KalDB.X_Start.elemente[1] := 3.055000e+02;
KalDB.X_Start.elemente[2] := 3.055000e+02;
KalDB.X_Start.elemente[3] := 1.058212e-03;
KalDB.X_Start.elemente[4] := 4.274435e+01;
KalDB.X_Start.elemente[5] := 1.099607e+01;
KalDB.X_Start.elemente[6] := 3.150000e+00;
KalDB.X_Start.elemente[7] := 5.397999e+01;
KalDB.X_LoLim.zeilenzahl := KalDB.NumberX;
KalDB.X_LoLim.elemente[1] := 0.000000e+00;
KalDB.X_LoLim.elemente[2] := 0.000000e+00;
KalDB.X_LoLim.elemente[3] := 0.000000e+00;
KalDB.X_LoLim.elemente[4] := 0.000000e+00;
KalDB.X_LoLim.elemente[5] := 0.000000e+00;
KalDB.X_LoLim.elemente[6] := 0.000000e+00;
KalDB.X_LoLim.elemente[7] := 0.000000e+00;
KalDB.X_HiLim.zeilenzahl := KalDB.NumberX;
KalDB.X_HiLim.elemente[1] := 5.000000e+02;
KalDB.X_HiLim.elemente[2] := 5.000000e+02;
KalDB.X_HiLim.elemente[3] := 3.000000e+00;
KalDB.X_HiLim.elemente[4] := 5.000000e+01;
KalDB.X_HiLim.elemente[5] := 5.000000e+01;
KalDB.X_HiLim.elemente[6] := 7.000000e+02;
KalDB.X_HiLim.elemente[7] := 9.000000e+02;
// OB100 - startup
CONST_ANLAUF:
;
// empty, but existing, otherwise an error occures.
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Processing of functions
// -------------------------------------------------------------------
// estimated value of output vector
// y_dach_minus_k = h_func(x_dach_minus_k);
// ==> here: Y_MinusK = h_func(X_MinusK);
CONST_ALGO_H_FUNC:
// initialising of matrices
Y_MinusK.zeilenzahl := NumberY; // Y_MinusK = (NumberY x 1)
// output equations:
Y_MinusK.elemente[1] := X_MinusK.elemente[1];
Y_MinusK.elemente[2] := X_MinusK.elemente[2];
// -------------------------------------------------------------------
Z.zeilenzahl := NumberZ; // Z = (NumberZ x 1)
// not measurable output equations:
Z.elemente[1] := (((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_MinusK.elemente[1])))*(X_MinusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_MinusK.elemente[5]/X_MinusK.elemente[7])))*( 10**(2.3*(1000/
(1.8*X_MinusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04);
Z.elemente[2] := (0.814*EXP((-5.13)*((5.2e-5*EXP(16.4*(X_MinusK.elemente[5]/X_MinusK.elemente[7])))*( 10**(2.3*
(555.5555555555555/(0.5*(X_MinusK.elemente[1]+(U.elemente[1]+X_MinusK.elemente[2])/ 2))-
1.563))))))*(((X_MinusK.elemente[3]/900+X_MinusK.elemente[4]/1000+X_MinusK.elemente[5]/1040)/0.193)*
1.59+0.167);
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// extrapolation of estimated states
// x_dach_minus_kplus1 = f_func(x_dach_plus_k, u_k, u_1_kdel(1), Ts);
// ==> here: X_MinK1 = f_func(X_PlusK, U, u1_kdel, SampleTime);
CONST_ALGO_F_FUNC:
// initialising of matrices
X_MinK1.zeilenzahl := NumberX; // X_MinK1 = (NumberX x 1)
// state equations in differential form:
X_MinK1.elemente[1] := X_PlusK.elemente[1]+2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*
4.187000e+00+X_PlusK.elemente[5]*3.140000e+00))*((U.elemente[5]*1.675000e+00)*(U.elemente[2]-
X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-X_PlusK.elemente[1])+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(((-0.814)*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/900+
X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
X_MinK1.elemente[2] := X_PlusK.elemente[2]+2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(((0.814*EXP((-5.13)*((5.2e-5*
EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/
900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)+((-4.187000e+00)*9.410000e-01)*(X_PlusK.elemente[2]-
U.elemente[7])));
X_MinK1.elemente[3] := X_PlusK.elemente[3]+2*(5.000000e+00*(U.elemente[5]+((((-5.500000e+01)*EXP((-2.965100e+04)/
(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**
0.4))*1.040000e+02));
X_MinK1.elemente[4] := X_PlusK.elemente[4]+2*(5.000000e+00*U.elemente[6]);
X_MinK1.elemente[5] := X_PlusK.elemente[5]+2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
X_MinK1.elemente[6] := X_PlusK.elemente[6]+2*(5.000000e+00*U.elemente[5]);
X_MinK1.elemente[7] := X_PlusK.elemente[7]+2*(5.000000e+00*(U.elemente[5]+U.elemente[6]));
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Jacobian of function h at value x_dach_minus_k
// H = Jacobi_h (x_dach_minus_k)
// ==> here: H = Jacobi_h (X_MinusK)
CONST_ALGO_JAKOBI_H:
// initialising of matrices
H.zeilenzahl := NumberY; // H = (NumberY x NumberX)
H.spaltenzahl := NumberX;
FOR i:= 1 TO H.zeilenzahl DO
FOR j:=1 TO H.spaltenzahl DO
H.elemente[i,j]:= 0;
END_FOR;
END_FOR;
// elements of Jacobian of outputs:
H.elemente[1,1] := 1;
H.elemente[1,2] := 0;
H.elemente[1,3] := 0;
H.elemente[1,4] := 0;
H.elemente[1,5] := 0;
H.elemente[1,6] := 0;
H.elemente[1,7] := 0;
H.elemente[2,1] := 0;
H.elemente[2,2] := 1;
H.elemente[2,3] := 0;
H.elemente[2,4] := 0;
H.elemente[2,5] := 0;
H.elemente[2,6] := 0;
H.elemente[2,7] := 0;
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Jacobian of function f at value x_dach_plus_k,U
// PHI = Jacobi_f(x_dach_plus_k, u_k, Ts);
// ==> here: PHI = Jacobi_f(X_PlusK, U, SampleTime);
CONST_ALGO_JAKOBI_F:
// initialising of matrices
PHI.zeilenzahl := NumberX; // PHI = (NumberX x NumberX)
PHI.spaltenzahl := NumberX;
FOR i:= 1 TO PHI.zeilenzahl DO
FOR j:=1 TO PHI.spaltenzahl DO
PHI.elemente[i,j]:= 0;
END_FOR;
END_FOR;
// elements of Jacobian of states:
PHI.elemente[1,1] := 1+2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((-U.elemente[5])*1.675000e+00+(-U.elemente[6])*4.187000e+00+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(0.4*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
((2.302585092994046*(2.3*((-1800)/((1.8*X_PlusK.elemente[1])** 2))))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563)))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6))))+((5.500000e+01*(EXP((-2.965100e+04)/
(8.314000e+00*X_PlusK.elemente[1]))*((2.965100e+04*8.314000e+00)/((8.314000e+00*
X_PlusK.elemente[1])** 2))))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**
0.4))*(--7.015200e+04)+((-0.814)*EXP((-5.13)*((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+
X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+
X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167)+(((-0.814)*(EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-277.7777777777778)/((0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))** 2))))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-
1.563))))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*
1.59+0.167))*(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)-5.667000e-03));
PHI.elemente[1,2] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((-0.814)*EXP((-5.13)*((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+
X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+
X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(-0.5)+(((-0.814)*(EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-138.8888888888889)/((0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))** 2))))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-
1.563))))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*
1.59+0.167))*(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)));
PHI.elemente[1,3] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*( 1/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+
(((-0.814)*EXP((-5.13)*((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*
0.009153713298791019)*(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2))+(((-
5.000000e+00)*1.675000e+00)/((X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00)** 2))*((U.elemente[5]*1.675000e+00)*(U.elemente[2]-
X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-X_PlusK.elemente[1])+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(((-0.814)*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/900+
X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
PHI.elemente[1,4] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((-0.814)*EXP((-5.13)*((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+
X_PlusK.elemente[2])/ 2))-1.563))))))*0.008238341968911919)*(X_PlusK.elemente[1]+(-U.elemente[1]-
X_PlusK.elemente[2])/ 2))+(((-5.000000e+00)*4.187000e+00)/((X_PlusK.elemente[3]*1.675000e+00+
X_PlusK.elemente[4]*4.187000e+00+X_PlusK.elemente[5]*3.140000e+00)** 2))*((U.elemente[5]*
1.675000e+00)*(U.elemente[2]-X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-
X_PlusK.elemente[1])+(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(((-0.814)*EXP((-5.13)*
((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/
900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
PHI.elemente[1,5] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6)))))*(--7.015200e+04)+(((-0.814)*EXP((-5.13)*
((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*0.007921482662415305+((-
0.814)*(EXP((-5.13)*((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*
((-5.13)*((5.2e-5*(EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*( 1/
X_PlusK.elemente[7]))))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+
X_PlusK.elemente[2])/ 2))-1.563)))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+
X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-U.elemente[1]-
X_PlusK.elemente[2])/ 2))+(((-5.000000e+00)*3.140000e+00)/((X_PlusK.elemente[3]*1.675000e+00+
X_PlusK.elemente[4]*4.187000e+00+X_PlusK.elemente[5]*3.140000e+00)** 2))*((U.elemente[5]*
1.675000e+00)*(U.elemente[2]-X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-
X_PlusK.elemente[1])+(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(((-0.814)*EXP((-5.13)*
((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/
900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
PHI.elemente[1,6] := 0;
PHI.elemente[1,7] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]**
2)))))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-
0.6)))))*(--7.015200e+04)+(((-0.814)*(EXP((-5.13)*((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+
X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]** 2)))))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))))*
(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*
(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)));
PHI.elemente[2,1] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*((0.814*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/900+
X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167)+((0.814*(EXP((-5.13)*((5.2e-5*
EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*
EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-277.7777777777778)/
((0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))** 2))))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-
1.563))))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*
1.59+0.167))*(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)));
PHI.elemente[2,2] := 1+2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(((0.814*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*(((X_PlusK.elemente[3]/900+
X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(-0.5)+((0.814*(EXP((-5.13)*
((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*
EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-138.8888888888889)/
((0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))** 2))))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-
1.563))))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*
1.59+0.167))*(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-4.187000e+00)*
9.410000e-01));
PHI.elemente[2,3] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(((0.814*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*0.009153713298791019)*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)));
PHI.elemente[2,4] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(((0.814*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*0.008238341968911919)*(X_PlusK.elemente[1]+(-
U.elemente[1]-X_PlusK.elemente[2])/ 2)));
PHI.elemente[2,5] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(((0.814*EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563))))))*0.007921482662415305+(0.814*(EXP((-5.13)*
((5.2e-5*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*
(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*
(EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*
(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))))*
(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*
(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)));
PHI.elemente[2,6] := 0;
PHI.elemente[2,7] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(((0.814*(EXP((-5.13)*((5.2e-5*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+
(U.elemente[1]+X_PlusK.elemente[2])/ 2))-1.563)))))*((-5.13)*((5.2e-5*(EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]**
2)))))*( 10**(2.3*(555.5555555555555/(0.5*(X_PlusK.elemente[1]+(U.elemente[1]+
X_PlusK.elemente[2])/ 2))-1.563)))))))*(((X_PlusK.elemente[3]/900+X_PlusK.elemente[4]/1000+
X_PlusK.elemente[5]/1040)/0.193)*1.59+0.167))*(X_PlusK.elemente[1]+(-U.elemente[1]-
X_PlusK.elemente[2])/ 2)));
PHI.elemente[3,1] := 2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-1800)/((1.8*X_PlusK.elemente[1])** 2))))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563)))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6))))+(((-
5.500000e+01)*(EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1]))*((2.965100e+04*8.314000e+00)/
((8.314000e+00*X_PlusK.elemente[1])** 2))))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**
0.4))*1.040000e+02));
PHI.elemente[3,2] := 0;
PHI.elemente[3,3] := 1+2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
( 1/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/
(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
PHI.elemente[3,4] := 0;
PHI.elemente[3,5] := 2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-
1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[3,6] := 0;
PHI.elemente[3,7] := 2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]** 2)))))*( 10**(2.3*(1000/
(1.8*X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[4,1] := 0;
PHI.elemente[4,2] := 0;
PHI.elemente[4,3] := 0;
PHI.elemente[4,4] := 1;
PHI.elemente[4,5] := 0;
PHI.elemente[4,6] := 0;
PHI.elemente[4,7] := 0;
PHI.elemente[5,1] := 2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-1800)/((1.8*X_PlusK.elemente[1])** 2))))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563)))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6))))+
((5.500000e+01*(EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1]))*((2.965100e+04*
8.314000e+00)/((8.314000e+00*X_PlusK.elemente[1])** 2))))*(X_PlusK.elemente[3]/1.040000e+02))*
(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
PHI.elemente[5,2] := 0;
PHI.elemente[5,3] := 2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*( 1/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
PHI.elemente[5,4] := 0;
PHI.elemente[5,5] := 1+2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-
1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[5,6] := 0;
PHI.elemente[5,7] := 2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]** 2)))))*( 10**(2.3*(1000/
(1.8*X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[6,1] := 0;
PHI.elemente[6,2] := 0;
PHI.elemente[6,3] := 0;
PHI.elemente[6,4] := 0;
PHI.elemente[6,5] := 0;
PHI.elemente[6,6] := 1;
PHI.elemente[6,7] := 0;
PHI.elemente[7,1] := 0;
PHI.elemente[7,2] := 0;
PHI.elemente[7,3] := 0;
PHI.elemente[7,4] := 0;
PHI.elemente[7,5] := 0;
PHI.elemente[7,6] := 0;
PHI.elemente[7,7] := 1;
// -------------------------------------------------------------------
// -------------------------------------------------------------------
ELSE :
Error := TRUE; // incorrect parameterisation
END_CASE;
END_IF;
END_FUNCTION
// @(#) $Header:: $
//Copyright (C) Siemens AG 2016. All Rights Reserved.
(*---------------------------------------------------------------------------
KalMod: Process model and Jacobians for the state estimation algorithm
based on time discrete extended kalman filters (EKF).
Version including information from former EKF user data block
individual FC for process model 1 only,
to be created automatically by Kalman Configurator.
03/2016: vectors declared as Vector15 instead of Matrix15
-------------------------------------------------------------------------- *)
//Author: Kalman Configurator Date: 05-Dec-2017 13:24:43 Version: 9.0
//
//Library SIMATIC PCS 7 Kalman
//Function: process model for state estimation algorithm
FUNCTION KalMod4:VOID
TITLE= 'Kalman-Funktionen'
AUTHOR: KalConfi
NAME: KalMod4
VERSION: '9.0'
FAMILY: Kalman
//KNOW_HOW_PROTECT
// constants
// **********************************************************************************************
CONST
CONST_RESTART := -2; // restart the function
CONST_ANLAUF := -1; // startup OB100
CONST_ALGO_H_FUNC := 1; // calculation: estimation of output vectors
CONST_ALGO_F_FUNC := 2; // calculation:extrapolation of estimated state values
CONST_ALGO_JAKOBI_H := 3; // calculation: Jacobian of function h for value x_dach_minus_k
CONST_ALGO_JAKOBI_F := 4; // calculation: Jacobian of function f for value x_dach_plus_k,u_k
CONST_ALGO_JAKOBI_H2 := 5; // calculation: Jacobian of function h2 for value x_dach_minus_n
CONST_ALGO_H_FUNC2 := 6; // calculation: estimation of laboratory values for x_dach_mins_n
END_CONST
// ##############################################################################################
// ##############################################################################################
//
// **********************************************************************************************
// !!! DO NOT CHANGE THE THE INTERFACE OF THE FUNCTION !!!
// **********************************************************************************************
// **********************************************************************************************
//
// Input parameters
// **********************************************************************************************
VAR_INPUT
SampleTime :REAL; // Sampling Time [s]
CalcFunc :INT; // chosen function
NumberX :INT; // maximal number of projected estimated states
NumberY :INT; // maximal number of projected outputs
NumberYn :INT; // maximal number of sporadic analysed states
NumberZ :INT; // maximal number of projected not measurable outputs
StcX_MinusK :StcVector15; // a priori estimated states (x_dach_minus_k)
X_MinusK AT StcX_MinusK :Vector15;
StcX_PlusK :StcVector15; // a posteriori estimated states (x_dach_plus_k)
X_PlusK AT StcX_PlusK :Vector15;
StcU :StcVector15; // Input vector
U AT StcU :Vector15;
END_VAR
//
// Output parameters
// **********************************************************************************************
VAR_OUTPUT
StcX_MinK1 :StcVector15; // a posteriori estimated states (x_dach_minus_kplus1)
X_MinK1 AT StcX_MinK1 :Vector15;
StcY_MinusK :StcVector15; // estimated output vectors (y_dach_minus_k)
Y_MinusK AT StcY_MinusK :Vector15;
StcZ :StcVector15; // calculated additonal output vector (z)
Z AT StcZ :Vector15;
StcH :StcMatrix15; // Jacobian matrix of h
H AT StcH :Matrix15;
StcPHI :StcMatrix15; // Jacobian matrix of f
PHI AT StcPHI :Matrix15;
StcKalDB :StcKal_DB; // contents of former user data block generated by Kalman-Configurator
KalDB AT StcKalDB :Kal_DB;
Error :BOOL; // 1=Error
END_VAR
// **********************************************************************************************
VAR_TEMP
i, j :INT;
END_VAR
// **********************************************************************************************
// ##############################################################################################
// ##############################################################################################
// procedure body
// **********************************************************************************************
// check input parameters
Error := FALSE; // ok
IF (SampleTime = 0 OR NumberX = 0 OR NumberY = 0
OR X_MinusK.zeilenzahl = 0
OR X_PlusK.zeilenzahl = 0
OR U.zeilenzahl = 0 )
THEN
Error := TRUE; // error
END_IF;
IF (NOT Error)
THEN
// Process model 4:
CASE CalcFunc OF
// -------------------------------------------------------------------
// startup
// Restart of superordinated block
CONST_RESTART:
KalDB.MatrixSize := 15;
KalDB.NumberX := 8;
KalDB.NumberU := 7;
KalDB.NumberY := 2;
KalDB.NumberZ := 1;
KalDB.SampleTime := 2.000000e+00;
KalDB.SampleEn := 1;
KalDB.NumberYn := 1;
KalDB.Q_Diag.zeilenzahl := KalDB.NumberX;
KalDB.Q_Diag.elemente[1] := 5.000000e-05;
KalDB.Q_Diag.elemente[2] := 5.000000e-03;
KalDB.Q_Diag.elemente[3] := 3.000000e-07;
KalDB.Q_Diag.elemente[4] := 5.000000e-06;
KalDB.Q_Diag.elemente[5] := 5.000000e-06;
KalDB.Q_Diag.elemente[6] := 7.000000e-05;
KalDB.Q_Diag.elemente[7] := 9.000000e-05;
KalDB.Q_Diag.elemente[8] := 1.000000e-06;
KalDB.R_Diag.zeilenzahl := KalDB.NumberY;
KalDB.R_Diag.elemente[1] := 1.077028e-02;
KalDB.R_Diag.elemente[2] := 1.245689e-02;
KalDB.X_Start.zeilenzahl := KalDB.NumberX;
KalDB.X_Start.elemente[1] := 3.050000e+02;
KalDB.X_Start.elemente[2] := 3.055000e+02;
KalDB.X_Start.elemente[3] := 0.000000e+00;
KalDB.X_Start.elemente[4] := 4.275000e+01;
KalDB.X_Start.elemente[5] := 1.100000e+01;
KalDB.X_Start.elemente[6] := 3.150000e+00;
KalDB.X_Start.elemente[7] := 5.398000e+01;
KalDB.X_Start.elemente[8] := 5.087747e-01;
KalDB.X_LoLim.zeilenzahl := KalDB.NumberX;
KalDB.X_LoLim.elemente[1] := 0.000000e+00;
KalDB.X_LoLim.elemente[2] := 0.000000e+00;
KalDB.X_LoLim.elemente[3] := 0.000000e+00;
KalDB.X_LoLim.elemente[4] := 0.000000e+00;
KalDB.X_LoLim.elemente[5] := 0.000000e+00;
KalDB.X_LoLim.elemente[6] := 0.000000e+00;
KalDB.X_LoLim.elemente[7] := 0.000000e+00;
KalDB.X_LoLim.elemente[8] := 0.000000e+00;
KalDB.X_HiLim.zeilenzahl := KalDB.NumberX;
KalDB.X_HiLim.elemente[1] := 5.000000e+02;
KalDB.X_HiLim.elemente[2] := 5.000000e+02;
KalDB.X_HiLim.elemente[3] := 3.000000e+00;
KalDB.X_HiLim.elemente[4] := 5.000000e+01;
KalDB.X_HiLim.elemente[5] := 5.000000e+01;
KalDB.X_HiLim.elemente[6] := 7.000000e+02;
KalDB.X_HiLim.elemente[7] := 9.000000e+02;
KalDB.X_HiLim.elemente[8] := 1.000000e+00;
KalDB.Rn_Diag.zeilenzahl := KalDB.NumberYn;
KalDB.Rn_Diag.elemente[1] := 1.000000e-05;
KalDB.Yn_LoLim.zeilenzahl := KalDB.NumberYn;
KalDB.Yn_LoLim.elemente[1] := 0.000000e+00;
KalDB.Yn_HiLim.zeilenzahl := KalDB.NumberYn;
KalDB.Yn_HiLim.elemente[1] := 3.000000e+00;
// OB100 - startup
CONST_ANLAUF:
;
// empty, but existing, otherwise an error occures.
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Processing of functions
// -------------------------------------------------------------------
// estimated value of output vector
// y_dach_minus_k = h_func(x_dach_minus_k);
// ==> here: Y_MinusK = h_func(X_MinusK);
CONST_ALGO_H_FUNC:
// initialising of matrices
Y_MinusK.zeilenzahl := NumberY; // Y_MinusK = (NumberY x 1)
// output equations:
Y_MinusK.elemente[1] := X_MinusK.elemente[1];
Y_MinusK.elemente[2] := X_MinusK.elemente[2];
// -------------------------------------------------------------------
Z.zeilenzahl := NumberZ; // Z = (NumberZ x 1)
// not measurable output equations:
Z.elemente[1] := (((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_MinusK.elemente[1])))*(X_MinusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_MinusK.elemente[5]/X_MinusK.elemente[7])))*( 10**(2.3*(1000/
(1.8*X_MinusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04);
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// extrapolation of estimated states
// x_dach_minus_kplus1 = f_func(x_dach_plus_k, u_k, u_1_kdel(1), Ts);
// ==> here: X_MinK1 = f_func(X_PlusK, U, u1_kdel, SampleTime);
CONST_ALGO_F_FUNC:
// initialising of matrices
X_MinK1.zeilenzahl := NumberX; // X_MinK1 = (NumberX x 1)
// state equations in differential form:
X_MinK1.elemente[1] := X_PlusK.elemente[1]+2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*
4.187000e+00+X_PlusK.elemente[5]*3.140000e+00))*((U.elemente[5]*1.675000e+00)*(U.elemente[2]-
X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-X_PlusK.elemente[1])+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(-X_PlusK.elemente[8])*(X_PlusK.elemente[1]+
(-U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
X_MinK1.elemente[2] := X_PlusK.elemente[2]+2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(X_PlusK.elemente[8]*
(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)+((-4.187000e+00)*9.410000e-01)*
(X_PlusK.elemente[2]-U.elemente[7])));
X_MinK1.elemente[3] := X_PlusK.elemente[3]+2*(5.000000e+00*(U.elemente[5]+((((-5.500000e+01)*EXP((-2.965100e+04)/
(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**
0.4))*1.040000e+02));
X_MinK1.elemente[4] := X_PlusK.elemente[4]+2*(5.000000e+00*U.elemente[6]);
X_MinK1.elemente[5] := X_PlusK.elemente[5]+2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
X_MinK1.elemente[6] := X_PlusK.elemente[6]+2*(5.000000e+00*U.elemente[5]);
X_MinK1.elemente[7] := X_PlusK.elemente[7]+2*(5.000000e+00*(U.elemente[5]+U.elemente[6]));
X_MinK1.elemente[8] := X_PlusK.elemente[8];
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Jacobian of function h at value x_dach_minus_k
// H = Jacobi_h (x_dach_minus_k)
// ==> here: H = Jacobi_h (X_MinusK)
CONST_ALGO_JAKOBI_H:
// initialising of matrices
H.zeilenzahl := NumberY; // H = (NumberY x NumberX)
H.spaltenzahl := NumberX;
FOR i:= 1 TO H.zeilenzahl DO
FOR j:=1 TO H.spaltenzahl DO
H.elemente[i,j]:= 0;
END_FOR;
END_FOR;
// elements of Jacobian of outputs:
H.elemente[1,1] := 1;
H.elemente[1,2] := 0;
H.elemente[1,3] := 0;
H.elemente[1,4] := 0;
H.elemente[1,5] := 0;
H.elemente[1,6] := 0;
H.elemente[1,7] := 0;
H.elemente[1,8] := 0;
H.elemente[2,1] := 0;
H.elemente[2,2] := 1;
H.elemente[2,3] := 0;
H.elemente[2,4] := 0;
H.elemente[2,5] := 0;
H.elemente[2,6] := 0;
H.elemente[2,7] := 0;
H.elemente[2,8] := 0;
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Jacobian of function f at value x_dach_plus_k,U
// PHI = Jacobi_f(x_dach_plus_k, u_k, Ts);
// ==> here: PHI = Jacobi_f(X_PlusK, U, SampleTime);
CONST_ALGO_JAKOBI_F:
// initialising of matrices
PHI.zeilenzahl := NumberX; // PHI = (NumberX x NumberX)
PHI.spaltenzahl := NumberX;
FOR i:= 1 TO PHI.zeilenzahl DO
FOR j:=1 TO PHI.spaltenzahl DO
PHI.elemente[i,j]:= 0;
END_FOR;
END_FOR;
// elements of Jacobian of states:
PHI.elemente[1,1] := 1+2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((-U.elemente[5])*1.675000e+00+(-U.elemente[6])*4.187000e+00+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(0.4*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
((2.302585092994046*(2.3*((-1800)/((1.8*X_PlusK.elemente[1])** 2))))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563)))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6))))+((5.500000e+01*(EXP((-2.965100e+04)/
(8.314000e+00*X_PlusK.elemente[1]))*((2.965100e+04*8.314000e+00)/((8.314000e+00*
X_PlusK.elemente[1])** 2))))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**
0.4))*(--7.015200e+04)-X_PlusK.elemente[8]-5.667000e-03));
PHI.elemente[1,2] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((-X_PlusK.elemente[8])*(-0.5)));
PHI.elemente[1,3] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*( 1/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*(--
7.015200e+04))+(((-5.000000e+00)*1.675000e+00)/((X_PlusK.elemente[3]*1.675000e+00+
X_PlusK.elemente[4]*4.187000e+00+X_PlusK.elemente[5]*3.140000e+00)** 2))*((U.elemente[5]*
1.675000e+00)*(U.elemente[2]-X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-
X_PlusK.elemente[1])+(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(-X_PlusK.elemente[8])*
(X_PlusK.elemente[1]+(-U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-
U.elemente[4])));
PHI.elemente[1,4] := 2*((((-5.000000e+00)*4.187000e+00)/((X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*
4.187000e+00+X_PlusK.elemente[5]*3.140000e+00)** 2))*((U.elemente[5]*1.675000e+00)*(U.elemente[2]-
X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-X_PlusK.elemente[1])+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(-X_PlusK.elemente[8])*(X_PlusK.elemente[1]+
(-U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
PHI.elemente[1,5] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6)))))*(--7.015200e+04))+(((-5.000000e+00)*
3.140000e+00)/((X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00)** 2))*((U.elemente[5]*1.675000e+00)*(U.elemente[2]-
X_PlusK.elemente[1])+(U.elemente[6]*4.187000e+00)*(U.elemente[3]-X_PlusK.elemente[1])+
(((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*(--7.015200e+04)+(-X_PlusK.elemente[8])*(X_PlusK.elemente[1]+
(-U.elemente[1]-X_PlusK.elemente[2])/ 2)+(-5.667000e-03)*(X_PlusK.elemente[1]-U.elemente[4])));
PHI.elemente[1,6] := 0;
PHI.elemente[1,7] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*
X_PlusK.elemente[1])))*(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]**
2)))))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-
0.6)))))*(--7.015200e+04)));
PHI.elemente[1,8] := 2*((5.000000e+00/(X_PlusK.elemente[3]*1.675000e+00+X_PlusK.elemente[4]*4.187000e+00+
X_PlusK.elemente[5]*3.140000e+00))*(-X_PlusK.elemente[1]+(U.elemente[1]+X_PlusK.elemente[2])/ 2));
PHI.elemente[2,1] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*X_PlusK.elemente[8]);
PHI.elemente[2,2] := 1+2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(X_PlusK.elemente[8]*(-0.5)+(-4.187000e+00)*
9.410000e-01));
PHI.elemente[2,3] := 0;
PHI.elemente[2,4] := 0;
PHI.elemente[2,5] := 0;
PHI.elemente[2,6] := 0;
PHI.elemente[2,7] := 0;
PHI.elemente[2,8] := 2*((5.000000e+00/(4.187000e+00*2.145500e+01))*(X_PlusK.elemente[1]+(-U.elemente[1]-
X_PlusK.elemente[2])/ 2));
PHI.elemente[3,1] := 2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-1800)/((1.8*X_PlusK.elemente[1])** 2))))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563)))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6))))+(((-
5.500000e+01)*(EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1]))*((2.965100e+04*8.314000e+00)/
((8.314000e+00*X_PlusK.elemente[1])** 2))))*(X_PlusK.elemente[3]/1.040000e+02))*(((0.052*EXP(16.4*
(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**
0.4))*1.040000e+02));
PHI.elemente[3,2] := 0;
PHI.elemente[3,3] := 1+2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
( 1/1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/
(1.8*X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
PHI.elemente[3,4] := 0;
PHI.elemente[3,5] := 2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-
1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[3,6] := 0;
PHI.elemente[3,7] := 2*(5.000000e+00*(((((-5.500000e+01)*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]** 2)))))*( 10**(2.3*(1000/
(1.8*X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[3,8] := 0;
PHI.elemente[4,1] := 0;
PHI.elemente[4,2] := 0;
PHI.elemente[4,3] := 0;
PHI.elemente[4,4] := 1;
PHI.elemente[4,5] := 0;
PHI.elemente[4,6] := 0;
PHI.elemente[4,7] := 0;
PHI.elemente[4,8] := 0;
PHI.elemente[5,1] := 2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*((2.302585092994046*(2.3*((-1800)/((1.8*X_PlusK.elemente[1])** 2))))*( 10**
(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563)))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6))))+
((5.500000e+01*(EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1]))*((2.965100e+04*
8.314000e+00)/((8.314000e+00*X_PlusK.elemente[1])** 2))))*(X_PlusK.elemente[3]/1.040000e+02))*
(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
PHI.elemente[5,2] := 0;
PHI.elemente[5,3] := 2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*( 1/
1.040000e+02))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**0.4))*1.040000e+02));
PHI.elemente[5,4] := 0;
PHI.elemente[5,5] := 1+2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*( 1/X_PlusK.elemente[7]))))*( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-
1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*( 10**(2.3*(1000/(1.8*
X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[5,6] := 0;
PHI.elemente[5,7] := 2*(5.000000e+00*((((5.500000e+01*EXP((-2.965100e+04)/(8.314000e+00*X_PlusK.elemente[1])))*
(X_PlusK.elemente[3]/1.040000e+02))*(0.4*(((0.052*(EXP(16.4*(X_PlusK.elemente[5]/
X_PlusK.elemente[7]))*(16.4*((-X_PlusK.elemente[5])/(X_PlusK.elemente[7]** 2)))))*( 10**(2.3*(1000/
(1.8*X_PlusK.elemente[1])-1.563))))*(((0.052*EXP(16.4*(X_PlusK.elemente[5]/X_PlusK.elemente[7])))*
( 10**(2.3*(1000/(1.8*X_PlusK.elemente[1])-1.563))))**(-0.6)))))*1.040000e+02));
PHI.elemente[5,8] := 0;
PHI.elemente[6,1] := 0;
PHI.elemente[6,2] := 0;
PHI.elemente[6,3] := 0;
PHI.elemente[6,4] := 0;
PHI.elemente[6,5] := 0;
PHI.elemente[6,6] := 1;
PHI.elemente[6,7] := 0;
PHI.elemente[6,8] := 0;
PHI.elemente[7,1] := 0;
PHI.elemente[7,2] := 0;
PHI.elemente[7,3] := 0;
PHI.elemente[7,4] := 0;
PHI.elemente[7,5] := 0;
PHI.elemente[7,6] := 0;
PHI.elemente[7,7] := 1;
PHI.elemente[7,8] := 0;
PHI.elemente[8,1] := 0;
PHI.elemente[8,2] := 0;
PHI.elemente[8,3] := 0;
PHI.elemente[8,4] := 0;
PHI.elemente[8,5] := 0;
PHI.elemente[8,6] := 0;
PHI.elemente[8,7] := 0;
PHI.elemente[8,8] := 1;
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// Jacobian of function h2 at value x_dach_minus_n
// H = Jacobi_h2 (x_dach_minus_n)
// ==> here: H = Jacobi_h2 (X_MinusK)
CONST_ALGO_JAKOBI_H2:
// initialising of matrices
H.zeilenzahl := NumberYn; // H = (NumberYn x NumberX)
H.spaltenzahl := NumberX;
FOR i:= 1 TO H.zeilenzahl DO
FOR j:=1 TO H.spaltenzahl DO
H.elemente[i,j]:= 0;
END_FOR;
END_FOR;
// elements of Jacobian H2:
H.elemente [1,3] := 1;
// -------------------------------------------------------------------
// -------------------------------------------------------------------
// estimated output vectors
// y_dach_minus_k = h_func2(x_dach_minus_n);
// ==> here: Y_MinusK = h_func2(X_MinusK);
CONST_ALGO_H_FUNC2:
// Matrix-Initialisierung
Y_MinusK.zeilenzahl := NumberYn; // Y_MinusK = (NumberYn x 1)
// elements of output vector h2:
Y_MinusK.elemente[1] := X_MinusK.elemente[3];
// -------------------------------------------------------------------
// -------------------------------------------------------------------
ELSE :
Error := TRUE; // incorrect parameterisation
END_CASE;
END_IF;
END_FUNCTION
Sources taken from : SIMATIC PCS 7 Kalman Configurator V9.0
Ïðîñìîòðîâ: 1584
Êîììåíòàðèè ê ìàòåðèàëó
Âàø áóäåò ïåðâûì.
Äîáàâèòü êîììåíòàðèé