[AC]三种方式实现经纬度方程(获取动态物体的经纬度坐标)

祥知道

发布于 2020-03-10 15:54:13

9300

发布于 2020-03-10 15:54:13

文章被收录于专栏：祥的专栏祥的专栏

原创文章，欢迎转载。转载请注明：转载自祥的博客

原文链接：https://cloud.tencent.com/developer/article/1596378

文章目录

- @[toc]0.测试环境1.原理2.递推方式实现3.状态方程实现4.Simulink模块实现5.测试结果6.拓展7.源码资料

0.测试环境

win7 x64
Matlab 2011a

1.原理

飞机或是其他物体在地理坐标系有已知的瞬时 北向速度V_north 和 东向速度 V_east，以及初始点的经纬度(Lat0,Lng0)，求之后飞机或是其他物体的 经纬度。

2.递推方式实现

通过上图的状态方程，可以推到出其递推方程，如下图所示：

利用Matlab的CS-Function进行实现，下面显示核心代码

/*
 通过将运动物体的向北、向东方向的速度进行计算，得到物体当前的经纬度
 只适用于定步长系统 !!!
 
 此程序的意义:
        用的是自己推到的离散化递推方程，可以用于无状态方程的形式
        抛开c-sfunction的状态方程系统
 
 [优先设置参数c程序中]:
        采样时间：T_ 定步长采样时间，在宏定义中，优先设置
 
 [初始化参数]:
        初始经纬度：LatLng0
        
 [输入]:
        向北速度：V_north
        向东速度：V_east
 [输出]:
        纬度：Lat
        经度：Lng
 
 */



#define S_FUNCTION_NAME  xy2LatLng_4_fix
#define S_FUNCTION_LEVEL 2


#include "simstruc.h"



// 优先设置--非常重要
// 可以在该模块前面加上一个Rate Transition模块
// 这样就不用担心仿真环境的采样时间与模型不对等
// 但前提是，仿真环境的采样时间要小于等于T_
// 即就是，仿真环境的采样频率要高于模块的计算频率
#define T_          ( 0.001 ) //每次调用的采样时间,必须是定步长的系统

//常数
#define R_earth     (6378137) //地球半径
#define E_          (0.0818191908426215) //偏率

#define PI          (3.1415926) //
#define r2d         (180.0/PI)
#define d2r         (PI/180.0)

//输入
#define V_north     (u[0])
#define V_east      (u[1])


//输出
#define Lat_out     (y[0])
#define Lng_out     (y[1])


const   real_T *LatLng0;//经纬度初始化值


/*====================*
 * S-function methods *
 *====================*/
static void mdlInitializeSizes(SimStruct *S)
{
    // [参数]:1 传递初始经纬度的参数
    ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /* Return if number of expected != number of actual parameters */
        return;
    }

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    // [input端口]:1
    if (!ssSetNumInputPorts(S, 1)) return;
    // [input端口1维数]:2
    ssSetInputPortWidth(S, 0, 2);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    /*
     * Set direct feedthrough flag (1=yes, 0=no).
     * A port has direct feedthrough if the input is used in either
     * the mdlOutputs or mdlGetTimeOfNextVarHit functions.
     * See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
     */
    ssSetInputPortDirectFeedThrough(S, 0, 1);

    // [out端口]:1
    if (!ssSetNumOutputPorts(S, 1)) return;
    // [out端口1维数]:2
    ssSetOutputPortWidth(S, 0, 2);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}




static void mdlInitializeSampleTimes(SimStruct *S)
{    
    ssSetSampleTime(S,  0,  T_ );//设置采样时间
    ssSetOffsetTime(S, 0, 0.0);
}



#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif /* MDL_INITIALIZE_CONDITIONS */



#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  static void mdlStart(SimStruct *S)
  {
    //参数只会调用1次
    LatLng0 = mxGetPr(ssGetSFcnParam(S,0));//
    
  }
#endif /*  MDL_START */




