Kalman Filter Based Parameter Estimation

Submitted To: Dr. Abdul Qayyum Khan

Submitted By : Hassan Shahid Hassan Saleem Sameer Farooq (53701) (53308) (52090)

M. Shahzad Afzal (53935) Kumail Raza Jafri (52685) Zaeem ul Haq (51376)

KALMAN FILTER BASED PARAMETER ESTIMATION

Abstract
The objective of this lab is to gain some insight into applications of extended kalman filter in parameter estimation of non linear systems. We will consider a 2 nd order system with 2 states and an additional state of system parameter to be estimated. The resulting plant will be non linear. The additional state which is damping ratio of system, is estimated by extended kalman filter. It makes use of linearized version of the plant. We will implement it in MATLAB using both SIMULINK model and embedded functions. The graphs will show the successful estimation of damping ratio by extended kalman filter.

INTRODUCTION
The Kalman filter, also known as linear quadratic estimation(LQE), is an algorithm which uses a series of measurements observed over time, containing noise (random variations) and other inaccuracies, and produces estimates of unknown variables that tend to be more precise than those that would be based on a single measurement alone. More formally, the Kalman filter operates recursively on streams of noisy input data to produce a statistically optimal estimate of the underlying system state. The filter is named for Rudolf (Rudy) E. Klmn, one of the primary developers of its theory.

HISTORICAL DEVELOPMENT
The filter is named after Hungarian emigr Rudolf E. Klmn, though Thorvald Nicolai Thiele and Peter Swerling developed a similar algorithm earlier. Richard S. Bucy of the University of Southern California contributed to the theory, leading to it often being called the Kalman-Bucy filter. Stanley F. Schmidt is generally credited with developing the first implementation of a Kalman filter. It was during a visit by Kalman to the NASA Ames Research Center that he saw the applicability of his ideas to the problem of trajectory estimation for the Apollo program, leading to its incorporation in the Apollo navigation computer. This Kalman filter was first described and partially developed in technical papers by Swerling (1958), Kalman (1960) and Kalman and Bucy (1961).

ALGORITHM
The algorithm works in a two-step process: in the prediction step, the Kalman filter produces estimates of the current state variables, along with their uncertainties. Once the outcome of the next measurement (necessarily corrupted with some amount of error, including random noise) is observed, these estimates are updated using a weighted average, with more weight being given to estimates with higher certainty. Because of the algorithm's recursive nature, it can run in real time using only the present input measurements and the previously calculated state; no additional past information is required.

LIMITATIONS AND EXTENSIONS

From a theoretical standpoint, the main assumption of the Kalman filter is that the underlying system is a linear dynamical system and that all error terms and measurements have a Gaussian distribution (often a multivariate Gaussian distribution). Extensions and generalizations to the method have also been developed, such as the Extended Kalman Filter and the Unscented Kalman filter which work on nonlinear systems. The underlying model is a Bayesian model similar to a hidden Markov model but where the state space of the latent variables is continuous and where all latent and observed variables have Gaussian distributions.

APPLICATIONS
The Kalman filter has numerous applications in technology. A common application is for guidance, navigation and control of vehicles, particularly aircraft and spacecraft. Kalman filters have been vital in the implementation of the navigation systems of U.S. Navy nuclear ballistic missile submarines, and in the guidance and navigation systems of cruise missiles such as the U.S. Navy's Tomahawk missile and the U.S. Air Force's Air Launched Cruise Missile. It is also used in the guidance and navigation systems of the NASA Space Shuttle and the attitude control and navigation systems of the International Space Station.

THE DISCRETE KALMAN FILTER

In 1960, R.E. Kalman published his famous paper describing a recursive solution to the discrete-data linear filtering problem [Kalman60]. Since that time, due in large part to advances in digital computing, the Kalman filter has been the subject of extensive research and application, particularly in the area of autonomous or assisted navigation. A very "friendly" introduction to the general idea of the Kalman filter can be found in Chapter 1 of [Maybeck79], while a more complete introductory discussion can be found in [Sorenson70], which also contains some interesting historical narrative. More extensive references include [Gelb74], [Maybeck79], [Lewis86], [Brown92], and [Jacobs93].

PROCESS
The Kalman filter addresses the general problem of trying to estimate the states of a discrete-time controlled process that is governed by the linear stochastic difference equation

with measurement equation is given as

The random variables and represent the process and measurement noise (respectively). They are assumed to be independent (of each other), white, and with normal probability distributions.

THE COMPUTATIONAL ORIGINS OF THE FILTER

We define (note the "super minus") to be our a priori state estimate at step k given knowledge of the process prior to step k, and to be our a posteriori state estimate at step k given measurement . We can then define a priori and a posteriori estimate errors as

In deriving the equations for the Kalman filter, we begin with the goal of finding an equation that computes an a posteriori state estimate as a linear combination of an a priori estimate and a weighted difference between an actual measurement and a measurement prediction as shown below in.

The matrix K in above equation is chosen to be the gain or blending factor that minimizes the a posteriori error covariance.

The Discrete Kalman Filter Algorithm

The Kalman filter estimates a process by using a form of feedback control: the filter estimates the process state at some time and then obtains feedback in the form of (noisy) measurements. As such, the equations for the Kalman filter fall into two groups: time update equations and measurement update equations. The time update equations are responsible for projecting forward (in time) the current state and error covariance estimates to obtain the a priori estimates for the next time step. The measurement update equations are responsible for the feedback--i.e. for incorporating a new measurement into the a priori estimate to obtain an improved a posteriori estimate. The time update equations can also be thought of as predictor equations, while the measurement update equations can be thought of as corrector equations. Indeed the final estimation algorithm resembles that of a predictor-corrector algorithm for solving numerical problems as shown below in Figure 1-1 .

Figure 1: The ongoing discrete Kalman filter cycle. The time update projects the current state estimate ahead in time. The measurement update adjusts the projected estimate by an actual measurement at that time.

Prediction Step

Computing the predicted state estimate: Computing the predicted measurement: Computing the a priori covariance matrix: ( )

Correction Step

Computing the Kalman gain:

Conditioning the predicted estimate on the measurement: given

Computing the a posteriori covariance matrix { }

So in summary first task during the measurement updated is to compute the kalman gain. The next step is to actually measure the process to obtain Zk, and then to generate an a posteriori state estimate by incorporating the measurement in equations. Graphically it can be viewed as

S-FUNCTION (LEVEL-I) IMPLEMENTATION

Level-I Embedded MATLAB Function

For Plant:
function y = plant(u) %#eml persistent x; if isempty(x) x = ones(2,1); end A = [0 1;-36 -4.2]; B = [0;1]; C = [1 0]; D = 0; T = 0.01; x = A*T*x+x + B*u; y = C*x+D*u;

For Estimator:
function x = kalman(z,Qk,Rk) persistent xe; persistent Pk; if isempty(xe) xe = [0;0;0]; end if isempty(Pk) Pk = [2 0 0;0 3 0;0 0 1]; end xe(1) = xe(2) = xe(3) = ze = [1 xe(1) + 0.0100*xe(2); -0.3600*xe(1) -0.1200*xe(2)*xe(3) + xe(2); xe(3); 0 0]*xe;

