版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/humanking7/article/details/84202605
原创文章,欢迎转载。转载请注明:转载自 祥的博客
原文链接:https://blog.csdn.net/humanking7/article/details/84202605
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;
6.拓展
其实s = V*T
,在递推公式中对速度*采样时间
进行替换,将这个乘积直接用向北位移
和 向东位移
进行替换,也可以得到相应的经纬度
, 不过前提是在个t
时间内,物体保持匀速运动。
7.源码资料
代码资源:https://download.csdn.net/download/humanking7/10792121