static void mdlOutputs(SimStruct *S, int_T tid)
{
    static int isFristIntoCode = 1;//是否第一次进入程序
    static real_T Lat;//纬度-用于累加
    static real_T Lng;//经度-用于累加
    real_T R_M;
    real_T R_N;
    real_T sin2_Lat;// sin(Lat)**2
    real_T tmp_EE,tmp1,tmp2;//
    real_T tmp;
    
    
    
    const real_T *u = (const real_T*) ssGetInputPortSignal(S,0);//[0] V_north,  [1] V_east
    real_T       *y = ssGetOutputPortSignal(S,0);//[0] Lat_out,  [1] Lng_out

    
    
    //初始化经纬度初始值
    if(1==isFristIntoCode)
    {//只会调用1次
        Lat = LatLng0[0];
        Lng = LatLng0[1];
        isFristIntoCode = 0;
    }

    
    //Step1.更新R_M
    tmp = Lat*d2r;
    sin2_Lat = sin(tmp)*sin(tmp);
    tmp_EE = (1-E_)*(1-E_);
    tmp1 = ( 1-(2-E_)*E_*sin2_Lat );
    tmp2 = pow( tmp1, -0.5 );
    
    R_M = R_earth*tmp_EE*pow(tmp2, 3);
    
    //Step2.更新R_N
    R_N = R_earth*tmp2;
    
    //Step3.更新纬度Lat
    Lat = Lat + (V_north/R_M)*T_*r2d;
    
    //Step4.更新经度Lng
    tmp = V_east/( R_N*cos(Lat*d2r) );
    Lng = Lng + tmp*T_*r2d;
    
    //Step5.输出
    Lat_out = Lat;
    Lng_out = Lng;
    

}



#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */



#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  static void mdlDerivatives(SimStruct *S)
  {
  }
#endif /* MDL_DERIVATIVES */




static void mdlTerminate(SimStruct *S)
{
}


#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif

3.状态方程实现

其实不需要自己推到递推方程，直接用CS-Function的状态方程形式就可以搞定，而且不需要关心Simulink环境的步长问题。

/*
 通过将运动物体的向北、向东方向的速度进行计算，得到物体当前的经纬度
 只适用于所有系统 !!!
 
 
 利用状态方程，不用再离散算法了
 方便简单，适用于Matlab
 
 
 [初始化参数]:
        初始经纬度：LatLng0
        
 [输入]:
        向北速度：V_north
        向东速度：V_east
 [输出]:
        纬度：Lat
        经度：Lng
 
 */



#define S_FUNCTION_NAME  xy2LatLng_4_all
#define S_FUNCTION_LEVEL 2


#include "simstruc.h"





//常数
#define R_earth     (6378137) //地球半径
#define E_          (0.0818191908426215) //偏率

#define PI          (3.1415926) //
#define r2d         (180.0/PI)
#define d2r         (PI/180.0)



//输入
#define V_north     (u[0])
#define V_east      (u[1])

//状态变量
#define Lat      (x[0]) //（弧度）
#define Lng      (x[1])//（弧度）

//状态变量
#define Lat_dot      (dx[0])//（弧度）
#define Lng_dot      (dx[1])//（弧度）

//输出
#define Lat_out     (y[0])//（角度）
#define Lng_out     (y[1])//（角度）





/*====================*
 * S-function methods *
 *====================*/

static void mdlInitializeSizes(SimStruct *S)
{
    // [参数]:1
    ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /* Return if number of expected != number of actual parameters */
        return;
    }

    //[状态变量]: 2
    ssSetNumContStates(S, 2);
    ssSetNumDiscStates(S, 0);

    // [input端口]:1
    if (!ssSetNumInputPorts(S, 1)) return;
    // [input端口1维数]:2
    ssSetInputPortWidth(S, 0, 2);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/

    ssSetInputPortDirectFeedThrough(S, 0, 1);

    // [out端口]:1
    if (!ssSetNumOutputPorts(S, 1)) return;
    // [out端口1维数]:2
    ssSetOutputPortWidth(S, 0, 2);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}



static void mdlInitializeSampleTimes(SimStruct *S)
{   
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}



