copy of kalman report

8/2/2019 Copy of Kalman Report

1/24

Kalman Filter Based Parameter

Estimation

Submitted By :

Hassan Shahid (53701)

Hassan Saleem (53308)

Sameer Farooq (52090)

M. Shahzad Afzal (53935)

Kumail Raza Jafri (52685)

Zaeem ul Haq (51376)

Submitted To:

Dr. Abdul Qayyum Khan


2/24

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 2nd 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.


3/24

INTRODUCTIONThe 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 thosethat 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.
http://en.wikipedia.org/wiki/Algorithmhttp://en.wikipedia.org/wiki/Statistical_noisehttp://en.wikipedia.org/wiki/Recursionhttp://en.wikipedia.org/wiki/Estimation_theoryhttp://en.wikipedia.org/wiki/State_space_(controls)http://en.wikipedia.org/wiki/Rudolf_E._K%C3%A1lm%C3%A1nhttp://en.wikipedia.org/wiki/Rudolf_E._K%C3%A1lm%C3%A1nhttp://en.wikipedia.org/wiki/Thorvald_Nicolai_Thielehttp://en.wikipedia.org/wiki/Peter_Swerlinghttp://en.wikipedia.org/wiki/University_of_Southern_Californiahttp://en.wikipedia.org/wiki/University_of_Southern_Californiahttp://en.wikipedia.org/wiki/Stanley_F._Schmidthttp://en.wikipedia.org/wiki/NASA_Ames_Research_Centerhttp://en.wikipedia.org/wiki/Project_Apollohttp://en.wikipedia.org/wiki/Weighted_meanhttp://en.wikipedia.org/wiki/Real-time_Control_Systemhttp://en.wikipedia.org/wiki/Real-time_Control_Systemhttp://en.wikipedia.org/wiki/Weighted_meanhttp://en.wikipedia.org/wiki/Project_Apollohttp://en.wikipedia.org/wiki/NASA_Ames_Research_Centerhttp://en.wikipedia.org/wiki/Stanley_F._Schmidthttp://en.wikipedia.org/wiki/University_of_Southern_Californiahttp://en.wikipedia.org/wiki/University_of_Southern_Californiahttp://en.wikipedia.org/wiki/Peter_Swerlinghttp://en.wikipedia.org/wiki/Thorvald_Nicolai_Thielehttp://en.wikipedia.org/wiki/Rudolf_E._K%C3%A1lm%C3%A1nhttp://en.wikipedia.org/wiki/Rudolf_E._K%C3%A1lm%C3%A1nhttp://en.wikipedia.org/wiki/State_space_(controls)http://en.wikipedia.org/wiki/Estimation_theoryhttp://en.wikipedia.org/wiki/Recursionhttp://en.wikipedia.org/wiki/Statistical_noisehttp://en.wikipedia.org/wiki/Algorithm


