本文共 22951 字,大约阅读时间需要 76 分钟。
空间3点投影定位算法
本文博客链接:,作者:jdh,转载请注明.
环境:
主机:WIN7
开发环境:Qt
说明:
<<仿的4星定位程序>>()提供了空间4点定位1点的算法.实际中此算法需要4个基站有较大的高度差,如果在同一高度则定位误差很大.实际中,定位基站一般装在同一平面.利用平面投影可以将空间定位转换为平面定位从而避免这个问题.
具体做法:
每个基站具有平面坐标(x,y)以及一个高度h.测到距离d后,对距离进行平面投影处理:d = sqrt(d^2 - h^2).接下来就是平面定位.
测试程序:
界面.ui:
矩阵头文件_matrix.h:Widget 0 0 561 372 Widget 60 270 75 23 计算 60 70 41 20 0 360 70 113 20 1 210 20 181 21 16 空间3点投影定位 10 70 48 12 第1点: 300 70 48 12 距离: 10 110 48 12 第2点: 300 110 48 12 距离: 10 150 48 12 第3点: 300 150 48 12 距离: 60 110 41 20 0 360 110 113 20 1.414 60 150 41 20 1 360 150 113 20 1 310 250 48 12 定位: 350 270 113 20 350 290 113 20 320 270 21 16 x: 320 290 21 16 y: 100 70 41 20 0 140 70 41 20 0 100 110 41 20 1 140 110 41 20 0 100 150 41 20 1 140 150 41 20 0
#ifndef _MATRIX_H#define _MATRIX_H//头文件#include矩阵函数_matrix.cpp:#include //矩阵数据结构//二维矩阵struct _Matrix{ int m; int n; float *arr;};//矩阵方法//设置mnvoid matrix_set(struct _Matrix *m,int mm,int nn);//设置mvoid matrix_set_m(struct _Matrix *m,int mm);//设置nvoid matrix_set_n(struct _Matrix *m,int nn);//初始化void matrix_init(struct _Matrix *m);//释放void matrix_free(struct _Matrix *m);//读取i,j坐标的数据//失败返回-31415,成功返回值float matrix_read(struct _Matrix *m,int i,int j);//写入i,j坐标的数据//失败返回-1,成功返回1int matrix_write(struct _Matrix *m,int i,int j,float val);//矩阵运算//成功返回1,失败返回-1int matrix_add(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);//C = A - B//成功返回1,失败返回-1int matrix_subtract(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);//C = A * B//成功返回1,失败返回-1int matrix_multiply(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);//行列式的值,只能计算2 * 2,3 * 3//失败返回-31415,成功返回值float matrix_det(struct _Matrix *A);//求转置矩阵,B = AT//成功返回1,失败返回-1int matrix_transpos(struct _Matrix *A,struct _Matrix *B);//求逆矩阵,B = A^(-1)//成功返回1,失败返回-1int matrix_inverse(struct _Matrix *A,struct _Matrix *B);//矩阵拷贝:A = B//成功返回1,失败返回-1int matrix_copy(struct _Matrix *A,struct _Matrix *B);//求方程根,A * X = B,//成功返回1,答案保存在C中,失败返回-1//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1int matrix_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);//利用克莱姆法则求方程根,A * X = B,//成功返回1,答案保存在C中,失败返回-1//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1int matrix_det_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C);//定位函数//PI:输入3点坐标,格式:是3 * 2维//D:4点距离未知点距离数组//PO:输出坐标//成功返回1,失败返回-1int locate(struct _Matrix *PI,float *D,float *PO);#endif
#include "_matrix.h"//矩阵方法//设置mnvoid matrix_set(struct _Matrix *m,int mm,int nn){ m->m = mm; m->n = nn;}//设置mvoid matrix_set_m(struct _Matrix *m,int mm){ m->m = mm;}//设置nvoid matrix_set_n(struct _Matrix *m,int nn){ m->n = nn;}//初始化void matrix_init(struct _Matrix *m){ m->arr = (float *)malloc(m->m * m->n * sizeof(float));}//释放void matrix_free(struct _Matrix *m){ free(m->arr);}//读取i,j坐标的数据//失败返回-31415,成功返回值float matrix_read(struct _Matrix *m,int i,int j){ if (i >= m->m || j >= m->n) { return -31415; } return *(m->arr + i * m->n + j);}//写入i,j坐标的数据//失败返回-1,成功返回1int matrix_write(struct _Matrix *m,int i,int j,float val){ if (i >= m->m || j >= m->n) { return -1; } *(m->arr + i * m->n + j) = val; return 1;}//矩阵运算//成功返回1,失败返回-1int matrix_add(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C){ int i = 0; int j = 0; //判断是否可以运算 if (A->m != B->m || A->n != B->n || \ A->m != C->m || A->n != C->n) { return -1; } //运算 for (i = 0;i < C->m;i++) { for (j = 0;j < C->n;j++) { matrix_write(C,i,j,matrix_read(A,i,j) + matrix_read(B,i,j)); } } return 1;}//C = A - B//成功返回1,失败返回-1int matrix_subtract(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C){ int i = 0; int j = 0; //判断是否可以运算 if (A->m != B->m || A->n != B->n || \ A->m != C->m || A->n != C->n) { return -1; } //运算 for (i = 0;i < C->m;i++) { for (j = 0;j < C->n;j++) { matrix_write(C,i,j,matrix_read(A,i,j) - matrix_read(B,i,j)); } } return 1;}//C = A * B//成功返回1,失败返回-1int matrix_multiply(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C){ int i = 0; int j = 0; int k = 0; float temp = 0; //判断是否可以运算 if (A->m != C->m || B->n != C->n || \ A->n != B->m) { return -1; } //运算 for (i = 0;i < C->m;i++) { for (j = 0;j < C->n;j++) { temp = 0; for (k = 0;k < A->n;k++) { temp += matrix_read(A,i,k) * matrix_read(B,k,j); } matrix_write(C,i,j,temp); } } return 1;}//行列式的值,只能计算2 * 2,3 * 3//失败返回-31415,成功返回值float matrix_det(struct _Matrix *A){ float value = 0; //判断是否可以运算 if (A->m != A->n || (A->m != 2 && A->m != 3)) { return -31415; } //运算 if (A->m == 2) { value = matrix_read(A,0,0) * matrix_read(A,1,1) - matrix_read(A,0,1) * matrix_read(A,1,0); } else { value = matrix_read(A,0,0) * matrix_read(A,1,1) * matrix_read(A,2,2) + \ matrix_read(A,0,1) * matrix_read(A,1,2) * matrix_read(A,2,0) + \ matrix_read(A,0,2) * matrix_read(A,1,0) * matrix_read(A,2,1) - \ matrix_read(A,0,0) * matrix_read(A,1,2) * matrix_read(A,2,1) - \ matrix_read(A,0,1) * matrix_read(A,1,0) * matrix_read(A,2,2) - \ matrix_read(A,0,2) * matrix_read(A,1,1) * matrix_read(A,2,0); } return value;}//求转置矩阵,B = AT//成功返回1,失败返回-1int matrix_transpos(struct _Matrix *A,struct _Matrix *B){ int i = 0; int j = 0; //判断是否可以运算 if (A->m != B->n || A->n != B->m) { return -1; } //运算 for (i = 0;i < B->m;i++) { for (j = 0;j < B->n;j++) { matrix_write(B,i,j,matrix_read(A,j,i)); } } return 1;}//求逆矩阵,B = A^(-1)//成功返回1,失败返回-1int matrix_inverse(struct _Matrix *A,struct _Matrix *B){ int i = 0; int j = 0; int k = 0; struct _Matrix m; float temp = 0; float b = 0; //判断是否可以运算 if (A->m != A->n || B->m != B->n || A->m != B->m) { return -1; } /* //如果是2维或者3维求行列式判断是否可逆 if (A->m == 2 || A->m == 3) { if (det(A) == 0) { return -1; } } */ //增广矩阵m = A | B初始化 matrix_set_m(&m,A->m); matrix_set_n(&m,2 * A->m); matrix_init(&m); for (i = 0;i < m.m;i++) { for (j = 0;j < m.n;j++) { if (j <= A->n - 1) { matrix_write(&m,i,j,matrix_read(A,i,j)); } else { if (i == j - A->n) { matrix_write(&m,i,j,1); } else { matrix_write(&m,i,j,0); } } } } //高斯消元 //变换下三角 for (k = 0;k < m.m - 1;k++) { //如果坐标为k,k的数为0,则行变换 if (matrix_read(&m,k,k) == 0) { for (i = k + 1;i < m.m;i++) { if (matrix_read(&m,i,k) != 0) { break; } } if (i >= m.m) { return -1; } else { //交换行 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,k,j); matrix_write(&m,k,j,matrix_read(&m,k + 1,j)); matrix_write(&m,k + 1,j,temp); } } } //消元 for (i = k + 1;i < m.m;i++) { //获得倍数 b = matrix_read(&m,i,k) / matrix_read(&m,k,k); //行变换 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j); matrix_write(&m,i,j,temp); } } } //变换上三角 for (k = m.m - 1;k > 0;k--) { //如果坐标为k,k的数为0,则行变换 if (matrix_read(&m,k,k) == 0) { for (i = k + 1;i < m.m;i++) { if (matrix_read(&m,i,k) != 0) { break; } } if (i >= m.m) { return -1; } else { //交换行 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,k,j); matrix_write(&m,k,j,matrix_read(&m,k + 1,j)); matrix_write(&m,k + 1,j,temp); } } } //消元 for (i = k - 1;i >= 0;i--) { //获得倍数 b = matrix_read(&m,i,k) / matrix_read(&m,k,k); //行变换 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j); matrix_write(&m,i,j,temp); } } } //将左边方阵化为单位矩阵 for (i = 0;i < m.m;i++) { if (matrix_read(&m,i,i) != 1) { //获得倍数 b = 1 / matrix_read(&m,i,i); //行变换 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,i,j) * b; matrix_write(&m,i,j,temp); } } } //求得逆矩阵 for (i = 0;i < B->m;i++) { for (j = 0;j < B->m;j++) { matrix_write(B,i,j,matrix_read(&m,i,j + m.m)); } } //释放增广矩阵 matrix_free(&m); return 1;}//矩阵拷贝:A = B//成功返回1,失败返回-1int matrix_copy(struct _Matrix *A,struct _Matrix *B){ int i = 0; int j = 0; if (A->m != B->m || A->n != B->n) { return -1; } for (i = 0;i < A->m;i++) { for (j = 0;j < A->n;j++) { matrix_write(B,i,j,matrix_read(A,i,j)); } } return 1;}//求方程根,A * X = B,//成功返回1,答案保存在C中,失败返回-1//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1int matrix_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C){ int i = 0; int j = 0; int k = 0; struct _Matrix m; float temp = 0; float b = 0; //判断是否可以运算 if (A->m != A->n || B->n != 1 || A->m != B->m || \ C->n != 1 || A->m != C->m) { return -1; } /* //如果是2维或者3维求行列式判断是否可逆 if (A->m == 2 || A->m == 3) { if (det(A) == 0) { return -1; } } */ //增广矩阵m = A | B初始化 matrix_set_m(&m,A->m); matrix_set_n(&m,A->m + 1); matrix_init(&m); for (i = 0;i < m.m;i++) { for (j = 0;j < m.n;j++) { if (j <= A->n - 1) { matrix_write(&m,i,j,matrix_read(A,i,j)); } else { matrix_write(&m,i,j,matrix_read(B,i,0)); } } } //高斯消元 //变换下三角 for (k = 0;k < m.m - 1;k++) { //如果坐标为k,k的数为0,则行变换 if (matrix_read(&m,k,k) == 0) { for (i = k + 1;i < m.m;i++) { if (matrix_read(&m,i,k) != 0) { break; } } if (i >= m.m) { return -1; } else { //交换行 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,k,j); matrix_write(&m,k,j,matrix_read(&m,k + 1,j)); matrix_write(&m,k + 1,j,temp); } } } //消元 for (i = k + 1;i < m.m;i++) { //获得倍数 b = matrix_read(&m,i,k) / matrix_read(&m,k,k); //行变换 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j); matrix_write(&m,i,j,temp); } } } //变换上三角 for (k = m.m - 1;k > 0;k--) { //如果坐标为k,k的数为0,则行变换 if (matrix_read(&m,k,k) == 0) { for (i = k + 1;i < m.m;i++) { if (matrix_read(&m,i,k) != 0) { break; } } if (i >= m.m) { return -1; } else { //交换行 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,k,j); matrix_write(&m,k,j,matrix_read(&m,k + 1,j)); matrix_write(&m,k + 1,j,temp); } } } //消元 for (i = k - 1;i >= 0;i--) { //获得倍数 b = matrix_read(&m,i,k) / matrix_read(&m,k,k); //行变换 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,i,j) - b * matrix_read(&m,k,j); matrix_write(&m,i,j,temp); } } } //将左边方阵化为单位矩阵 for (i = 0;i < m.m;i++) { if (matrix_read(&m,i,i) != 1) { //获得倍数 b = 1 / matrix_read(&m,i,i); //行变换 for (j = 0;j < m.n;j++) { temp = matrix_read(&m,i,j) * b; matrix_write(&m,i,j,temp); } } } //求得解 for (i = 0;i < C->m;i++) { matrix_write(C,i,0,matrix_read(&m,i,m.n - 1)); } //释放增广矩阵 matrix_free(&m); return 1;}//利用克莱姆法则求方程根,A * X = B,//成功返回1,答案保存在C中,失败返回-1//要求:A必须是方阵,如果A是m*m方阵,则B必须是m * 1,C必须是m * 1int matrix_det_solve(struct _Matrix *A,struct _Matrix *B,struct _Matrix *C){ struct _Matrix m; float det_m; float det_m_temp; int i = 0; int j = 0; //初始化m matrix_set_m(&m,A->m); matrix_set_n(&m,A->n); matrix_init(&m); //得到A的行列式值 det_m = matrix_det(A); //判断是否有效 if (det_m == 0) { matrix_free(&m); return -1; } for (i = 0;i < 2;i++) { //得到新的行列式 matrix_copy(A,&m); for (j = 0;j < 2;j++) { matrix_write(&m,j,i,matrix_read(B,j,0)); } det_m_temp = matrix_det(&m); //求解 matrix_write(C,i,0,det_m_temp / det_m); } matrix_free(&m); return 1;}//定位函数//PI:输入3点坐标,格式:是3 * 2维//D:3点距离未知点距离数组//PO:输出坐标//成功返回1,失败返回-1int locate(struct _Matrix *PI,float *D,float *PO){ int i = 0; int j = 0; struct _Matrix A; struct _Matrix B; struct _Matrix C; float temp = 0; //判断是否可以运算 if (PI->m != 3 || PI->n != 2) { return -1; } //初始化ABC矩阵 matrix_set_m(&A,2); matrix_set_n(&A,2); matrix_init(&A); matrix_set_m(&B,2); matrix_set_n(&B,1); matrix_init(&B); matrix_set_m(&C,2); matrix_set_n(&C,1); matrix_init(&C); //初始化A矩阵 for (i = 0;i < 2;i++) { for (j = 0;j < 2;j++) { temp = matrix_read(PI,i + 1,j) - matrix_read(PI,i,j); matrix_write(&A,i,j,temp); } } //初始化B矩阵 for (i = 0;i < 2;i++) { temp = matrix_read(PI,i + 1,0) * matrix_read(PI,i + 1,0); temp += matrix_read(PI,i + 1,1) * matrix_read(PI,i + 1,1); temp -= matrix_read(PI,i,0) * matrix_read(PI,i,0); temp -= matrix_read(PI,i,1) * matrix_read(PI,i,1); temp -= D[i + 1] * D[i + 1] - D[i] * D[i]; temp /= 2; matrix_write(&B,i,0,temp); } //解方程 //if (matrix_solve(&A,&B,&C) > 0) if (matrix_det_solve(&A,&B,&C) > 0) { PO[0] = matrix_read(&C,0,0); PO[1] = matrix_read(&C,1,0); return 1; } return -1;}widget.h:
#ifndef WIDGET_H#define WIDGET_H#includewidget.cpp:#include "public.h"#include "_four_point_locate.h"#include "_Matrix.h"namespace Ui { class Widget;}class Widget : public QWidget{ Q_OBJECTpublic: explicit Widget(QWidget *parent = 0); ~Widget();private: Ui::Widget *ui; _Four_Point_Locate four_point_locate; _Matrix pi; float d[3]; float p[2];private slots: void on_pushButton_clicked();};#endif // WIDGET_H
#include "widget.h"#include "ui_widget.h"#include "math.h"Widget::Widget(QWidget *parent) : QWidget(parent), ui(new Ui::Widget){ ui->setupUi(this); //初始化矩阵 matrix_set(&pi,3,2); matrix_init(&pi);}Widget::~Widget(){ delete ui;}void Widget::on_pushButton_clicked(){ bool ok; float h = 0; int i = 0; int j = 0; float temp = 0; //距离 d[0] = ui->d1->text().toFloat(&ok); d[1] = ui->d2->text().toFloat(&ok); d[2] = ui->d3->text().toFloat(&ok); //获得坐标 matrix_write(&pi,0,0,ui->p1x->text().toFloat(&ok)); matrix_write(&pi,0,1,ui->p1y->text().toFloat(&ok)); h = ui->p1z->text().toFloat(&ok); d[0] = sqrt(d[0] * d[0] - h * h); matrix_write(&pi,1,0,ui->p2x->text().toFloat(&ok)); matrix_write(&pi,1,1,ui->p2y->text().toFloat(&ok)); h = ui->p2z->text().toFloat(&ok); d[1] = sqrt(d[1] * d[1]- h * h); matrix_write(&pi,2,0,ui->p3x->text().toFloat(&ok)); matrix_write(&pi,2,1,ui->p3y->text().toFloat(&ok)); h = ui->p3z->text().toFloat(&ok); d[2] = sqrt(d[2] * d[2] - h * h); qDebug() << "pi" << matrix_read(&pi,0,0) << matrix_read(&pi,0,1); qDebug() << "pi" << matrix_read(&pi,1,0) << matrix_read(&pi,1,1); qDebug() << "pi" << matrix_read(&pi,2,0) << matrix_read(&pi,2,1); qDebug() << "d:" << d[0] << d[1] << d[2]; if (locate(&pi,d,p) > 0) { //成功 ui->outx->setText(QString::number(p[0])); ui->outy->setText(QString::number(p[1])); } else { //失败 ui->outx->setText("fail"); ui->outy->setText("fail"); }}运行效果:
再分享一下我老师大神的人工智能教程吧。零基础!通俗易懂!风趣幽默!还带黄段子!希望你也加入到我们人工智能的队伍中来!