phi = [1 0.01 0;... -0.3600 -0.1200*xe(3)+1 -0.1200*xe(2);... 0 0 1]; Hk = [1 0 0 ]; Pk = phi*Pk*phi'+Qk; Kk= Pk*Hk'*inv(Hk*Pk*Hk' + Rk); Pk = (eye(3) - Kk*Hk)*Pk; xe = xe + Kk*(z - ze); x = xe;

Simulation Results:

Process Output 1.2 1 0.8 0.6

Output

0.4 0.2 0 -0.2 -0.4

10

15

20

25 30 Time (Sec)

35

40

45

50

Estimated States 2
State 1

0 -2 0 5 10 15 20 25 30 35 40 45 50

10
State 2

0 -10 0 5 10 15 20 25 30 35 40 45 50

Estimated Zeta

0.4 0.2 0 0 5 10 15 20 25 30 Time (sec) 35 40 45 50

SIMULINK IMPLEMENTATION

Overall Interconnection of Subsystems:

0 Constant

Process_Plant

Zero-Order Hold

Scope

Scope1

xe xe(k-1) ze

xe

phi

phi Pk-1

Kk Pk

Memory

predicted_state_output

phi gen Memory1

Scope3

Kalman Gain

xe Kk z ze

xe_new

corrector

Scope4 Scope2

FIGURE 1: OVERALL DIAGRAM OF PARAMETER ESTIMIZATION SYSTEM

0 1 B Matrix Multiply 1 u Product2 1 xo s Integrator 1 1 xos Matrix Multiply Product

[1 0] C Matrix Multiply Product1 1 z

0 1 -36 -4.2

FIGURE 2: PROCESS SIMULINK MODEL

0.01 Constant2 Product1 1 0.36 Constant3 [1 0 0] Product2 Hk 0.12 Constant4 Product3 xe Matrix Multiply Product

1 xe(k-1)

2 ze

FIGURE 3: ESTIMATED STATE PREDICTION MODEL

1 Constant1 0.01 Constant2 0 Constant3 -0.36 Constant4 0.12 1 xe Product 1 -0.12 Constant7 Product1 Constant5 Constant6 A 11 A A 12 13 A 21 A 22 A 23 A A 31 32 A 33

1 phi

Create 3x3 Matrix

FIGURE 4: LINEARIZED MODEL GENRATOR (PHI GENERATOR)

1 0 0

0 1 0 eye

0 0 1 Matrix Multiply Matrix Multiply Product3 Matrix Multiply General Inverse (LU) LU Inverse Product1

2 Pk

1 phi 2 Pk-1

Matrix Multiply Product

Pk_int

Product4

1 Kk

Transpose

Matrix Multiply Product2 [1 0 0] u

T

4.45 0 0

0 4.45 0 Qk

0 0 4.45

Hk

Transpose1 0.01 Rk

FIGURE 5: KALMAN GAIN CALCULATOR

1 xe 2 Kk 3 z 4 ze Matrix Multiply Product 1 xe_new

FIGURE 6: STATE ESTIMATE CORRECTOR

Simulation Results:
Process Output 1.2 1 0.8 0.6
Output

0.4 0.2 0 -0.2 -0.4

10

15

20

25 30 time (Sec)

35

40

45

50

0.4 0.35 0.3 0.25

Estimated Zeta

0.2 0.15 0.1 0.05 0 -0.05

10

15

20

25 30 Time (Sec)

35

40

45

50

CODE FOR PROCESS

/* * sam.c * * Code generation for model "sam.mdl". * * Model version : 1.1 * Simulink Coder version : 8.0 (R2011a) 09-Mar-2011 * C source code generated on : Mon Apr 02 19:45:35 2012 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping * Embedded hardware selection: 32-bit Generic * Code generation objectives: Unspecified * Validation result: Passed (0), Warning (1), Error (0) */ #include "sam.h" #include "sam_private.h" /* Block signals (auto storage) */ BlockIO_sam sam_B; /* Block states (auto storage) */ D_Work_sam sam_DWork; /* External inputs (root inport signals with auto storage) */ ExternalInputs_sam sam_U; /* External outputs (root outports fed by signals with auto storage) */ ExternalOutputs_sam sam_Y; /* Real-time model */ RT_MODEL_sam sam_M_; RT_MODEL_sam *const sam_M = &sam_M_; /* Model output function */ static void sam_output(int_T tid) { /* local block i/o variables */ real_T rtb_x1k; real_T rtb_x2k; real_T rtb_zK; real_T rtb_Gain2; real_T rtb_Gain3; /* UnitDelay: '<S1>/Unit Delay' */ rtb_x1k = sam_DWork.UnitDelay_DSTATE; /* UnitDelay: '<S1>/Unit Delay1' */ rtb_x2k = sam_DWork.UnitDelay1_DSTATE; /* Outport: '<Root>/x(k)' incorporates: * Constant: '<S1>/Constant' * SignalConversion: '<S1>/ConcatBufferAtVector ConcatenateIn1' * SignalConversion: '<S1>/ConcatBufferAtVector ConcatenateIn2' * SignalConversion: '<S1>/ConcatBufferAtVector ConcatenateIn3' */ sam_Y.xk[0] = rtb_x1k; sam_Y.xk[1] = rtb_x2k; sam_Y.xk[2] = sam_P.Constant_Value; /* Sum: '<Root>/Sum2' incorporates: * Inport: '<Root>/v(k)' */ rtb_zK = sam_U.vk + rtb_x1k; } /* Outport: '<Root>/z(k)' */ sam_Y.zk = rtb_zK; /* Model initialize function */ } /* Model update function */ static void sam_update(int_T tid) { /* Update for UnitDelay: '<S1>/Unit Delay' */ sam_DWork.UnitDelay_DSTATE = sam_B.x1k1; /* Update for UnitDelay: '<S1>/Unit Delay1' */ sam_DWork.UnitDelay1_DSTATE = sam_B.Sum1; /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++sam_M->Timing.clockTick0)) { ++sam_M->Timing.clockTickH0; } sam_M->Timing.t[0] = sam_M->Timing.clockTick0 * sam_M->Timing.stepSize0 + sam_M->Timing.clockTickH0 * sam_M->Timing.stepSize0 * 4294967296.0; /* tid is required for a uniform function interface. * Argument tid is not used in the function. */ UNUSED_PARAMETER(tid); /* Outport: '<Root>/h(k)' */ sam_Y.hk = rtb_x1k; /* Gain: '<Root>/Gain2' incorporates: * Inport: '<Root>/w(k)' */ rtb_Gain2 = sam_P.Gain2_Gain * sam_U.wk; /* Gain: '<Root>/Gain3' incorporates: * Inport: '<Root>/u(k)' */ rtb_Gain3 = sam_P.Gain3_Gain * sam_U.uk; /* Sum: '<S1>/Sum' */ sam_B.x1k1 = rtb_x2k + rtb_x1k; /* Sum: '<S1>/Sum1' incorporates: * Constant: '<S1>/Constant' * Gain: '<S1>/Gain' * Gain: '<S1>/Gain1' * Gain: '<S1>/Gain3' * Product: '<S1>/Product' */ sam_B.Sum1 = (((rtb_x2k - sam_P.Gain_Gain * rtb_x1k * sam_P.Constant_Value) sam_P.Gain1_Gain * rtb_x2k) + rtb_Gain2) + sam_P.Gain3_Gain_a * rtb_Gain3; /* tid is required for a uniform function interface. * Argument tid is not used in the function. */ UNUSED_PARAMETER(tid);

void sam_initialize(boolean_T firstTime) { (void)firstTime; /* Registration code */ /* initialize non-finites */ rt_InitInfAndNaN(sizeof(real_T)); /* initialize real-time model */ (void) memset((void *)sam_M, 0, sizeof(RT_MODEL_sam)); /* Initialize timing info */ { int_T *mdlTsMap = sam_M>Timing.sampleTimeTaskIDArray; mdlTsMap[0] = 0; sam_M->Timing.sampleTimeTaskIDPtr = (&mdlTsMap[0]); sam_M->Timing.sampleTimes = (&sam_M>Timing.sampleTimesArray[0]); sam_M->Timing.offsetTimes = (&sam_M>Timing.offsetTimesArray[0]); /* task periods */ sam_M->Timing.sampleTimes[0] = (0.01); /* task offsets */ sam_M->Timing.offsetTimes[0] = (0.0); } rtmSetTPtr(sam_M, &sam_M->Timing.tArray[0]); { int_T *mdlSampleHits = sam_M->Timing.sampleHitArray; mdlSampleHits[0] = 1; sam_M->Timing.sampleHits = (&mdlSampleHits[0]); } rtmSetTFinal(sam_M, 10.0); sam_M->Timing.stepSize0 = 0.01; /* Setup for data logging */ { static RTWLogInfo rt_DataLoggingInfo; sam_M->rtwLogInfo = &rt_DataLoggingInfo; } /* Setup for data logging */ { rtliSetLogXSignalInfo(sam_M->rtwLogInfo, (NULL)); rtliSetLogXSignalPtrs(sam_M->rtwLogInfo, (NULL)); rtliSetLogT(sam_M->rtwLogInfo, "tout"); rtliSetLogX(sam_M->rtwLogInfo, ""); rtliSetLogXFinal(sam_M->rtwLogInfo, ""); rtliSetSigLog(sam_M->rtwLogInfo, ""); rtliSetLogVarNameModifier(sam_M->rtwLogInfo, "rt_"); rtliSetLogFormat(sam_M->rtwLogInfo, 2); rtliSetLogMaxRows(sam_M->rtwLogInfo, 1000); rtliSetLogDecimation(sam_M->rtwLogInfo, 1); /* * Set pointers to the data and signal info for each output */ { static void * rt_LoggedOutputSignalPtrs[] = { &sam_Y.xk[0], &sam_Y.zk, &sam_Y.hk }; rtliSetLogYSignalPtrs(sam_M->rtwLogInfo, ((LogSignalPtrsType) rt_LoggedOutputSignalPtrs)); }

{ static int_T rt_LoggedOutputWidths[] = { 3, 1, 1 }; static int_T rt_LoggedOutputNumDimensions[] = { 2, 1, 1 }; static int_T rt_LoggedOutputDimensions[] = { 3, 1, 1, 1 }; static boolean_T rt_LoggedOutputIsVarDims[] = { 0, 0, 0 }; static int_T* rt_LoggedCurrentSignalDimensions[] = { (NULL), (NULL), (NULL), (NULL) }; static BuiltInDTypeId rt_LoggedOutputDataTypeIds[] = { SS_DOUBLE, SS_DOUBLE, SS_DOUBLE }; static int_T rt_LoggedOutputComplexSignals[] = { 0, 0, 0 }; static const char_T *rt_LoggedOutputLabels[] = { "", "z(K)", "" }; static const char_T *rt_LoggedOutputBlockNames[] = { "sam/x(k)", "sam/z(k)", "sam/h(k)" }; static RTWLogDataTypeConvert rt_RTWLogDataTypeConvert[] = { { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 }, { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 }, { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 } }; static RTWLogSignalInfo rt_LoggedOutputSignalInfo[] = { { 3, rt_LoggedOutputWidths, rt_LoggedOutputNumDimensions, rt_LoggedOutputDimensions, rt_LoggedOutputIsVarDims, rt_LoggedCurrentSignalDimensions, rt_LoggedOutputDataTypeIds, rt_LoggedOutputComplexSignals, (NULL),

{ rt_LoggedOutputLabels }, (NULL), (NULL), (NULL), { rt_LoggedOutputBlockNames }, { (NULL) }, (NULL), rt_RTWLogDataTypeConvert } }; rtliSetLogYSignalInfo(sam_M->rtwLogInfo, rt_LoggedOutputSignalInfo); /* set currSigDims field */ rt_LoggedCurrentSignalDimensions[0] &rt_LoggedOutputWidths[0]; rt_LoggedCurrentSignalDimensions[1] &rt_LoggedOutputWidths[0]; rt_LoggedCurrentSignalDimensions[2] &rt_LoggedOutputWidths[1]; rt_LoggedCurrentSignalDimensions[3] &rt_LoggedOutputWidths[2]; } rtliSetLogY(sam_M->rtwLogInfo, "yout"); } sam_M->solverInfoPtr = (&sam_M->solverInfo); sam_M->Timing.stepSize = (0.01); rtsiSetFixedStepSize(&sam_M->solverInfo, 0.01); rtsiSetSolverMode(&sam_M->solverInfo, SOLVER_MODE_SINGLETASKING); /* block I/O */ sam_M->ModelData.blockIO = ((void *) &sam_B); (void) memset(((void *) &sam_B), 0, sizeof(BlockIO_sam)); /* parameters */ sam_M->ModelData.defaultParam = ((real_T *)&sam_P);

*============================================= ===========================*/ void MdlOutputs(int_T tid) { sam_output(tid); } void MdlUpdate(int_T tid) { sam_update(tid); } void MdlInitializeSizes(void) { sam_M->Sizes.numContStates = (0); /* Number of continuous states */ sam_M->Sizes.numY = (5); /* Number of model outputs */ sam_M->Sizes.numU = (3); /* Number of model inputs */ sam_M->Sizes.sysDirFeedThru = (1); /* The model is direct feedthrough */ sam_M->Sizes.numSampTimes = (1); /* Number of sample times */ sam_M->Sizes.numBlocks = (23); /* Number of blocks */ sam_M->Sizes.numBlockIO = (2); /* Number of block outputs */ sam_M->Sizes.numBlockPrms = (8); /* Sum of parameter "widths" */ } void MdlInitializeSampleTimes(void) { } void MdlInitialize(void) { /* InitializeConditions for UnitDelay: '<S1>/Unit Delay' */ sam_DWork.UnitDelay_DSTATE = sam_P.UnitDelay_X0; /* InitializeConditions for UnitDelay: '<S1>/Unit Delay1' */ sam_DWork.UnitDelay1_DSTATE = sam_P.UnitDelay1_X0; }

= = = =

/* states (dwork) */ sam_M->Work.dwork = ((void *) &sam_DWork); (void) memset((void *)&sam_DWork, 0, sizeof(D_Work_sam)); /* external inputs */ sam_M->ModelData.inputs = (((void*)&sam_U)); (void) memset((void *)&sam_U, 0, sizeof(ExternalInputs_sam)); /* external outputs */ sam_M->ModelData.outputs = (&sam_Y); (void) memset((void *)&sam_Y, 0, sizeof(ExternalOutputs_sam)); } /* Model terminate function */ void sam_terminate(void) { } /*============================================ ============================* * Start of GRT compatible call interface *

void MdlStart(void) { MdlInitialize(); } void MdlTerminate(void) { sam_terminate(); } RT_MODEL_sam *sam(void) { sam_initialize(1); return sam_M; } /*============================================ ============================* * End of GRT compatible call interface * *============================================= ===========================*/

C CODE FOR PROCESS AND KALMAN FILTER

/* * hassan3.c * * Code generation for model "hassan3.mdl". * * Model version : 1.4 * Simulink Coder version : 8.1 (R2011b) 08-Jul-2011 * C source code generated on : Mon Apr 02 22:03:20 2012 * * Target selection: grt.tlc * Note: GRT includes extra infrastructure and instrumentation for prototyping * Embedded hardware selection: 32-bit Generic * Code generation objectives: Unspecified * Validation result: Passed (0), Warning (1), Error (0) */ #include "hassan3.h" #include "hassan3_private.h" /* Block signals (auto storage) */ BlockIO_hassan3 hassan3_B; /* Continuous states */ ContinuousStates_hassan3 hassan3_X; /* Block states (auto storage) */ D_Work_hassan3 hassan3_DWork; /* Real-time model */ RT_MODEL_hassan3 hassan3_M_; RT_MODEL_hassan3 *const hassan3_M = &hassan3_M_; /* * This function updates continuous states using the ODE3 fixed-step * solver algorithm */ static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) { /* Solver Matrices */ static const real_T rt_ODE3_A[3] = { 1.0/2.0, 3.0/4.0, 1.0 }; static const real_T rt_ODE3_B[3][3] = { { 1.0/2.0, 0.0, 0.0 }, { 0.0, 3.0/4.0, 0.0 }, { 2.0/9.0, 1.0/3.0, 4.0/9.0 } }; time_T t = rtsiGetT(si); time_T tnew = rtsiGetSolverStopTime(si); time_T h = rtsiGetStepSize(si); real_T *x = rtsiGetContStates(si); ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si); real_T *y = id->y; real_T *f0 = id->f[0]; real_T *f1 = id->f[1]; real_T *f2 = id->f[2]; real_T hB[3]; int_T i; int_T nXc = 2; rtsiSetSimTimeStep(si,MINOR_TIME_STEP); /* Save the state values at time t in y, we'll use x as ynew. */ (void) memcpy(y, x, nXc*sizeof(real_T)); /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ /* f0 = f(t,y) */ rtsiSetdX(si, f0); hassan3_derivatives(); /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */ hB[0] = h * rt_ODE3_B[0][0]; for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0]); } rtsiSetT(si, t + h*rt_ODE3_A[0]); rtsiSetdX(si, f1); hassan3_output(0); hassan3_derivatives(); /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */ for (i = 0; i <= 1; i++) { hB[i] = h * rt_ODE3_B[1][i]; } for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]); } rtsiSetT(si, t + h*rt_ODE3_A[1]); rtsiSetdX(si, f2); hassan3_output(0); hassan3_derivatives(); /* tnew = t + hA(3); ynew = y + f*hB(:,3); */ for (i = 0; i <= 2; i++) { hB[i] = h * rt_ODE3_B[2][i]; } for (i = 0; i < nXc; i++) { x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]); } rtsiSetT(si, tnew); rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); } void LUf_int32_Treal_T(real_T outU[], real_T outP[], const int32_T N) { int32_T k; int32_T c; int32_T r; int32_T idx; real_T tmp; int32_T p; real_T mTmp; boolean_T isDiagZero; real_T mAccum; /* S-Function (sdsplu2): '<S9>/LU Factorization' */ /* initialize row-pivot indices */ for (k = 0; k < N; k++) { outP[k] = (real_T)k + 1.0; } for (k = 0; k < N; k++) { p = k; /* Scan the lower triangular part of this column only. */ /* Record row of largest value */ idx = k * N; mTmp = outU[idx + k]; if (mTmp < 0.0) { mTmp = -mTmp; }

for (r = k + 1; r < N; r++) { tmp = outU[idx + r]; if (tmp < 0.0) { tmp = -tmp; } if (tmp > mTmp) { p = r; mTmp = tmp; } } /* swap rows if required */ if (p != k) { for (c = 0; c < N; c++) { idx = c * N; mTmp = outU[idx + p]; tmp = outU[idx + k]; outU[idx + p] = tmp; outU[idx + k] = mTmp; } /* swap pivot row indices */ tmp = outP[p]; outP[p] = outP[k]; outP[k] = tmp; } idx = k * N + k; isDiagZero = FALSE; if (outU[idx] == 0.0) { isDiagZero = TRUE; } if (!isDiagZero) { p = k * N; for (r = k + 1; r < N; r++) { mTmp = outU[p + r]; tmp = outU[idx]; outU[p + r] = mTmp / tmp; } for (c = k + 1; c < N; c++) { idx = c * N; for (r = k + 1; r < N; r++) { mAccum = outU[idx + r]; mTmp = outU[p + r]; tmp = outU[idx + k] * mTmp; mAccum -= tmp; outU[idx + r] = mAccum; } } } } /* End of S-Function (sdsplu2): '<S9>/LU Factorization' */ } /* Model output function */ void hassan3_output(int_T tid) { /* local block i/o variables */ real_T rtb_Sum1; real_T rtb_Integrator[2]; real_T rtb_Product1_h[3]; real_T rtb_Product_h; real_T rtb_Product_e[3]; real_T rtb_LUFactorization_o1; real_T rtb_Sum1_l; real_T rtb_VectorConcatenate[9]; real_T rtb_Sum[9]; real_T tmp[9]; int32_T i; real_T rtb_Sum_0[3]; int32_T i_0;

real_T rtb_TmpSignalConversionAtProd_0; real_T rtb_TmpSignalConversionAtProd_1; real_T rtb_TmpSignalConversionAtProd_2; if (rtmIsMajorTimeStep(hassan3_M)) { /* set solver stop time */ if (!(hassan3_M->Timing.clockTick0+1)) { rtsiSetSolverStopTime(&hassan3_M->solverInfo, ((hassan3_M->Timing.clockTickH0 + 1) * hassan3_M->Timing.stepSize0 * 4294967296.0)); } else { rtsiSetSolverStopTime(&hassan3_M->solverInfo, ((hassan3_M->Timing.clockTick0 + 1) * hassan3_M->Timing.stepSize0 + hassan3_M>Timing.clockTickH0 * hassan3_M->Timing.stepSize0 * 4294967296.0)); } } /* end MajorTimeStep */ /* Update absolute time of base rate at minor time step */ if (rtmIsMinorTimeStep(hassan3_M)) { hassan3_M->Timing.t[0] = rtsiGetT(&hassan3_M>solverInfo); } if (rtmIsMajorTimeStep(hassan3_M)) { /* Constant: '<S2>/xos' */ hassan3_B.xos[0] = hassan3_P.xos_Value[0]; hassan3_B.xos[1] = hassan3_P.xos_Value[1]; } /* Integrator: '<S2>/Integrator' */ if (hassan3_DWork.Integrator_IWORK.IcNeedsLoading) { hassan3_X.Integrator_CSTATE[0] = hassan3_B.xos[0]; hassan3_X.Integrator_CSTATE[1] = hassan3_B.xos[1]; hassan3_DWork.Integrator_IWORK.IcNeedsLoading = 0; } rtb_Integrator[0] = hassan3_X.Integrator_CSTATE[0]; rtb_Integrator[1] = hassan3_X.Integrator_CSTATE[1]; /* Product: '<S2>/Product1' incorporates: * Constant: '<S2>/C' */ hassan3_B.Product1 = hassan3_P.C_Value[0] * rtb_Integrator[0] + hassan3_P.C_Value[1] * rtb_Integrator[1]; if (rtmIsMajorTimeStep(hassan3_M)) { /* Scope: '<Root>/Scope' */ if (rtmIsMajorTimeStep(hassan3_M)) { StructLogVar *svar = (StructLogVar *)hassan3_DWork.Scope_PWORK.LoggedData; LogVar *var = svar->signals.values; /* time */ { double locTime = hassan3_M->Timing.t[1]; rt_UpdateLogVar((LogVar *)svar->time, &locTime, 0); } /* signals */ { real_T up0[1]; up0[0] = hassan3_B.Product1; rt_UpdateLogVar((LogVar *)var, up0, 0); } } /* Memory: '<Root>/Memory' */ rtb_Product_e[0] hassan3_DWork.Memory_PreviousInput[0]; rtb_Product_e[1] hassan3_DWork.Memory_PreviousInput[1]; rtb_Product_e[2] hassan3_DWork.Memory_PreviousInput[2];

= = =

/* Sum: '<S5>/Sum' incorporates: * Constant: '<S5>/Constant2' * Product: '<S5>/Product1' */ rtb_Product_h = hassan3_P.Constant2_Value rtb_Product_e[1] + rtb_Product_e[0]; /* Sum: '<S5>/Sum1' incorporates: * Constant: '<S5>/Constant3' * Constant: '<S5>/Constant4' * Product: '<S5>/Product2' * Product: '<S5>/Product3' */ rtb_LUFactorization_o1 = (rtb_Product_e[1] hassan3_P.Constant3_Value * rtb_Product_e[0]) hassan3_P.Constant4_Value rtb_Product_e[1] * rtb_Product_e[2];

/* Product: '<S4>/Product1' incorporates: * Constant: '<S4>/Constant7' */ rtb_VectorConcatenate[7] = hassan3_P.Constant7_Value * rtb_LUFactorization_o1; /* SignalConversion: '<S13>/ConcatBufferAtVector ConcatenateIn9' incorporates: * Constant: '<S4>/Constant1' */ rtb_VectorConcatenate[8] = hassan3_P.Constant1_Value; /* Product: '<S1>/Product' incorporates: * Math: '<S7>/Math Function' */ for (i = 0; i < 3; i++) { for (i_0 = 0; i_0 < 3; i_0++) { tmp[i + 3 * i_0] = 0.0; tmp[i + 3 * i_0] = tmp[3 * i_0 + i] + hassan3_DWork.Memory1_PreviousInput[i] rtb_VectorConcatenate[i_0]; tmp[i + 3 * i_0] = tmp[3 * i_0 + i] + hassan3_DWork.Memory1_PreviousInput[i + rtb_VectorConcatenate[i_0 + 3]; tmp[i + 3 * i_0] = tmp[3 * i_0 + i] + hassan3_DWork.Memory1_PreviousInput[i + rtb_VectorConcatenate[i_0 + 6]; } }

/* SignalConversion: '<S5>/TmpSignal ConversionAtProductInport2' */ rtb_TmpSignalConversionAtProd_0 = rtb_Product_h; rtb_TmpSignalConversionAtProd_1 = rtb_LUFactorization_o1; rtb_TmpSignalConversionAtProd_2 = rtb_Product_e[2]; /* Product: '<S5>/Product' incorporates: * Constant: '<S5>/Hk' * SignalConversion: '<S5>/TmpSignal ConversionAtProductInport2' */ rtb_Product_h = (hassan3_P.Hk_Value[0] * rtb_Product_h + hassan3_P.Hk_Value [1] * rtb_LUFactorization_o1) + hassan3_P.Hk_Value[2] * rtb_Product_e[2]; /* SignalConversion: '<S13>/ConcatBufferAtVector ConcatenateIn1' incorporates: * Constant: '<S4>/Constant1' */ rtb_VectorConcatenate[0] = hassan3_P.Constant1_Value; /* Constant: '<S4>/Constant4' */ rtb_VectorConcatenate[1] = hassan3_P.Constant4_Value_g; /* SignalConversion: '<S13>/ConcatBufferAtVector ConcatenateIn3' incorporates: * Constant: '<S4>/Constant3' */ rtb_VectorConcatenate[2] = hassan3_P.Constant3_Value_p; /* Constant: '<S4>/Constant2' */ rtb_VectorConcatenate[3] = hassan3_P.Constant2_Value_h; /* Sum: '<S4>/Sum' incorporates: * Constant: '<S4>/Constant5' * Constant: '<S4>/Constant6' * Product: '<S4>/Product' */ rtb_VectorConcatenate[4] = hassan3_P.Constant5_Value hassan3_P.Constant6_Value * rtb_Product_e[2]; /* SignalConversion: '<S13>/ConcatBufferAtVector ConcatenateIn6' incorporates: * Constant: '<S4>/Constant3' */ rtb_VectorConcatenate[5] = hassan3_P.Constant3_Value_p; /* SignalConversion: '<S13>/ConcatBufferAtVector ConcatenateIn7' incorporates: * Constant: '<S4>/Constant3' */ rtb_VectorConcatenate[6] = hassan3_P.Constant3_Value_p;

3]

6]

/* Sum: '<S1>/Sum' incorporates: * Constant: '<S1>/Qk' * Product: '<S1>/Product' */ for (i = 0; i < 3; i++) { for (i_0 = 0; i_0 < 3; i_0++) { rtb_Sum[i + 3 * i_0] = ((tmp[3 * i_0 + 1] * rtb_VectorConcatenate[i + 3] + tmp[3 * i_0] * rtb_VectorConcatenate[i]) + tmp[3 * i_0 + 2] * rtb_VectorConcatenate[i + 6]) + hassan3_P.Qk_Value[3 * i_0 + i]; } } /* End of Sum: '<S1>/Sum' */ /* Math: '<S8>/Math Function' incorporates: * Constant: '<S1>/Hk' */ rtb_Product_e[0] = hassan3_P.Hk_Value_n[0]; rtb_Product_e[1] = hassan3_P.Hk_Value_n[1]; rtb_Product_e[2] = hassan3_P.Hk_Value_n[2]; /* Product: '<S1>/Product2' incorporates: * Constant: '<S1>/Hk' */ for (i = 0; i < 3; i++) { rtb_Sum_0[i] = rtb_Sum[i + 6] * rtb_Product_e[2] + (rtb_Sum[i + 3] * rtb_Product_e[1] + rtb_Sum[i] * rtb_Product_e[0]); } rtb_LUFactorization_o1 = rtb_Sum_0[0] + hassan3_P.Hk_Value_n[1] hassan3_P.Hk_Value_n[2] * rtb_Sum_0[2]; (hassan3_P.Hk_Value_n[0] * rtb_Sum_0[1]) * +

/* End of Product: '<S1>/Product2' */ /* Sum: '<S1>/Sum1' incorporates:

* Constant: '<S1>/Rk' */ rtb_Sum1 = rtb_LUFactorization_o1 + hassan3_P.Rk_Value; /* S-Function (sdsplu2): '<S9>/LU Factorization' */ rtb_LUFactorization_o1 = rtb_Sum1; LUf_int32_Treal_T(&rtb_LUFactorization_o1, &hassan3_B.LUFactorization_o2, 1); /* DSP System Toolbox Permute Matrix (sdspperm2) '<S9>/Permute Matrix' */ /* Scalar output equals scalar input. */ rtb_Sum1_l = hassan3_B.IdentityMatrix; /* S-Function (sdspfbsub2): '<S9>/Backward Substitution' */ rtb_Sum1_l /= rtb_LUFactorization_o1; /* Product: '<S1>/Product1' */ for (i = 0; i < 3; i++) { rtb_Product1_h[i] = 0.0; rtb_Product1_h[i] += rtb_Product_e[0] * rtb_Sum1_l * rtb_Sum[i]; rtb_Product1_h[i] += rtb_Sum[i + 3] * (rtb_Product_e[1] * rtb_Sum1_l); rtb_Product1_h[i] += rtb_Sum[i + 6] * (rtb_Product_e[2] * rtb_Sum1_l); } /* End of Product: '<S1>/Product1' */ /* ZeroOrderHold: '<Root>/Zero-Order Hold' */ rtb_Sum1_l = hassan3_B.Product1; /* Sum: '<S3>/Sum1' */ rtb_Sum1_l -= rtb_Product_h; /* Product: '<S3>/Product' */ rtb_Product_e[0] = rtb_Product1_h[0] * rtb_Sum1_l; rtb_Product_e[1] = rtb_Product1_h[1] * rtb_Sum1_l; rtb_Product_e[2] = rtb_Product1_h[2] * rtb_Sum1_l; /* Sum: '<S3>/Sum' */ hassan3_B.Sum[0] = rtb_TmpSignalConversionAtProd_0 + rtb_Product_e[0]; hassan3_B.Sum[1] = rtb_TmpSignalConversionAtProd_1 + rtb_Product_e[1]; hassan3_B.Sum[2] = rtb_TmpSignalConversionAtProd_2 + rtb_Product_e[2]; /* Scope: '<Root>/Scope4' */ if (rtmIsMajorTimeStep(hassan3_M)) { StructLogVar *svar = (StructLogVar *)hassan3_DWork.Scope4_PWORK.LoggedData; LogVar *var = svar->signals.values; /* time */ { double locTime = hassan3_M->Timing.t[1]; rt_UpdateLogVar((LogVar *)svar->time, &locTime, 0); } /* signals */ { real_T up0[1]; up0[0] = hassan3_B.Sum[2]; rt_UpdateLogVar((LogVar *)var, up0, 0); } } /* Sum: '<S1>/Sum2' incorporates: * Constant: '<S1>/Hk' * Constant: '<S1>/eye' * Product: '<S1>/Product3' * Product: '<S1>/Product4' */ *

for (i = 0; i < 3; i++) { tmp[i] = hassan3_P.eye_Value[i] - rtb_Product1_h[i] * hassan3_P.Hk_Value_n[0]; tmp[i + 3] = hassan3_P.eye_Value[i + 3] - rtb_Product1_h[i] hassan3_P.Hk_Value_n[1]; tmp[i + 6] = hassan3_P.eye_Value[i + 6] - rtb_Product1_h[i] * hassan3_P.Hk_Value_n[2]; } /* End of Sum: '<S1>/Sum2' */ /* Product: '<S1>/Product3' */ for (i = 0; i < 3; i++) { for (i_0 = 0; i_0 < 3; i_0++) { hassan3_B.Product3[i + 3 * i_0] = 0.0; hassan3_B.Product3[i + 3 * i_0] = hassan3_B.Product3[3 * i_0 + i] + rtb_Sum[3 * i_0] * tmp[i]; hassan3_B.Product3[i + 3 * i_0] = rtb_Sum[3 * i_0 + 1] * tmp[i + 3] + hassan3_B.Product3[3 * i_0 + i]; hassan3_B.Product3[i + 3 * i_0] = rtb_Sum[3 * i_0 + 2] * tmp[i + 6] + hassan3_B.Product3[3 * i_0 + i]; } } /* Product: '<S2>/Product2' incorporates: * Constant: '<Root>/Constant' * Constant: '<S2>/B' */ hassan3_B.Product2[0] = hassan3_P.B_Value[0] hassan3_P.Constant_Value; hassan3_B.Product2[1] = hassan3_P.B_Value[1] hassan3_P.Constant_Value; } /* Sum: '<S2>/Sum' incorporates: * Constant: '<S2>/A' * Product: '<S2>/Product' */ hassan3_B.Sum_d[0] = (hassan3_P.A_Value[0] rtb_Integrator[0] + hassan3_P.A_Value[2] * rtb_Integrator[1]) + hassan3_B.Product2[0]; hassan3_B.Sum_d[1] = (hassan3_P.A_Value[1] rtb_Integrator[0] + hassan3_P.A_Value[3] * rtb_Integrator[1]) + hassan3_B.Product2[1]; /* tid is required for a uniform function interface. * Argument tid is not used in the function. */ UNUSED_PARAMETER(tid); } /* Model update function */ void hassan3_update(int_T tid) { if (rtmIsMajorTimeStep(hassan3_M)) { /* Update for Memory: '<Root>/Memory' */ hassan3_DWork.Memory_PreviousInput[0] hassan3_B.Sum[0]; hassan3_DWork.Memory_PreviousInput[1] hassan3_B.Sum[1]; hassan3_DWork.Memory_PreviousInput[2] hassan3_B.Sum[2];

* *

= = =

/* Update for Memory: '<Root>/Memory1' */ memcpy((void *)hassan3_DWork.Memory1_PreviousInput, (void *) (&hassan3_B.Product3[0]), 9U * sizeof(real_T)); }

if (rtmIsMajorTimeStep(hassan3_M)) { rt_ertODEUpdateContinuousStates(&hassan3_M>solverInfo); } /* Update absolute time for base rate */ /* The "clockTick0" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick0" * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick0 and the high bits * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. */ if (!(++hassan3_M->Timing.clockTick0)) { ++hassan3_M->Timing.clockTickH0; } hassan3_M->Timing.t[0] rtsiGetSolverStopTime(&hassan3_M->solverInfo); { /* Update absolute timer for sample time: [0.01s, 0.0s] */ /* The "clockTick1" counts the number of times the code of this task has * been executed. The absolute time is the multiplication of "clockTick1" * and "Timing.stepSize1". Size of "clockTick1" ensures timer will not * overflow during the application lifespan selected. * Timer of this task consists of two 32 bit unsigned integers. * The two integers represent the low bits Timing.clockTick1 and the high bits * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. */ if (!(++hassan3_M->Timing.clockTick1)) { ++hassan3_M->Timing.clockTickH1; } hassan3_M->Timing.t[1] = hassan3_M->Timing.clockTick1 * hassan3_M->Timing.stepSize1 + hassan3_M>Timing.clockTickH1 * hassan3_M->Timing.stepSize1 * 4294967296.0; } /* tid is required for a uniform function interface. * Argument tid is not used in the function. */ UNUSED_PARAMETER(tid); } /* Derivatives for root system: '<Root>' */ void hassan3_derivatives(void) { /* Derivatives for Integrator: '<S2>/Integrator' */ { ((StateDerivatives_hassan3 *) hassan3_M>ModelData.derivs) ->Integrator_CSTATE[0] = hassan3_B.Sum_d[0]; ((StateDerivatives_hassan3 *) hassan3_M>ModelData.derivs) ->Integrator_CSTATE[1] = hassan3_B.Sum_d[1]; } } /* Model initialize function */ void hassan3_initialize(boolean_T firstTime) { (void)firstTime; =

/* Registration code */ /* initialize non-finites */ rt_InitInfAndNaN(sizeof(real_T)); /* initialize real-time model */ (void) memset((void *)hassan3_M, 0, sizeof(RT_MODEL_hassan3)); { /* Setup solver object */ rtsiSetSimTimeStepPtr(&hassan3_M->solverInfo, &hassan3_M->Timing.simTimeStep); rtsiSetTPtr(&hassan3_M->solverInfo, &rtmGetTPtr(hassan3_M)); rtsiSetStepSizePtr(&hassan3_M->solverInfo, &hassan3_M>Timing.stepSize0); rtsiSetdXPtr(&hassan3_M->solverInfo, &hassan3_M>ModelData.derivs); rtsiSetContStatesPtr(&hassan3_M->solverInfo, &hassan3_M->ModelData.contStates); rtsiSetNumContStatesPtr(&hassan3_M->solverInfo, &hassan3_M->Sizes.numContStates); rtsiSetErrorStatusPtr(&hassan3_M->solverInfo, (&rtmGetErrorStatus(hassan3_M))); rtsiSetRTModelPtr(&hassan3_M->solverInfo, hassan3_M); } rtsiSetSimTimeStep(&hassan3_M->solverInfo, MAJOR_TIME_STEP); hassan3_M->ModelData.intgData.y = hassan3_M>ModelData.odeY; hassan3_M->ModelData.intgData.f[0] = hassan3_M>ModelData.odeF[0]; hassan3_M->ModelData.intgData.f[1] = hassan3_M>ModelData.odeF[1]; hassan3_M->ModelData.intgData.f[2] = hassan3_M>ModelData.odeF[2]; hassan3_M->ModelData.contStates = ((real_T *) &hassan3_X); rtsiSetSolverData(&hassan3_M->solverInfo, (void *) &hassan3_M->ModelData.intgData); rtsiSetSolverName(&hassan3_M->solverInfo,"ode3"); /* Initialize timing info */ { int_T *mdlTsMap = >Timing.sampleTimeTaskIDArray; mdlTsMap[0] = 0; mdlTsMap[1] = 1; hassan3_M->Timing.sampleTimeTaskIDPtr (&mdlTsMap[0]); hassan3_M->Timing.sampleTimes = >Timing.sampleTimesArray[0]); hassan3_M->Timing.offsetTimes = >Timing.offsetTimesArray[0]); /* task periods */ hassan3_M->Timing.sampleTimes[0] = (0.0); hassan3_M->Timing.sampleTimes[1] = (0.01); /* task offsets */ hassan3_M->Timing.offsetTimes[0] = (0.0); hassan3_M->Timing.offsetTimes[1] = (0.0); } rtmSetTPtr(hassan3_M, &hassan3_M->Timing.tArray[0]); { int_T *mdlSampleHits = hassan3_M>Timing.sampleHitArray; mdlSampleHits[0] = 1; mdlSampleHits[1] = 1; hassan3_M->Timing.sampleHits = (&mdlSampleHits[0]);

hassan3_M-

= (&hassan3_M(&hassan3_M-

} rtmSetTFinal(hassan3_M, 50.0); hassan3_M->Timing.stepSize0 = 0.01; hassan3_M->Timing.stepSize1 = 0.01; rtmSetFirstInitCond(hassan3_M, 1); /* Setup for data logging */ { static RTWLogInfo rt_DataLoggingInfo; hassan3_M->rtwLogInfo = &rt_DataLoggingInfo; } /* Setup for data logging */ { rtliSetLogXSignalInfo(hassan3_M->rtwLogInfo, (NULL)); rtliSetLogXSignalPtrs(hassan3_M->rtwLogInfo, (NULL)); rtliSetLogT(hassan3_M->rtwLogInfo, "tout"); rtliSetLogX(hassan3_M->rtwLogInfo, ""); rtliSetLogXFinal(hassan3_M->rtwLogInfo, ""); rtliSetSigLog(hassan3_M->rtwLogInfo, ""); rtliSetLogVarNameModifier(hassan3_M->rtwLogInfo, "rt_"); rtliSetLogFormat(hassan3_M->rtwLogInfo, 2); rtliSetLogMaxRows(hassan3_M->rtwLogInfo, 1000); rtliSetLogDecimation(hassan3_M->rtwLogInfo, 1); rtliSetLogY(hassan3_M->rtwLogInfo, ""); rtliSetLogYSignalInfo(hassan3_M->rtwLogInfo, (NULL)); rtliSetLogYSignalPtrs(hassan3_M->rtwLogInfo, (NULL)); } hassan3_M->solverInfoPtr = (&hassan3_M->solverInfo); hassan3_M->Timing.stepSize = (0.01); rtsiSetFixedStepSize(&hassan3_M->solverInfo, 0.01); rtsiSetSolverMode(&hassan3_M->solverInfo, SOLVER_MODE_SINGLETASKING); /* block I/O */ hassan3_M->ModelData.blockIO = ((void *) &hassan3_B); (void) memset(((void *) &hassan3_B), 0, sizeof(BlockIO_hassan3)); /* parameters */ hassan3_M->ModelData.defaultParam *)&hassan3_P); /* states (continuous) */ { real_T *x = (real_T *) &hassan3_X; hassan3_M->ModelData.contStates = (x); (void) memset((void *)&hassan3_X, 0, sizeof(ContinuousStates_hassan3)); } /* states (dwork) */ hassan3_M->Work.dwork = ((void *) &hassan3_DWork); (void) memset((void *)&hassan3_DWork, 0, sizeof(D_Work_hassan3)); } /* Model terminate function */ void hassan3_terminate(void) { } /*============================================ ============================* * Start of GRT compatible call interface * *============================================= ===========================*/ /* Solver interface called by GRT_Main */ #ifndef USE_GENERATED_SOLVER void rt_ODECreateIntegrationData(RTWSolverInfo *si)