4/24

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 whichwork 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 ofU.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 FILTERIn 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 digitalcomputing, 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
http://en.wikipedia.org/wiki/Linear_dynamical_systemhttp://en.wikipedia.org/wiki/Gaussian_distributionhttp://en.wikipedia.org/wiki/Gaussian_distributionhttp://en.wikipedia.org/wiki/Multivariate_Gaussian_distributionhttp://en.wikipedia.org/wiki/Extended_Kalman_Filterhttp://en.wikipedia.org/wiki/Kalman_Filter#Unscented_Kalman_filterhttp://en.wikipedia.org/wiki/Bayesian_modelhttp://en.wikipedia.org/wiki/Hidden_Markov_modelhttp://en.wikipedia.org/wiki/Hidden_Markov_modelhttp://en.wikipedia.org/wiki/Latent_variablehttp://en.wikipedia.org/wiki/Kalman_filter#Applicationshttp://en.wikipedia.org/wiki/Guidance,_navigation_and_control_(engineering)http://en.wikipedia.org/wiki/Guidance,_navigation_and_control_(engineering)http://en.wikipedia.org/wiki/U.S._Navyhttp://en.wikipedia.org/wiki/Ballistic_missile_submarinehttp://en.wikipedia.org/wiki/Tomahawk_missilehttp://en.wikipedia.org/wiki/Tomahawk_missilehttp://en.wikipedia.org/wiki/U.S._Air_Forcehttp://en.wikipedia.org/wiki/AGM-86_ALCMhttp://en.wikipedia.org/wiki/NASAhttp://en.wikipedia.org/wiki/Space_Shuttlehttp://en.wikipedia.org/wiki/Attitude_dynamics_and_controlhttp://en.wikipedia.org/wiki/International_Space_Stationhttp://en.wikipedia.org/wiki/International_Space_Stationhttp://en.wikipedia.org/wiki/Attitude_dynamics_and_controlhttp://en.wikipedia.org/wiki/Space_Shuttlehttp://en.wikipedia.org/wiki/NASAhttp://en.wikipedia.org/wiki/AGM-86_ALCMhttp://en.wikipedia.org/wiki/U.S._Air_Forcehttp://en.wikipedia.org/wiki/Tomahawk_missilehttp://en.wikipedia.org/wiki/Tomahawk_missilehttp://en.wikipedia.org/wiki/Ballistic_missile_submarinehttp://en.wikipedia.org/wiki/U.S._Navyhttp://en.wikipedia.org/wiki/Guidance,_navigation_and_control_(engineering)http://en.wikipedia.org/wiki/Guidance,_navigation_and_control_(engineering)http://en.wikipedia.org/wiki/Kalman_filter#Applicationshttp://en.wikipedia.org/wiki/Latent_variablehttp://en.wikipedia.org/wiki/Hidden_Markov_modelhttp://en.wikipedia.org/wiki/Hidden_Markov_modelhttp://en.wikipedia.org/wiki/Bayesian_modelhttp://en.wikipedia.org/wiki/Kalman_Filter#Unscented_Kalman_filterhttp://en.wikipedia.org/wiki/Extended_Kalman_Filterhttp://en.wikipedia.org/wiki/Multivariate_Gaussian_distributionhttp://en.wikipedia.org/wiki/Gaussian_distributionhttp://en.wikipedia.org/wiki/Gaussian_distributionhttp://en.wikipedia.org/wiki/Linear_dynamical_system


5/24

The random variables and represent the process and measurement noise (respectively). They areassumed 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 givenmeasurement . 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.
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/WELCH/kalman.1.html#36942http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/WELCH/kalman.1.html#36942


6/24

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) thecurrent 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:
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/WELCH/kalman.1.html#36550http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/WELCH/kalman.1.html#36550


7/24

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


8/24

S-FUNCTION (LEVEL-I)IMPLEMENTATION

Level-I Embedded MATLAB Function

For Plant:

function y = plant(u)%#emlpersistent x;if isempty(x)

x = ones(2,1);endA = [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];

endif isempty(Pk)Pk = [2 0 0;0 3 0;0 0 1];

end

