资源简介
绝对是好东西啊！基于极大似然估计的三维定位算法，输入参考点的坐标以及观察点的距离，通过极大似然估计算法计算出观察点的坐标，附送支持矩阵运算的C++类库！
代码片段和文件信息
// LMLE.cpp : 定义控制台应用程序的入口点。
//
#include “stdafx.h“
//
//
//int _tmain（int argc _TCHAR* argv[]）
//{
// return 0;
//}
// LMLE.cpp : 定义控制台应用程序的入口点。
//
///////////////////////////必要的包含头///////////////////////////////////
//如果要使用Matrix类，就必须包含下面文件
#include “matrix.h“
//为保证正确使用计算空间，进行如下的命名空间引用以及宏定义
#ifndef _NO_NAMESPACE
using namespace std;
using namespace math;
#define STD std
#else
#define STD
#endif
#ifndef _NO_TEMPLATE
typedef matrix Matrix;
#else
typedef matrix Matrix;
#endif
//如果要讲LMLE函数进行移植的话，请将上面部分保留到移植的文件中
//////////////////////////////////////////////////////////////////////////
// LMLE用于计算已知n个观测点P的三维坐标，以及到被观测目标的距离L的情况下，估算观测目标的三维坐标
// 因L存在误差因此采用LMLE即线性最大似然比模型求估计解
// 为保证计算不会出错，请一定保证每个观测点位置是不一样，即观测值不能完全一样，这样保证矩阵满秩，才可以求逆
double * LMLE（double * Pdouble *Lint n）
{
Matrix A（n3）;//观测矩阵
Matrix b（n1）;//观测结果
Matrix X（31）;//求解坐标，xdydhd
//下面根据PL生成观测矩阵H
int ijk;
double xkxjykyjhkhjljlk;
for （i=0;i
j = i; k = （i+1）%n;//后面的点与前面的点求差，最后一个与第一个求差
xk = *（P+3*k）;
xj = *（P+3*j）;
yk = *（P+3*k+1）;
yj = *（P+3*j+1）;
hk = *（P+3*k+2）;
hj = *（P+3*j+2）;
lk = *（L+k）;
lj = *（L+j）;
A（i0） = 2*（xk-xj）;
A（i1） = 2*（yk-yj）;
A（i2） = 2*（hk-hj）;
b（i0）= lj*lj-lk*lk+xk*xk-xj*xj+yk*yk-yj*yj+hk*hk-hj*hj;//即公式：lj^2-lk^2+xk^2-xj^2+yk^2-yj^2+hk^2-hj^2
}
X = （!（（~A）*A））*（~A）*b;//即（AT*A）*（AT）*bAT表示A的转置
double * P0 = new double[3];
P0[0] = X（00）;
P0[1] = X（10）;
P0[2] = X（20）;
return P0;
}
int main（int argc char* argv[]）
{
double * P; //已知的几个观测位置坐标，格式：x1y1h1x2y2h2....
double * L; //观测到的距离，格式：L1L2L3...
int n; //观测站的数目：3到8
//以下是数据输入初始化，请根据自己需要，自行设置：
n = 4;
P = new double[12];
P[0] = 4.2;
P[1] = 20.0;
P[2] = 0.0;
P[3] = 8.2;
P[4] = 13.7;
P[5] = 0.5;
P[6] = 4.7;
P[7] = 17.2;
P[8] = 1.0;
P[9] = 5.5;
P[10] = 10.5;
P[11] = 0;
L = new double[4];
L[0] = 2.881996;
L[1] = 2.881996;
L[2] = 2.998050;
L[3] = 2.939451;
//调用LMLE（）计算输出结果如果前面数据初始化了，下面两行可以去除注释，可以计算结果
double * p0;//计算出来的结果，p0[3]方式存储
p0 = LMLE（PLn）;
//输出p0
printf（“xd=%fyd=%fhd=%f\n“p0[0]p0[1]p0[2]）;
scanf（“%d“&n）;
return 0;
}
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 61440 2012-11-02 14:01 LMLE\debug\LMLE.exe
文件 387516 2012-11-02 14:01 LMLE\debug\LMLE.ilk
文件 584704 2012-11-02 14:01 LMLE\debug\LMLE.pdb
文件 20944 2012-11-02 14:01 LMLE\LMLE\Debug\BuildLog.htm
文件 406 2012-11-02 14:00 LMLE\LMLE\Debug\LMLE.exe.em
文件 472 2012-11-02 14:00 LMLE\LMLE\Debug\LMLE.exe.em
文件 388 2012-11-02 14:01 LMLE\LMLE\Debug\LMLE.exe.intermediate.manifest
文件 77715 2012-11-02 14:01 LMLE\LMLE\Debug\LMLE.obj
文件 1048576 2012-11-02 14:00 LMLE\LMLE\Debug\LMLE.pch
文件 65 2012-11-02 14:01 LMLE\LMLE\Debug\mt.dep
文件 10868 2012-11-02 14:00 LMLE\LMLE\Debug\stdafx.obj
文件 183296 2012-11-02 14:01 LMLE\LMLE\Debug\vc80.idb
文件 249856 2012-11-02 14:01 LMLE\LMLE\Debug\vc80.pdb
文件 2600 2012-11-02 14:01 LMLE\LMLE\LMLE.cpp
文件 4538 2012-09-05 14:27 LMLE\LMLE\LMLE.vcproj
文件 1403 2012-11-02 14:03 LMLE\LMLE\LMLE.vcproj.HZT.Administrator.user
文件 27343 2012-09-05 14:27 LMLE\LMLE\matrix.h
文件 932 2012-09-05 14:27 LMLE\LMLE\ReadMe.txt
文件 21804 2012-11-02 14:03 LMLE\LMLE\Release\BuildLog.htm
文件 383 2012-11-02 14:03 LMLE\LMLE\Release\LMLE.exe.intermediate.manifest
文件 463479 2012-11-02 14:03 LMLE\LMLE\Release\LMLE.obj
文件 1048576 2012-11-02 14:03 LMLE\LMLE\Release\LMLE.pch
文件 65 2012-11-02 14:03 LMLE\LMLE\Release\mt.dep
文件 29508 2012-11-02 14:03 LMLE\LMLE\Release\stdafx.obj
文件 101376 2012-11-02 14:03 LMLE\LMLE\Release\vc80.idb
文件 217088 2012-11-02 14:03 LMLE\LMLE\Release\vc80.pdb
文件 209 2012-09-05 14:27 LMLE\LMLE\stdafx.cpp
文件 378 2012-09-05 14:27 LMLE\LMLE\stdafx.h
文件 2059264 2012-11-02 14:03 LMLE\LMLE.ncb
文件 877 2012-09-05 14:27 LMLE\LMLE.sln
............此处省略12个文件信息
川公网安备 51152502000135号
评论
共有 条评论