{ UNUSED_PARAMETER(si); return; } /* do nothing */ void rt_ODEDestroyIntegrationData(RTWSolverInfo *si) { UNUSED_PARAMETER(si); return; } /* do nothing */ void rt_ODEUpdateContinuousStates(RTWSolverInfo *si) { UNUSED_PARAMETER(si); return; } /* do nothing */ #endif void MdlOutputs(int_T tid) { hassan3_output(tid); } void MdlUpdate(int_T tid) { hassan3_update(tid); } void MdlInitializeSizes(void) { hassan3_M->Sizes.numContStates = (2);/* Number of continuous states */ hassan3_M->Sizes.numY = (0); /* Number of model outputs */ hassan3_M->Sizes.numU = (0); /* Number of model inputs */ hassan3_M->Sizes.sysDirFeedThru = (0);/* The model is not direct feedthrough */ hassan3_M->Sizes.numSampTimes = (2); /* Number of sample times */ hassan3_M->Sizes.numBlocks = (67); /* Number of blocks */ hassan3_M->Sizes.numBlockIO = (8); /* Number of block outputs */ hassan3_M->Sizes.numBlockPrms = (58);/* Sum of parameter "widths" */ } void MdlInitializeSampleTimes(void) { } void MdlInitialize(void) { /* InitializeConditions for Integrator: '<S2>/Integrator' */ if (rtmIsFirstInitCond(hassan3_M)) { hassan3_X.Integrator_CSTATE[0] = 1.0; hassan3_X.Integrator_CSTATE[1] = 1.0; } hassan3_DWork.Integrator_IWORK.IcNeedsLoading = 1; /* InitializeConditions for Memory: '<Root>/Memory' */ hassan3_DWork.Memory_PreviousInput[0] hassan3_P.Memory_X0[0]; hassan3_DWork.Memory_PreviousInput[1] hassan3_P.Memory_X0[1]; hassan3_DWork.Memory_PreviousInput[2] hassan3_P.Memory_X0[2];