xe(1) = xe(1) + 0.0100*xe(2);xe(2) = -0.3600*xe(1) -0.1200*xe(2)*xe(3) + xe(2);xe(3) = xe(3);ze = [1 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;


9/24

Simulation Results:

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

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

Time (Sec)

Output

Process Output

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

0

2Estimated States

State1

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

0

10

State2

0 5 10 15 20 25 30 35 40 45 500

0.2

0.4

Time (sec)

EstimatedZeta


10/24

SIMULINK IMPLEMENTATION

Overall Interconnection of Subsystems:

FIGURE 1: OVERALL DIAGRAM OF PARAMETER ESTIMIZATION SYSTEM

FIGURE 2: PROCESS SIMULINK MODEL

xe(k-1)xe

ze

predicted_state_output

xe phi

phi gen

xeKkzze

xe_new

corrector

Zero-Order

Hold

Scope4

Scope3

Scope2

Scope1

Scope

u z

Process_Plant

Memory1Memory

phi

Pk-1

Kk

Pk

Kalman Gain

0

Constant

1

z

1

1

xos

Matrix

Multiply

Product2

Matrix

ultiply

Product1

Matrix

Multiply

Product

1

sxo

Integrator

[1 0]

C

0

1

B

0 1

-36 -4.2

A

1

u


11/24

FIGURE 3: ESTIMATED STATE PREDICTION MODEL

FIGURE 4: LINEARIZED MODEL GENRATOR (PHI GENERATOR)

2

ze

1

xe

Product3

Product2

Product1

Matrix

ultiply

Product

[1 0 0]

Hk

0.12

Constant4

0.36

Constant3

0.01

Constant2

1

xe(k-1)

1

phi

Product1

Product

A11

A12

A13

A21

A22

A23

A31A32

A33

A

Create 3x3 Matrix-0.12

Constant7

0.12

Constant6

1

Constant5

-0.36

Constant4

0

Constant3

0.01

Constant2

1

Constant1

1

xe


12/24

FIGURE 5: KALMAN GAIN CALCULATOR

FIGURE 6: STATE ESTIMATE CORRECTOR

Pk_int

2

Pk

1

Kk

1 0 0

0 1 0

0 0 1

eye

uT

Transpose1

uT

Transpose

0.01

Rk

4.45 0 0

0 4.45 0

0 0 4.45

Qk

Matrixultiply

Product4

Matrix

ultiply

Product3

Matrix

ultiply

Product2

Matrix

Multiply

Product1

Matrix

ultiply

Product

[1 0 0]

Hk

GeneraInverse

(LU)

LU Inverse

2

Pk-1

1

phi

1

xe_new

Matrix

Multiply

Product

4

ze

3

z

2

Kk

1

xe


13/24

Simulation Results:

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

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

time (Sec)

Output

Process Output

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

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

Time (Sec)

Estimated

Zeta


14/24

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: '/Unit Delay' */

rtb_x1k = sam_DWork.UnitDelay_DSTATE;

/* UnitDelay: '/Unit Delay1' */

rtb_x2k = sam_DWork.UnitDelay1_DSTATE;

/* Outport: '/x(k)' incorporates:

* Constant: '/Constant'* SignalConversion: '/ConcatBufferAtVector

ConcatenateIn1'

* SignalConversion: '/ConcatBufferAtVectorConcatenateIn2'

* SignalConversion: '/ConcatBufferAtVector

ConcatenateIn3'*/

sam_Y.xk[0] = rtb_x1k;

sam_Y.xk[1] = rtb_x2k;sam_Y.xk[2] = sam_P.Constant_Value;

/* Sum: '/Sum2' incorporates:* Inport: '/v(k)'

*/

rtb_zK = sam_U.vk + rtb_x1k;

/* Outport: '/z(k)' */

sam_Y.zk = rtb_zK;

/* Outport: '/h(k)' */sam_Y.hk = rtb_x1k;

/* Gain: '/Gain2' incorporates:

* Inport: '/w(k)'*/

rtb_Gain2 = sam_P.Gain2_Gain * sam_U.wk;

/* Gain: '/Gain3' incorporates:

* Inport: '/u(k)'

*/

rtb_Gain3 = sam_P.Gain3_Gain * sam_U.uk;

/* Sum: '/Sum' */

sam_B.x1k1 = rtb_x2k + rtb_x1k;

/* Sum: '/Sum1' incorporates:

* Constant: '/Constant'

* Gain: '/Gain'* Gain: '/Gain1'

* Gain: '/Gain3'* Product: '/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);

}

/* Model update function */

static void sam_update(int_T tid)

{

/* Update for UnitDelay: '/Unit Delay' */

sam_DWork.UnitDelay_DSTATE = sam_B.x1k1;

/* Update for UnitDelay: '/Unit Delay1' */

sam_DWork.UnitDelay1_DSTATE = sam_B.Sum1;

/* Update absolute time for base rate */

/* The "clockTick0" counts the number of times the code ofthis 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);

}

/* Model initialize function */


15/24

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;

}


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 RTWLogDataTypeConvertrt_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),


