controller.c

一个飞机控制器的控制器源码

#endif
#ifndef min
#define min(a,b) ((a)<(b)?(a):(b))
#endif
/* Finished generating shared code for Controller */
/* SE's input variable declarations. */
static Token Controller_SE_u;
/* SE's output variable declarations. */
static double Controller_SE_T_est;
static Token Controller_SE_p_est;
static Token Controller_SE_v_est;
static Token Controller_SE_Theta;
static Token Controller_SE_trim_value;
static Token Controller_SE_z;
static Token Controller_SE_Theta_est;
/* VectorDisassembler's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_VectorDisassembler_input;
/* VectorAssembler's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_VectorAssembler_input[4];
/* MultiplyDivide's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale_Scale_input;
/* Scale's type convert variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale_Scale_output_0;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale2_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale2_Scale_input;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale3_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale3_Scale_input;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale4_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale4_Scale_input;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale5_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_scale5_Scale_input;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_alpha_u1_Matrix_add_u2;
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_Matrix_Multiply2_u2;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_x_k1k_Matrix_add_u2;
/* Expression's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Expression_u;
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Matrix_Multiply2_u2;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_P_k1k_Matrix_add_u2;
/* VectorDisassembler's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_VectorDisassembler_input;
/* VectorAssembler's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_VectorAssembler_input[3];
/* VectorAssembler2's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_VectorAssembler2_input[6];
/* VectorAssembler3's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_VectorAssembler3_input[6];
/* Discard's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_Discard_input[1];
/* VectorDisassembler2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_VectorDisassembler2_input;
/* VectorAssembler4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_VectorAssembler4_input[3];
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_Multiply2_u2;
/* Matrix_Multiply3's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_Multiply3_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_Multiply3_u2;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_add_u2;
/* Matrix_add2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_add2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_dpdot_conca_Matrix_add2_u2;
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_Multiply2_u2;
/* Matrix_Multiply3's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_Multiply3_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_Multiply3_u2;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_add_u2;
/* Matrix_add2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_add2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_vdot_conca_Matrix_add2_u2;
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_Multiply2_u2;
/* Matrix_Multiply3's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_Multiply3_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_Multiply3_u2;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_add_u2;
/* Matrix_add2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_add2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_d_Thetadot_conca_Matrix_add2_u2;
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_Multiply2_u2;
/* Matrix_Multiply3's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_Multiply3_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_Multiply3_u2;
/* Matrix_add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_add_u2;
/* Matrix_add2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_add2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_conca_Matrix_add2_u2;
/* MultiplyDivide's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide_multiply[2];
/* VectorDisassembler's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_VectorDisassembler_input;
/* MultiplyDivide4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide4_multiply[2];
/* MultiplyDivide6's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide6_multiply[3];
/* AddSubtract's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_AddSubtract_plus[1];
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_AddSubtract_minus[1];
/* MultiplyDivide7's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide7_multiply[3];
/* MultiplyDivide8's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide8_multiply[2];
/* MultiplyDivide9's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide9_multiply[3];
/* AddSubtract2's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_AddSubtract2_plus[1];
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_AddSubtract2_minus[1];
/* VectorAssembler's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_VectorAssembler_input[3];
/* MultiplyDivide10's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide10_multiply[3];
/* MultiplyDivide11's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide11_multiply[2];
/* AddSubtract3's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_AddSubtract3_minus[2];
/* MultiplyDivide12's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide12_multiply[3];
/* MultiplyDivide13's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide13_multiply[3];
/* MultiplyDivide14's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide14_multiply[2];
/* AddSubtract4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_AddSubtract4_plus[2];
/* MultiplyDivide15's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide15_multiply[2];
/* VectorAssembler2's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_VectorAssembler2_input[3];
/* MultiplyDivide16's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_MultiplyDivide16_multiply[2];
/* VectorAssembler3's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_VectorAssembler3_input[3];
/* Const's type convert variable declarations. */
static int Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_Const_output_0;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_MultiplyDivide_multiply[2];
/* Const2's type convert variable declarations. */
static int Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_Const2_output_0;
/* MultiplyDivide2's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_MultiplyDivide2_multiply[2];
/* Const3's type convert variable declarations. */
static int Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_Const3_output_0;
/* MultiplyDivide3's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_MultiplyDivide3_multiply[2];
/* Const4's type convert variable declarations. */
static int Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_Const4_output_0;
/* MultiplyDivide4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_MultiplyDivide4_multiply[2];
/* Const5's type convert variable declarations. */
static int Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_Const5_output_0;
/* MultiplyDivide5's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_MultiplyDivide5_multiply[2];
/* Const6's type convert variable declarations. */
static int Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_Const6_output_0;
/* MultiplyDivide6's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_CompositeActor_MultiplyDivide6_multiply[2];
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale_Scale_input;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale2_MultiplyDivide_multiply[2];
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale2_Scale_input;
/* MultiplyDivide's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale3_MultiplyDivide_multiply[2];
/* MultiplyDivide's type convert variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale3_MultiplyDivide_output_0;
/* Scale's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_scale3_Scale_input;
/* Discard's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_Discard_input[1];
/* VectorAssembler4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_VectorAssembler4_input[4];
/* Matrix_Multiply's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Multiply_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Multiply_u2;
/* Matrix_Multiply2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Multiply2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Multiply2_u2;
/* Matrix_Multiply3's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Multiply3_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Multiply3_u2;
/* Matrix_Add's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Add_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Add_u2;
/* Matrix_Add2's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Add2_u1;
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_vdot_conca_Matrix_Add2_u2;
/* VectorDisassembler's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_VectorDisassembler_input;
/* Discard's input variable declarations. */
static Token Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_Discard_input[1];
/* MultiplyDivide4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide4_multiply[2];
/* MultiplyDivide5's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide5_multiply[2];
/* AddSubtract's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract_plus[1];
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract_minus[1];
/* MultiplyDivide6's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide6_multiply[2];
/* VectorAssembler's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_VectorAssembler_input[3];
/* MultiplyDivide7's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide7_multiply[2];
/* MultiplyDivide8's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide8_multiply[2];
/* AddSubtract2's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract2_plus[2];
/* MultiplyDivide9's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide9_multiply[1];
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide9_divide[1];
/* UnaryMathFunction's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_UnaryMathFunction_input;
/* VectorAssembler2's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_VectorAssembler2_input[3];
/* MultiplyDivide10's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide10_multiply[2];
/* MultiplyDivide11's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide11_multiply[2];
/* AddSubtract3's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract3_minus[2];
/* VectorAssembler3's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_VectorAssembler3_input[3];
/* MultiplyDivide12's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide12_multiply[2];
/* MultiplyDivide13's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide13_multiply[2];
/* AddSubtract4's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract4_plus[1];
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract4_minus[1];
/* MultiplyDivide14's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide14_multiply[1];
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide14_divide[1];
/* MultiplyDivide15's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide15_multiply[2];
/* MultiplyDivide16's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide16_multiply[2];
/* AddSubtract5's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_AddSubtract5_plus[2];
/* MultiplyDivide17's input variable declarations. */
static double Controller_SE_Kalman_Filter_KF_Prediction_Compute_phi_k1k_and_psi_k1k_compute_Fx_o_Thetadot_MultiplyDivide17_multiply[2];
/* MultiplyDivide18's input variable d