((real_T

= = =

/* InitializeConditions for Memory: '<Root>/Memory1' */ memcpy((void *)hassan3_DWork.Memory1_PreviousInput, (void *) hassan3_P.Memory1_X0, 9U * sizeof(real_T));

/* set "at time zero" to false */ if (rtmIsFirstInitCond(hassan3_M)) { rtmSetFirstInitCond(hassan3_M, 0); } } void MdlStart(void) { /* Start for Constant: '<S2>/xos' */ hassan3_B.xos[0] = hassan3_P.xos_Value[0]; hassan3_B.xos[1] = hassan3_P.xos_Value[1]; /* Start for Scope: '<Root>/Scope' */ { RTWLogSignalInfo rt_ScopeSignalInfo; static int_T rt_ScopeSignalWidths[] = { 1 }; static int_T rt_ScopeSignalNumDimensions[] = { 2 }; static int_T rt_ScopeSignalDimensions[] = { 1, 1 }; static void *rt_ScopeCurrSigDims[] = { (NULL), (NULL) }; static int_T rt_ScopeCurrSigDimsSize[] = { 4, 4 };

} /* Start for S-Function (sdspeye): '<S6>/Identity Matrix' */ /* Fill in 1's on the diagonal. */ hassan3_B.IdentityMatrix = 1.0; /* Start for Scope: '<Root>/Scope4' */ { RTWLogSignalInfo rt_ScopeSignalInfo; static int_T rt_ScopeSignalWidths[] = { 1 }; static int_T rt_ScopeSignalNumDimensions[] = { 1 }; static int_T rt_ScopeSignalDimensions[] = { 1 }; static void *rt_ScopeCurrSigDims[] = { (NULL) }; static int_T rt_ScopeCurrSigDimsSize[] = { 4 }; static const char_T *rt_ScopeSignalLabels[] = { "" }; static char_T rt_ScopeSignalTitles[] = ""; static int_T rt_ScopeSignalTitleLengths[] = { 0 }; static boolean_T rt_ScopeSignalIsVarDims[] = { 0 };

static const char_T *rt_ScopeSignalLabels[] = { "" }; static int_T rt_ScopeSignalPlotStyles[] = { 1 }; static char_T rt_ScopeSignalTitles[] = ""; static int_T rt_ScopeSignalTitleLengths[] = { 0 }; static boolean_T rt_ScopeSignalIsVarDims[] = { 0 }; static int_T rt_ScopeSignalPlotStyles[] = { 0 }; BuiltInDTypeId dTypes[1] = { SS_DOUBLE }; static char_T rt_ScopeBlockName[] = "hassan3/Scope"; rt_ScopeSignalInfo.numSignals = 1; rt_ScopeSignalInfo.numCols = rt_ScopeSignalWidths; rt_ScopeSignalInfo.numDims = rt_ScopeSignalNumDimensions; rt_ScopeSignalInfo.dims = rt_ScopeSignalDimensions; rt_ScopeSignalInfo.isVarDims = rt_ScopeSignalIsVarDims; rt_ScopeSignalInfo.currSigDims = rt_ScopeCurrSigDims; rt_ScopeSignalInfo.currSigDimsSize = rt_ScopeCurrSigDimsSize; rt_ScopeSignalInfo.dataTypes = dTypes; rt_ScopeSignalInfo.complexSignals = (NULL); rt_ScopeSignalInfo.frameData = (NULL); rt_ScopeSignalInfo.labels.cptr = rt_ScopeSignalLabels; rt_ScopeSignalInfo.titles = rt_ScopeSignalTitles; rt_ScopeSignalInfo.titleLengths = rt_ScopeSignalTitleLengths; rt_ScopeSignalInfo.plotStyles = rt_ScopeSignalPlotStyles; rt_ScopeSignalInfo.blockNames.cptr = (NULL); rt_ScopeSignalInfo.stateNames.cptr = (NULL); rt_ScopeSignalInfo.crossMdlRef = (NULL); rt_ScopeSignalInfo.dataTypeConvert = (NULL); hassan3_DWork.Scope_PWORK.LoggedData = rt_CreateStructLogVar( hassan3_M->rtwLogInfo, 0.0, rtmGetTFinal(hassan3_M), hassan3_M->Timing.stepSize0, (&rtmGetErrorStatus(hassan3_M)), "ScopeData", 1, 0, 1, 0.01, &rt_ScopeSignalInfo, rt_ScopeBlockName); if (hassan3_DWork.Scope_PWORK.LoggedData == (NULL)) return; BuiltInDTypeId dTypes[1] = { SS_DOUBLE }; static char_T rt_ScopeBlockName[] = "hassan3/Scope4"; rt_ScopeSignalInfo.numSignals = 1; rt_ScopeSignalInfo.numCols = rt_ScopeSignalWidths; rt_ScopeSignalInfo.numDims = rt_ScopeSignalNumDimensions; rt_ScopeSignalInfo.dims = rt_ScopeSignalDimensions; rt_ScopeSignalInfo.isVarDims = rt_ScopeSignalIsVarDims; rt_ScopeSignalInfo.currSigDims = rt_ScopeCurrSigDims; rt_ScopeSignalInfo.currSigDimsSize = rt_ScopeCurrSigDimsSize; rt_ScopeSignalInfo.dataTypes = dTypes; rt_ScopeSignalInfo.complexSignals = (NULL); rt_ScopeSignalInfo.frameData = (NULL); rt_ScopeSignalInfo.labels.cptr = rt_ScopeSignalLabels; rt_ScopeSignalInfo.titles = rt_ScopeSignalTitles; rt_ScopeSignalInfo.titleLengths = rt_ScopeSignalTitleLengths; rt_ScopeSignalInfo.plotStyles = rt_ScopeSignalPlotStyles; rt_ScopeSignalInfo.blockNames.cptr = (NULL); rt_ScopeSignalInfo.stateNames.cptr = (NULL); rt_ScopeSignalInfo.crossMdlRef = (NULL); rt_ScopeSignalInfo.dataTypeConvert = (NULL); hassan3_DWork.Scope4_PWORK.LoggedData = rt_CreateStructLogVar( hassan3_M->rtwLogInfo, 0.0, rtmGetTFinal(hassan3_M), hassan3_M->Timing.stepSize0, (&rtmGetErrorStatus(hassan3_M)), "est", 1, 0, 1, 0.01, &rt_ScopeSignalInfo, rt_ScopeBlockName); if (hassan3_DWork.Scope4_PWORK.LoggedData == (NULL)) return; } MdlInitialize(); } void MdlTerminate(void)

{ hassan3_terminate(); } RT_MODEL_hassan3 *hassan3(void) { hassan3_initialize(1); return hassan3_M;

} /*============================================ ============================* * End of GRT compatible call interface * *============================================= ===========================*/