16/24

{ 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);

/* 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 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: '/Unit Delay' */

sam_DWork.UnitDelay_DSTATE = sam_P.UnitDelay_X0;

/* InitializeConditions for UnitDelay: '/Unit Delay1' */

sam_DWork.UnitDelay1_DSTATE = sam_P.UnitDelay1_X0;}

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 *

*=============================================

===========================*/


17/24

CCODE 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 ODE3fixed-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


18/24

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): '/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: '/xos' */hassan3_B.xos[0] = hassan3_P.xos_Value[0];

hassan3_B.xos[1] = hassan3_P.xos_Value[1];}

/* Integrator: '/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: '/Product1' incorporates:* Constant: '/C'

*/hassan3_B.Product1 = hassan3_P.C_Value[0] *

rtb_Integrator[0] +

hassan3_P.C_Value[1] * rtb_Integrator[1];


/* Scope: '/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: '/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];


19/24

/* Sum: '/Sum' incorporates:

* Constant: '/Constant2'* Product: '/Product1'

*/

rtb_Product_h = hassan3_P.Constant2_Value *rtb_Product_e[1] +

rtb_Product_e[0];

/* Sum: '/Sum1' incorporates:* Constant: '/Constant3'

* Constant: '/Constant4'* Product: '/Product2'

* Product: '/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];

/* SignalConversion: '/TmpSignal

ConversionAtProductInport2' */

rtb_TmpSignalConversionAtProd_0 = rtb_Product_h;rtb_TmpSignalConversionAtProd_1 =

rtb_LUFactorization_o1;rtb_TmpSignalConversionAtProd_2 = rtb_Product_e[2];

/* Product: '/Product' incorporates:* Constant: '/Hk'

* SignalConversion: '/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: '/ConcatBufferAtVector

ConcatenateIn1' incorporates:

* Constant: '/Constant1'*/

rtb_VectorConcatenate[0] = hassan3_P.Constant1_Value;

/* Constant: '/Constant4' */

rtb_VectorConcatenate[1] = hassan3_P.Constant4_Value_g;


ConcatenateIn3' incorporates:* Constant: '/Constant3'

*/

rtb_VectorConcatenate[2] = hassan3_P.Constant3_Value_p;

/* Constant: '/Constant2' */

rtb_VectorConcatenate[3] = hassan3_P.Constant2_Value_h;

/* Sum: '/Sum' incorporates:* Constant: '/Constant5'* Constant: '/Constant6'

* Product: '/Product'

*/rtb_VectorConcatenate[4] = hassan3_P.Constant5_Value -

hassan3_P.Constant6_Value * rtb_Product_e[2];









/* Product: '/Product1' incorporates:* Constant: '/Constant7'

*/

rtb_VectorConcatenate[7] = hassan3_P.Constant7_Value *rtb_LUFactorization_o1;


ConcatenateIn9' incorporates:* Constant: '/Constant1'

*/rtb_VectorConcatenate[8] = hassan3_P.Constant1_Value;

/* Product: '/Product' incorporates:* Math: '/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 + 3] *

rtb_VectorConcatenate[i_0+ 3];

tmp[i + 3 * i_0] = tmp[3 * i_0 + i] +

hassan3_DWork.Memory1_PreviousInput[i + 6] *rtb_VectorConcatenate[i_0

+ 6];

}}

/* Sum: '/Sum' incorporates:* Constant: '/Qk'

* Product: '/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: '/Sum' */

/* Math: '/Math Function' incorporates:

* Constant: '/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: '/Product2' incorporates:* Constant: '/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 = (hassan3_P.Hk_Value_n[0] *rtb_Sum_0[0] +

hassan3_P.Hk_Value_n[1] * rtb_Sum_0[1]) +

hassan3_P.Hk_Value_n[2] *

rtb_Sum_0[2];

/* End of Product: '/Product2' */



