Modelica坐标转换代码

package CoodrnateTransform "直角坐标系转换为经纬高坐标系"
  block Transform "北-东-地坐标系坐标转经纬高"
    import D2R = Modelica.Constants.D2R;
    import R2D = Modelica.Constants.R2D;
    import PI = Modelica.Constants.pi;
    import SI = Modelica.SIunits;
    //直角坐标系坐标原点位置(默认东经122.04,北纬37.5,高度0)
    parameter SI.Angle Longitude0 = 2.129999819134;
    parameter SI.Angle Latitude0 = 0.654498469498;
    parameter SI.Length Altitude0 = 0;
    //参考椭球(默认WGS84)
    parameter SI.Length a = 6378137;
    parameter Real f = 1 / 298.257223563;
    Modelica.Blocks.Interfaces.RealInput X annotation(
      Placement(visible = true, transformation(origin = {-120, 60}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 60}, extent = {{-20, -20}, {20, 20}}, rotation = 0)));
    Modelica.Blocks.Interfaces.RealInput Y annotation(
      Placement(visible = true, transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}, rotation = 0)));
    Modelica.Blocks.Interfaces.RealInput Z annotation(
      Placement(visible = true, transformation(origin = {-120, -60}, extent = {{-20, -20}, {20, 20}}, rotation = 0), iconTransformation(origin = {-120, -60}, extent = {{-20, -20}, {20, 20}}, rotation = 0)));
    Modelica.Blocks.Interfaces.RealOutput Longitude "经度 deg" annotation(
      Placement(visible = true, transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 0)));
    Modelica.Blocks.Interfaces.RealOutput Latitude "纬度 deg" annotation(
      Placement(visible = true, transformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, 60}, extent = {{-10, -10}, {10, 10}}, rotation = 0)));
    Modelica.Blocks.Interfaces.RealOutput Altitude "高度 m" annotation(
      Placement(visible = true, transformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0), iconTransformation(origin = {110, -60}, extent = {{-10, -10}, {10, 10}}, rotation = 0)));
    //目标点在地心坐标系下的坐标
    Real Xe, Ye, Ze;
    //经纬度(rad)
    Real Lo, La;
  equation
    (Xe, Ye, Ze) = NED2ECEF(X, Y, Z, Longitude0, Latitude0, Altitude0, a, f);
    (Lo, La, Altitude) = ECEF2LLA(Xe, Ye, Ze, a, f);
    Longitude = Lo * R2D;
    Latitude = La * R2D;
  end Transform;

  function LLA2ECEF "地理坐标系到地心坐标系转换。输入：经度(rad)、纬度(rad)、高度、椭球长半轴、扁率；输出：地心坐标系X，Y，Z坐标"
    import SI = Modelica.SIunits;
    //经纬高
    input SI.Angle Longitude;
    input SI.Angle Latitude;
    input SI.Length Altitude;
    //长半轴和扁率
    input SI.Length a;
    input Real f;
    //地心坐标系坐标
    output SI.Length X, Y, Z;
  protected
    //短半轴长度
    Real b;
    //离心率平方
    Real e2;
    //椭球表面沿法向到球心坐标系Z轴的距离
    Real Nphi;
  algorithm
    b := a * (1 - f);
    e2 := 1 - (b / a) ^ 2;
    Nphi := a / sqrt(1 - e2 * sin(Latitude) ^ 2);
    X := (Nphi + Altitude) * cos(Latitude) * cos(Longitude);
    Y := (Nphi + Altitude) * cos(Latitude) * sin(Longitude);
    Z := (Nphi * (b / a) ^ 2 + Altitude) * sin(Latitude);
  end LLA2ECEF;

  function ECEF2LLA "地心坐标系到地理坐标系转换。输入：地心坐标系X，Y，Z坐标，椭球长半轴、扁率；输出：经度(rad)、纬度(rad)、高度"
    import eps = Modelica.Constants.eps;
    import pi = Modelica.Constants.pi;
    import SI = Modelica.SIunits;
    //地心坐标系坐标
    input SI.Length X, Y, Z;
    //长半轴和扁率
    input SI.Length a;
    input Real f;
    //经纬高
    output SI.Angle Longitude;
    output SI.Angle Latitude;
    output SI.Length Altitude;
  protected
    //椭圆短半轴（椭球极轴）
    Real b;
    //坐标点在其所在椭球截面中的坐标
    Real x0, z0;
    //迭代方程系数
    Real A, B, C;
    //包含t的方程和导数计算结果
    Real ft, dft;
    //迭代方程计算结果
    Real tk, tk1;
    //迭代次数，最大次数
    Integer k;
    parameter Integer kmax = 1000;
  algorithm
    b := a * (1 - f);
    x0 := sqrt(X ^ 2 + Y ^ 2);
    z0 := Z;
    A := b * z0;
    B := 2 * a * x0;
    C := 2 * (a ^ 2 - b ^ 2);
    if abs(Z) <= eps * 1000 then
      Latitude := 0;
      Altitude := x0 - a;
    else
      tk := (-(1 - f) * x0 / z0) + sign(z0) * sqrt(1 + ((1 - f) * x0 / z0) ^ 2);
      k := 0;
      ft := A * tk ^ 4 + (B + C) * tk ^ 3 + (B - C) * tk - A;
      dft := 4 * A * tk ^ 3 + 3 * (B + C) * tk ^ 2 + B - C;
      tk1 := tk - ft / dft;
      while abs(tk1 - tk) > eps * 1000 and k < kmax loop
        k := k + 1;
        tk := tk1;
        ft := A * tk ^ 4 + (B + C) * tk ^ 3 + (B - C) * tk - A;
        dft := 4 * A * tk ^ 3 + 3 * (B + C) * tk ^ 2 + B - C;
        tk1 := tk - ft / dft;
      end while;
      Latitude := pi / 2 - atan((1 - f) * (1 - tk ^ 2) / 2 / tk);
      Altitude := sign(Latitude) * (z0 - b * 2 * tk / (1 + tk ^ 2)) * sqrt(1 + ((1 - f) * (1 - tk ^ 2) / 2 / tk) ^ 2);
    end if;
    Longitude := atan2(Y, X);
  end ECEF2LLA;

  function axisRotation "坐标系绕单轴旋转后由旧坐标计算新坐标的的坐标转换矩阵，输入：（1）旋转轴：1表示绕x轴；2表示绕y轴；3表示绕z轴；（2）旋转角（rad）；输出：将坐标由原坐标系转换到新坐标系的坐标转换矩阵"
    import SI = Modelica.SIunits;
    //旋转轴
    input Integer axis(min = 1, max = 3);
    //旋转角
    input SI.Angle angle;
    //坐标转换矩阵
    output Real transMatrix[3, 3];
  algorithm
    if axis == 1 then
      transMatrix := [1, 0, 0; 0, Modelica.Math.cos(angle), Modelica.Math.sin(angle); 0, -Modelica.Math.sin(angle), Modelica.Math.cos(angle)];
    elseif axis == 2 then
      transMatrix := [Modelica.Math.cos(angle), 0, -Modelica.Math.sin(angle); 0, 1, 0; Modelica.Math.sin(angle), 0, Modelica.Math.cos(angle)];
    else
      transMatrix := [Modelica.Math.cos(angle), Modelica.Math.sin(angle), 0; -Modelica.Math.sin(angle), Modelica.Math.cos(angle), 0; 0, 0, 1];
    end if;
  end axisRotation;

  function NED2ECEF "北-东-地坐标系坐标转地心坐标系，输入：北-东-地坐标系坐标XYZ，原点经、纬、高，参考椭球长轴、扁率；输出：地心坐标系坐标XYZ"
    import PI = Modelica.Constants.pi;
    import SI = Modelica.SIunits;
    //北-东-地坐标系坐标
    input SI.Length Xr, Yr, Zr;
    //北-东-地坐标系原点位置，经度、纬度、高度
    input SI.Angle Longitude, Latitude;
    input SI.Length Altitude;
    //参考椭球面长轴、扁率
    input SI.Length a;
    input Real f;
    //地心坐标系坐标
    output SI.Length Xe, Ye, Ze;
  protected
    //坐标转换矩阵
    Real T[3, 3];
    //北-东-地坐标系原点在地心坐标系的坐标
    Real X, Y, Z;
  algorithm
    T := transpose(axisRotation(2, -(Latitude + PI / 2)) * axisRotation(3, Longitude));
    (X, Y, Z) := LLA2ECEF(Longitude, Latitude, Altitude, a, f);
    Xe := T[1, 1] * Xr + T[1, 2] * Yr + T[1, 3] * Zr + X;
    Ye := T[2, 1] * Xr + T[2, 2] * Yr + T[2, 3] * Zr + Y;
    Ze := T[3, 1] * Xr + T[3, 2] * Yr + T[3, 3] * Zr + Z;
  end NED2ECEF;

  function vNED2vLLA "北东地方向速度转经纬高，输入：北东地方向速度Vx,Vy,Vz，飞行器坐标经度纬度高度，参考椭球长轴、扁率；输出：经度、纬度、高度变化率"
    import SI = Modelica.SIunits;
    //飞行器当前速度在北、东、地三个方向上的分量单位m/s
    input SI.Velocity Vx, Vy, Vz;
    //飞行器当前经度(deg)、纬度(deg)和高度(m)
    input SI.Angle Longitude, Latitude;
    input SI.Length Altitude;
    //参考椭球面长半轴长度(m)和扁率"
    input SI.Length a;
    input Real f;
    //飞行器的经度(rad/s)、纬度(rad/s)、高度变化率(m/s)
    output SI.AngularFrequency dLo, dLa;
    output SI.Velocity dAl;
  protected
    //垂直半径和曲率半径
    Real Nphi, Mphi;
    //椭圆第一偏心率的平方
    Real e2;
    //椭圆短半轴长度
    Real b;
  algorithm
    b := a * (1 - f);
    e2 := 1 - (b / a) ^ 2;
    Nphi := a / sqrt(1 - e2 * sin(Latitude) ^ 2);
    Mphi := (1 - e2) * Nphi ^ 3 / a ^ 2;
    dLa := (Mphi + Altitude) * Vx;
    dLo := (Nphi + Altitude) * cos(Latitude) * Vy;
    dAl := -Vz;
  end vNED2vLLA;

  block NED2LLAinWGS84 "北-东-地坐标系到WGS84坐标转换"
    import D2R = Modelica.Constants.D2R;
    //Longitude0,Latitude0,Altitude0为NED坐标系原点经（deg）纬（deg）高度(m)，a(m)、f为WGS84中地球椭球长半轴长度与扁率
    extends Transform(Longitude0 = 122.04 * D2R, Latitude0 = 37.5 * D2R, Altitude0 = 0, a = 6378137, f = 1 / 298.257223563);
  end NED2LLAinWGS84;

  block NED2LLAinCGCS2000 "北-东-地坐标系到WGS84坐标转换"
    import D2R = Modelica.Constants.D2R;
    //Longitude0,Latitude0,Altitude0为NED坐标系原点经（deg）纬（deg）高度(m)，a(m)、f为CGCS2000中地球椭球长半轴长度与扁率
    extends Transform(Longitude0 = 122.04 * D2R, Latitude0 = 37.5 * D2R, Altitude0 = 0, a = 6378137, f = 1 / 298.257222101);
  end NED2LLAinCGCS2000;
end CoodrnateTransform;