qlib.mdl

四元数工具箱

	    Branch {
	      DstBlock		      "Dot Product 2"
	      DstPort		      1
	    }
	    Branch {
	      Points		      [0, 75]
	      Branch {
		DstBlock		"Dot Product 3"
		DstPort			1
	      }
	      Branch {
		Points			[0, 75]
		DstBlock		"Dot Product 4"
		DstPort			1
	      }
	    }
	  }
	}
	Line {
	  SrcBlock		  "Dot Product 1"
	  SrcPort		  1
	  DstBlock		  "Mux5"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Dot Product 3"
	  SrcPort		  1
	  DstBlock		  "Mux5"
	  DstPort		  3
	}
	Line {
	  SrcBlock		  "Dot Product 4"
	  SrcPort		  1
	  DstBlock		  "Mux5"
	  DstPort		  4
	}
	Line {
	  SrcBlock		  "Mux5"
	  SrcPort		  1
	  DstBlock		  "q1q2"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Dot Product 2"
	  SrcPort		  1
	  DstBlock		  "Mux5"
	  DstPort		  2
	}
	Line {
	  SrcBlock		  "Mux"
	  SrcPort		  1
	  Points		  [10, 0]
	  Branch {
	    Points		    [0, 40]
	    Branch {
	      Points		      [0, 75]
	      DstBlock		      "Demux1"
	      DstPort		      1
	    }
	    Branch {
	      DstBlock		      "Bus\nSelector2"
	      DstPort		      1
	    }
	  }
	  Branch {
	    Points		    [0, -35]
	    Branch {
	      Points		      [0, -75]
	      DstBlock		      "Bus\nSelector"
	      DstPort		      1
	    }
	    Branch {
	      DstBlock		      "Bus\nSelector1"
	      DstPort		      1
	    }
	  }
	}
	Line {
	  Labels		  [1, 0]
	  SrcBlock		  "q2"
	  SrcPort		  1
	  DstBlock		  "Demux"
	  DstPort		  1
	}
	Line {
	  Name			  "q2(1)"
	  Labels		  [0, 0]
	  SrcBlock		  "Demux"
	  SrcPort		  1
	  DstBlock		  "Mux"
	  DstPort		  1
	}
	Line {
	  Name			  "q2(2)"
	  Labels		  [0, 0]
	  SrcBlock		  "Demux"
	  SrcPort		  2
	  DstBlock		  "Mux"
	  DstPort		  2
	}
	Line {
	  Name			  "q2(3)"
	  Labels		  [0, 0]
	  SrcBlock		  "Demux"
	  SrcPort		  3
	  DstBlock		  "Mux"
	  DstPort		  3
	}
	Line {
	  Name			  "q2(4)"
	  Labels		  [0, 0]
	  SrcBlock		  "Demux"
	  SrcPort		  4
	  DstBlock		  "Mux"
	  DstPort		  4
	}
      }
    }
    Block {
      BlockType		      SubSystem
      Name		      "Quaternion\nNormalize"
      Ports		      [1, 1, 0, 0, 0]
      Position		      [40, 89, 80, 121]
      ShowPortLabels	      on
      MaskType		      "Quaternion Normalize"
      MaskDescription	      "qout = q/sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2)"
      MaskInitialization      "pos = get_param(gcb, 'Position');\nwidth = pos("
"3) - pos(1); height = pos(4) - pos(2);\nfontsize = get_param(gcb, 'fontsize')"
";\nif fontsize==-1, scale=1; else scale=fontsize/10; end"
      MaskDisplay	      "text(width/2-scale*3, (height/2+scale*10), 'q')"
"\ntext(width/2-scale*8, (height/2-scale*6), '||q||')\nplot([width/2-scale*10 "
"width/2+scale*10], height/2*[1 1])"
      MaskIconFrame	      on
      MaskIconOpaque	      on
      MaskIconRotate	      "none"
      MaskIconUnits	      "pixels"
      System {
	Name			"Quaternion\nNormalize"
	Location		[7, 366, 335, 496]
	Open			off
	ModelBrowserVisibility	off
	ModelBrowserWidth	200
	ScreenColor		"white"
	PaperOrientation	"landscape"
	PaperPositionMode	"auto"
	PaperType		"usletter"
	PaperUnits		"inches"
	ZoomFactor		"100"
	AutoZoom		on
	Block {
	  BlockType		  Inport
	  Name			  "q"
	  Position		  [25, 53, 55, 67]
	  BackgroundColor	  "cyan"
	  Port			  "1"
	  PortWidth		  "4"
	  SampleTime		  "-1"
	  DataType		  "auto"
	  SignalType		  "auto"
	  Interpolate		  on
	}
	Block {
	  BlockType		  Product
	  Name			  "Product"
	  Ports			  [2, 1, 0, 0, 0]
	  Position		  [230, 24, 245, 71]
	  Inputs		  "*/"
	  SaturateOnIntegerOverflow on
	}
	Block {
	  BlockType		  Math
	  Name			  "Square"
	  Ports			  [1, 1, 0, 0, 0]
	  Position		  [90, 47, 115, 73]
	  Operator		  "square"
	  OutputSignalType	  "auto"
	}
	Block {
	  BlockType		  Math
	  Name			  "Square\nRoot"
	  Ports			  [1, 1, 0, 0, 0]
	  Position		  [175, 45, 205, 75]
	  Operator		  "sqrt"
	  OutputSignalType	  "auto"
	}
	Block {
	  BlockType		  Sum
	  Name			  "Sum"
	  Ports			  [1, 1, 0, 0, 0]
	  Position		  [135, 50, 155, 70]
	  IconShape		  "round"
	  Inputs		  "1"
	  SaturateOnIntegerOverflow on
	}
	Block {
	  BlockType		  Outport
	  Name			  "qnorm"
	  Position		  [270, 43, 300, 57]
	  BackgroundColor	  "yellow"
	  Port			  "1"
	  OutputWhenDisabled	  "held"
	  InitialOutput		  "0"
	}
	Line {
	  SrcBlock		  "Product"
	  SrcPort		  1
	  DstBlock		  "qnorm"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Square\nRoot"
	  SrcPort		  1
	  DstBlock		  "Product"
	  DstPort		  2
	}
	Line {
	  SrcBlock		  "q"
	  SrcPort		  1
	  Points		  [10, 0]
	  Branch {
	    Points		    [0, -25]
	    DstBlock		    "Product"
	    DstPort		    1
	  }
	  Branch {
	    DstBlock		    "Square"
	    DstPort		    1
	  }
	}
	Line {
	  SrcBlock		  "Square"
	  SrcPort		  1
	  DstBlock		  "Sum"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Sum"
	  SrcPort		  1
	  DstBlock		  "Square\nRoot"
	  DstPort		  1
	}
      }
    }
    Block {
      BlockType		      SubSystem
      Name		      "Quaternion\nPropagation"
      Ports		      [1, 1, 0, 0, 0]
      Position		      [40, 164, 85, 196]
      FontName		      "helvetica"
      ShowPortLabels	      on
      MaskType		      "Quaternion Propagation"
      MaskDescription	      "Integrates dq/dt to propagate a quaternion, whe"
"re:\n\n    dq/dt = (1/2) (q) ([w 0])\n \nand the product (q)([w 0]) is calcul"
"ated as a quaternion multiplication.\n\nThe input rates, w, are inertial, but"
" written in the body frame coordinates.  The quaternion indicates the orienta"
"tion of the body frame with respect to the inertial frame.  The initlal quate"
"rnion, q0, must be specfied below.\n\nThe quaternion is renormalized at each "
"iteration. "
      MaskPromptString	      "q0"
      MaskStyleString	      "edit"
      MaskTunableValueString  "on"
      MaskEnableString	      "on"
      MaskVisibilityString    "on"
      MaskVariables	      "q0=@1;"
      MaskInitialization      "pos = get_param(gcb, 'Position');\nwidth = pos("
"3) - pos(1); height = pos(4) - pos(2);\nfontsize = get_param(gcb, 'fontsize')"
";\nif fontsize==-1, scale=1; else scale=fontsize/10; end\n\nt1=-pi/2:(pi/2)/2"
"0:0; t2=pi:-(pi/2)/20:(pi/2);\nx=[cos(t1)  cos(t2)+2]; y=[sin(t1)+1  sin(t2)+"
"4];\n\nny=0.9*y/max(y)*scale;\nnx=0.9*x/max(y)*scale;\n\ns=20;\nsy=s*(ny - ma"
"x(ny)/2) + height/2;\nsx=s*nx + width/2 - 15*scale;\n\nt=0:2*pi/100:2*pi;\na="
"1;\ntx=a*cos(t)+width/2-5*scale; ty=a*sin(t)+height/2+4*scale;\n"
      MaskDisplay	      "plot(sx,sy);\ntext(width/2-7*scale, height/2, '"
"q dt');\nplot(tx,ty)\n\n"
      MaskIconFrame	      on
      MaskIconOpaque	      on
      MaskIconRotate	      "none"
      MaskIconUnits	      "pixels"
      MaskValueString	      "[0 0 0 1]"
      System {
	Name			"Quaternion\nPropagation"
	Location		[7, 559, 529, 697]
	Open			off
	ModelBrowserVisibility	off
	ModelBrowserWidth	200
	ScreenColor		"white"
	PaperOrientation	"landscape"
	PaperPositionMode	"auto"
	PaperType		"usletter"
	PaperUnits		"inches"
	ZoomFactor		"100"
	AutoZoom		on
	Block {
	  BlockType		  Inport
	  Name			  "w"
	  Position		  [20, 58, 50, 72]
	  BackgroundColor	  "cyan"
	  Port			  "1"
	  PortWidth		  "3"
	  SampleTime		  "-1"
	  DataType		  "auto"
	  SignalType		  "auto"
	  Interpolate		  on
	}
	Block {
	  BlockType		  Constant
	  Name			  "Constant"
	  Position		  [60, 74, 80, 96]
	  Value			  "0"
	}
	Block {
	  BlockType		  Gain
	  Name			  "Gain"
	  Position		  [230, 45, 270, 85]
	  Gain			  "0.5"
	  SaturateOnIntegerOverflow on
	}
	Block {
	  BlockType		  Integrator
	  Name			  "Integrator"
	  Ports			  [1, 1, 0, 0, 0]
	  Position		  [300, 47, 330, 83]
	  ExternalReset		  "none"
	  InitialConditionSource  "internal"
	  InitialCondition	  "q0"
	  LimitOutput		  off
	  UpperSaturationLimit	  "inf"
	  LowerSaturationLimit	  "-inf"
	  ShowSaturationPort	  off
	  ShowStatePort		  off
	  AbsoluteTolerance	  "auto"
	}
	Block {
	  BlockType		  Mux
	  Name			  "Mux"
	  Ports			  [2, 1, 0, 0, 0]
	  Position		  [110, 56, 115, 94]
	  ShowName		  off
	  Inputs		  "2"
	  DisplayOption		  "bar"
	}
	Block {
	  BlockType		  Reference
	  Name			  "Quaternion\nMultiply"
	  Ports			  [2, 1, 0, 0, 0]
	  Position		  [155, 47, 195, 83]
	  SourceBlock		  "qlib/Quaternion\nMultiply"
	  SourceType		  "Quaternion Multiply"
	}
	Block {
	  BlockType		  Reference
	  Name			  "Quaternion\nNormalize"
	  Ports			  [1, 1, 0, 0, 0]
	  Position		  [370, 49, 410, 81]
	  SourceBlock		  "qlib/Quaternion\nNormalize"
	  SourceType		  "Quaternion Normalize"
	}
	Block {
	  BlockType		  Outport
	  Name			  "q"
	  Position		  [465, 58, 495, 72]
	  BackgroundColor	  "yellow"
	  Port			  "1"
	  OutputWhenDisabled	  "held"
	  InitialOutput		  "0"
	}
	Line {
	  SrcBlock		  "Quaternion\nMultiply"
	  SrcPort		  1
	  DstBlock		  "Gain"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Gain"
	  SrcPort		  1
	  DstBlock		  "Integrator"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Integrator"
	  SrcPort		  1
	  DstBlock		  "Quaternion\nNormalize"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Quaternion\nNormalize"
	  SrcPort		  1
	  Points		  [25, 0]
	  Branch {
	    DstBlock		    "q"
	    DstPort		    1
	  }
	  Branch {
	    Points		    [0, -40; -310, 0; 0, 30]
	    DstBlock		    "Quaternion\nMultiply"
	    DstPort		    1
	  }
	}
	Line {
	  SrcBlock		  "w"
	  SrcPort		  1
	  DstBlock		  "Mux"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Constant"
	  SrcPort		  1
	  DstBlock		  "Mux"
	  DstPort		  2
	}
	Line {
	  SrcBlock		  "Mux"
	  SrcPort		  1
	  DstBlock		  "Quaternion\nMultiply"
	  DstPort		  2
	}
      }
    }
    Block {
      BlockType		      SubSystem
      Name		      "Quaternion\nVector Rotation"
      Ports		      [2, 1, 0, 0, 0]
      Position		      [220, 162, 260, 198]
      ShowPortLabels	      on
      MaskType		      "Quaternion Vector Rotation"
      MaskDescription	      "Rotates a vector using a quaternion:\n\n    vou"
"t = (q)(v)(q*)\n\nThe first input is the vector, the second input is the quat"
"ernion.\n\nNote that for purposes of quaternion-vector multiplication, a vect"
"or is treated as a quaterion with a scalar element of zero."
      MaskIconFrame	      on
      MaskIconOpaque	      on
      MaskIconRotate	      "none"
      MaskIconUnits	      "pixels"
      System {
	Name			"Quaternion\nVector Rotation"
	Location		[497, 761, 975, 918]
	Open			off
	ModelBrowserVisibility	off
	ModelBrowserWidth	200
	ScreenColor		"white"
	PaperOrientation	"landscape"
	PaperPositionMode	"auto"
	PaperType		"usletter"
	PaperUnits		"inches"
	ZoomFactor		"100"
	AutoZoom		on
	Block {
	  BlockType		  Inport
	  Name			  "V"
	  Position		  [30, 38, 60, 52]
	  BackgroundColor	  "cyan"
	  Port			  "1"
	  PortWidth		  "3"
	  SampleTime		  "-1"
	  DataType		  "auto"
	  SignalType		  "auto"
	  Interpolate		  on
	}
	Block {
	  BlockType		  Inport
	  Name			  "q"
	  Position		  [95, 38, 125, 52]
	  BackgroundColor	  "cyan"
	  Port			  "2"
	  PortWidth		  "4"
	  SampleTime		  "-1"
	  DataType		  "auto"
	  SignalType		  "auto"
	  Interpolate		  on
	}
	Block {
	  BlockType		  Constant
	  Name			  "Constant"
	  Position		  [85, 104, 105, 126]
	  Value			  "0"
	}
	Block {
	  BlockType		  Demux
	  Name			  "Demux"
	  Ports			  [1, 2, 0, 0, 0]
	  Position		  [355, 61, 360, 99]
	  BackgroundColor	  "black"
	  ShowName		  off
	  Outputs		  "[3 1]"
	}
	Block {
	  BlockType		  Mux
	  Name			  "Mux"
	  Ports			  [2, 1, 0, 0, 0]
	  Position		  [135, 86, 140, 124]
	  ForegroundColor	  "blue"
	  ShowName		  off
	  Inputs		  "2"
	  DisplayOption		  "bar"
	}
	Block {
	  BlockType		  Reference
	  Name			  "Quaternion\nConjugate"
	  Ports			  [1, 1, 0, 0, 0]
	  Position		  [215, 79, 235, 101]
	  SourceBlock		  "qlib/Quaternion\nConjugate"
	  SourceType		  "Quaternion Conjugate"
	}
	Block {
	  BlockType		  Reference
	  Name			  "Quaternion\nMultiply 1"
	  Ports			  [2, 1, 0, 0, 0]
	  Position		  [205, 37, 245, 73]
	  NamePlacement		  "alternate"
	  SourceBlock		  "qlib/Quaternion\nMultiply"
	  SourceType		  "Quaternion Multiply"
	}
	Block {
	  BlockType		  Reference
	  Name			  "Quaternion\nMultiply 2"
	  Ports			  [2, 1, 0, 0, 0]
	  Position		  [290, 62, 330, 98]
	  NamePlacement		  "alternate"
	  SourceBlock		  "qlib/Quaternion\nMultiply"
	  SourceType		  "Quaternion Multiply"
	}
	Block {
	  BlockType		  Terminator
	  Name			  "Terminator"
	  Position		  [385, 80, 405, 100]
	}
	Block {
	  BlockType		  Outport
	  Name			  "qVq*"
	  Position		  [415, 63, 445, 77]
	  BackgroundColor	  "yellow"
	  NamePlacement		  "alternate"
	  Port			  "1"
	  OutputWhenDisabled	  "held"
	  InitialOutput		  "0"
	}
	Line {
	  SrcBlock		  "q"
	  SrcPort		  1
	  Points		  [25, 0]
	  Branch {
	    Points		    [0, 45]
	    DstBlock		    "Quaternion\nConjugate"
	    DstPort		    1
	  }
	  Branch {
	    DstBlock		    "Quaternion\nMultiply 1"
	    DstPort		    1
	  }
	}
	Line {
	  SrcBlock		  "Constant"
	  SrcPort		  1
	  DstBlock		  "Mux"
	  DstPort		  2
	}
	Line {
	  Labels		  [3, 0]
	  SrcBlock		  "V"
	  SrcPort		  1
	  Points		  [15, 0; 0, 50]
	  DstBlock		  "Mux"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Demux"
	  SrcPort		  1
	  DstBlock		  "qVq*"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Demux"
	  SrcPort		  2
	  DstBlock		  "Terminator"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Quaternion\nMultiply 2"
	  SrcPort		  1
	  DstBlock		  "Demux"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Quaternion\nMultiply 1"
	  SrcPort		  1
	  Points		  [15, 0; 0, 15]
	  DstBlock		  "Quaternion\nMultiply 2"
	  DstPort		  1
	}
	Line {
	  SrcBlock		  "Mux"
	  SrcPort		  1
	  Points		  [30, 0; 0, -40]
	  DstBlock		  "Quaternion\nMultiply 1"
	  DstPort		  2
	}
	Line {
	  SrcBlock		  "Quaternion\nConjugate"
	  SrcPort		  1
	  DstBlock		  "Quaternion\nMultiply 2"
	  DstPort		  2
	}
      }
    }
    Block {
      BlockType		      SubSystem
      Name		      "Quaternion\nVector Transform"
      Ports		      [2, 1, 0, 0, 0]
      Position		      [130, 162, 170, 198]
      ShowPortLabels	      on
      MaskType		      "Quaternion Vector Transform"
      MaskDescription	      "Transforms a vector using a quaternion:\n\n    "
"vout = (q*)(v)(q)\n\nThe first input is the quaternion, the second input is t"
"he vector.\n\nNote that for purposes of quaternion-vector multiplication, a v"
"ector is treated as a quaterion with a scalar element of zero."
      MaskIconFrame	      on
      MaskIconOpaque	      on
      MaskIconRotate	      "none"
      MaskIconUnits	      "pixels"
      System {
	Name			"Quaternion\nVector Transform"
	Location		[11, 680, 489, 837]
	Open			off
	ModelBrowserVisibility	off
	ModelBrowserWidth	200
	ScreenColor		"white"
	PaperOrientation	"landscape"
	PaperPositionMode	"auto"
	PaperType		"usletter"
	PaperUnits		"inches"
	ZoomFactor		"100"