程序代写代做 InitialModel_Quaternions.mdl

InitialModel_Quaternions.mdl
Task 1
Full 6 DoF ROV dynamics simulator is show in Figure. Quaternions are used internally for attitude representation. Four blocks are removed from the model: B1 Euler Angles to Quaternions, B2 Quaternions to Euler Angles, B3 Position to VR Translation, B4 Orientation to VR Rotation. Your task is to rebuild missing blocks in Simulink using formulas below. Insert new blocks back to initial model and save it as new model with name “FinalModel_Quaternions.mdl”.
B1 Euler Angles to Quaternions
⎡/2 /2 /2 +/2 /2 /2 ⎤ =⎢−/2  /2  /2 +/2  /2  /2 ⎥ ⎢/2 /2 /2 +/2 /2 /2 ⎥ ⎣/2 /2 /2 −/2 /2 /2 ⎦
Input: Vector [R;P;Y]
Output: Quaternion [q0;q1;q2;q3]
B2 Quaternions to Euler Angles
Normalise q and then
 ⎡22 + ,1−2 + ⎤
 =⎢ !”2 − # ⎥ ⎢⎥
⎢22  +  ,1−2 + ⎥ ⎣⎦
Input: Quaternion [q0;q1;q2;q3] Output: Vector [R;P;Y]

B3 Position to VR Translation
010 
VRtranslation =  0 0 −1eta1_ E −1 0 0

Input: eta1_E = [x;y;z]
Output: Vector VR_translation
B4 Orientation to VR Rotation
Normalise q and then
extract q1,q2,q3
0 1 0q1
  
VRrotation=0 0 −1q2
−10 0q   3
 2 arccos(q0 ) 
Input: Quaternion [q0;q1;q2;q3] Output: Vector VR_rotation

Task 2
Expand Matlab Simulink model “FinalModel_Quaternions.mdl” with new NED -> Lat/Lon subsystem (see diagrams below). New block shoud have 3 inputs and one output. Inputs: etadot (linear and angular velocities in {n} frame, p b/n_n(0) ({b} origin relative to {n}) and [LON(0);LAT(0);h(0)] ({n} origin relative to {e}). Output: [LON,LAT,h] (ROV position).