20/24

* Constant: '/Rk'

*/rtb_Sum1 = rtb_LUFactorization_o1 + hassan3_P.Rk_Value;

/* S-Function (sdsplu2): '/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) -

'/Permute Matrix' *//* Scalar output equals scalar input. */

rtb_Sum1_l = hassan3_B.IdentityMatrix;

/* S-Function (sdspfbsub2): '/Backward Substitution' */

rtb_Sum1_l /= rtb_LUFactorization_o1;

/* Product: '/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: '/Product1' */

/* ZeroOrderHold: '/Zero-Order Hold' */rtb_Sum1_l = hassan3_B.Product1;

/* Sum: '/Sum1' */rtb_Sum1_l -= rtb_Product_h;

/* Product: '/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: '/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: '/Scope4' */


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);

}}


* Constant: '/Hk'

* Constant: '/eye'

* Product: '/Product3'* Product: '/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: '/Sum2' */

/* Product: '/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: '/Product2' incorporates:

* Constant: '/Constant'* Constant: '/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: '/Sum' incorporates:

* Constant: '/A'* Product: '/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];



UNUSED_PARAMETER(tid);}

/* Model update function */void hassan3_update(int_T tid)

{

if (rtmIsMajorTimeStep(hassan3_M)) {/* Update for Memory: '/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: '/Memory1' */

memcpy((void *)hassan3_DWork.Memory1_PreviousInput,

(void *)

(&hassan3_B.Product3[0]), 9U * sizeof(real_T));}


21/24


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 timerwill 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, thehigh 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.clockTick1and 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;

}



UNUSED_PARAMETER(tid);}

/* Derivatives for root system: '' */void hassan3_derivatives(void)

{

/* Derivatives for Integrator: '/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 = hassan3_M-

>Timing.sampleTimeTaskIDArray;mdlTsMap[0] = 0;

mdlTsMap[1] = 1;

hassan3_M->Timing.sampleTimeTaskIDPtr =(&mdlTsMap[0]);

hassan3_M->Timing.sampleTimes = (&hassan3_M-

>Timing.sampleTimesArray[0]);hassan3_M->Timing.offsetTimes = (&hassan3_M-

>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]);


22/24

}

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;}


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 = ((real_T*)&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 modeloutputs */

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 blockoutputs */

hassan3_M->Sizes.numBlockPrms = (58);/* Sum of parameter

"widths" */

}

void MdlInitializeSampleTimes(void){

}

void MdlInitialize(void)

{

/* InitializeConditions for Integrator: '/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: '/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];

/* InitializeConditions for Memory: '/Memory1' */

memcpy((void *)hassan3_DWork.Memory1_PreviousInput,

(void *)hassan3_P.Memory1_X0, 9U * sizeof(real_T));


23/24

/* set "at time zero" to false */

if (rtmIsFirstInitCond(hassan3_M)) {rtmSetFirstInitCond(hassan3_M, 0);

}

}

void MdlStart(void)

{

/* Start for Constant: '/xos' */hassan3_B.xos[0] = hassan3_P.xos_Value[0];

hassan3_B.xos[1] = hassan3_P.xos_Value[1];

/* Start for Scope: '/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 };

static const char_T *rt_ScopeSignalLabels[] = { "" };

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;

}

/* Start for S-Function (sdspeye): '/Identity Matrix' */

/* Fill in 1's on the diagonal. */

hassan3_B.IdentityMatrix = 1.0;

/* Start for Scope: '/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 int_T rt_ScopeSignalPlotStyles[] = { 1 };

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)


24/24

{

hassan3_terminate();}

RT_MODEL_hassan3 *hassan3(void){

hassan3_initialize(1);

return hassan3_M;

}

/*============================================

============================*

* End of GRT compatible call interface *

*=============================================

===========================*/

copy of kalman report

Documents