Escolar Documentos
Profissional Documentos
Cultura Documentos
Submitted By : Hassan Shahid Hassan Saleem Sameer Farooq (53701) (53308) (52090)
M. Shahzad Afzal (53935) Kumail Raza Jafri (52685) Zaeem ul Haq (51376)
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.
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.
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
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.
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.
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
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
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:
Output
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
SIMULINK IMPLEMENTATION
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
Scope3
Kalman Gain
xe Kk z ze
xe_new
corrector
Scope4 Scope2
0 1 -36 -4.2
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
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
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
Pk_int
Product4
1 Kk
Transpose
4.45 0 0
0 4.45 0 Qk
0 0 4.45
Hk
Transpose1 0.01 Rk
Simulation Results:
Process Output 1.2 1 0.8 0.6
Output
10
15
20
25 30 time (Sec)
35
40
45
50
10
15
20
25 30 Time (Sec)
35
40
45
50
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 * *============================================= ===========================*/
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]) * +
* 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];
* *
= = =
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
= = =
/* 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)