#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  static void mdlInitializeConditions(SimStruct *S)
  {
        //赋值给初始的经纬度
        real_T *x = ssGetContStates(S);
        int i;        
        /* Initialize the state vector */
        for (i = 0; i < ssGetNumContStates(S); i++)
        {
            //输入参数是角度值，状态变量是弧度制
            x[i] = d2r * ( mxGetPr(ssGetSFcnParam(S, 0))[i] ); 
        }
  }
#endif /* MDL_INITIALIZE_CONDITIONS */



#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  static void mdlStart(SimStruct *S)
  {
    
  }
#endif /*  MDL_START */



static void mdlOutputs(SimStruct *S, int_T tid)
{   
    real_T       *x  = ssGetContStates(S);//[0] Lat,  [1] Lng (弧度制)
    real_T       *y = ssGetOutputPortSignal(S,0);//[0] Lat_out,  [1] Lng_out (角度制)

        
    //输出
    Lat_out = Lat * r2d;
    Lng_out = Lng * r2d;
    

}



#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */



#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  /* Function: mdlDerivatives =================================================
   * Abstract:
   *    In this function, you compute the S-function block's derivatives.
   *    The derivatives are placed in the derivative vector, ssGetdX(S).
   */
  static void mdlDerivatives(SimStruct *S)
  {
    real_T R_M;
    real_T R_N;
    real_T sin2_Lat;// sin(Lat)**2
    real_T tmp_EE, tmp1, tmp2;//
    
    
    const real_T *u = (const real_T*) ssGetInputPortSignal(S,0);//[0] V_north,  [1] V_east
    real_T *x  = ssGetContStates(S);//[0] Lat,  [1] Lng (弧度制)
    real_T *dx = ssGetdX(S);//[0] Lat_dot,  [1] Lng_dot (弧度制)
    
    //Step1.更新R_M    
    sin2_Lat = sin(Lat)*sin(Lat);
    tmp_EE = (1-E_)*(1-E_);
    tmp1 = ( 1-(2-E_)*E_*sin2_Lat );
    tmp2 = pow( tmp1, -0.5 );    
    R_M = R_earth*tmp_EE*pow(tmp2, 3);
    
    //Step2.更新R_N
    R_N = R_earth*tmp2;
    
    
    //Step3. 更新状态方程
    //================
    //Step3.1.  更新纬度Lat    
    Lat_dot = V_north / R_M;
    //Step3.2.  更新经度Lng    
    Lng_dot = V_east / ( R_N * cos(Lat) );    
    
  }
#endif /* MDL_DERIVATIVES */


static void mdlTerminate(SimStruct *S)
{
}



#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif

4.Simulink模块实现

这个没什么好说的，直接用模块搭建，而且有积分模块，非常方便。只是通过模块反推公式简直就是噩梦。

5.测试结果

初始化代码：

%% 设置常量
r2d   =  180.0 / pi;
d2r   =  pi / 180.0;



%% 设置初始化条件
Lat0 = 33.70941849;
Lng0 = 110.26690006;

LatLng0 = [Lat0, Lng0];

%% 编译
mex xy2LatLng_4_fix.c
mex xy2LatLng_4_all.c

画图代码：

close all


%% 画fix（用递推公式计算）数据
len1 = length(Lng_qfx_fix);
interv1 = 50;
start1 = 1;
plot(Lng_qfx_fix(start1 : interv1 : len1),  Lat_qfx_fix(start1 : interv1 : len1), 'r*' );
hold on


%% 画all（用状态方程计算）数据
len2 = length(Lng_qfx_all);
interv2 = 100;
start2 = 150;
plot(Lng_qfx_all(start2 : interv2 : len2),  Lat_qfx_all(start2 : interv2 : len2), 'bo' );
hold on


%% 画simulink模块搭建计算的数据
len3 = length(Lng_module);
interv3 = 50;
start3 = 50;
plot(Lng_module(start3 : interv3 : len3),  Lat_module(start3 : interv3 : len3), 'k.-', 'markersize', 10 );
hold on



%% 加标签
xlabel('经度-longitude');
ylabel('纬度-latitude');
legend('qfx_f_i_x',   'qfx_a_l_l',  'use\_mod');


axis equal;