仿GPS的4星定位程序
本文博客链接:http://blog.csdn.net/jdh99,作者:jdh,转载请注明.
环境:
主机:WIN7
开发环境:Qt
功能:
给4点坐标,以及与未知点的距离,求出未知点坐标.
说明:
现在在做一个原理类似GPS的定位系统.所以需要解一个定位方程组.与GPS不同的是我这边没有接收机和卫星的时间差.
解方程的原理是根据圆公式列出4个距离方程,然后用线性代数的克莱姆法则求解.
设未知点向量为P,空间已知4点向量为Vi(i = 1:4),4点距离未知点坐标为di(i = 1:4).
则有方程:di^2 = (P - V)^2.
则有4个方程:
P^2 - 2V1 * P = d1 - V^2
P^2 - 2V2 * P = d2 - V^2
P^2 - 2V3 * P = d3 - V^2
P^2 - 2V4 * P = d4 - V^2
两两相减得到3个线性方程:
(V2 - V1) * P = 1 / 2 (V2^2 - V1^2 - (d2^2 - d1^2))
(V3 - V2) * P = 1 / 2 (V3^2 - V2^2 - (d3^2 - d2^2))
(V4 - V3) * P = 1 / 2 (V4^2 - V3^2 - (d4^2 - d3^2))
设等式左边为矩阵A,右边为矩阵B,则有:
A * P = B
求解A的行列式的值,如果不为0,则有解.然后用克莱姆法则求解出未知点坐标.
下面是测试程序.
界面:
源程序:
public.h:
#ifndef PUBLIC_H
#define PUBLIC_H
//头文件
#include "QDebug"
//输出
void printf_matrix(float *m);
//三维行列式的值
//m:3 * 3数组
double det(float *m);
//将一个行列式的值赋给另一个
//src,dst:3 * 3数组
void copy_matrix(float *src,float *dst);
//解方程
//m:方阵,3 * 3数组
//b:解
//x:返回值
//fail:back -1
int solve(float *m,float *b,float *x);
//计算空间点到原点距离的平方
float d_p_square(float *p);
#endif // PUBLIC_H
public.c:
#include "public.h"
//输出
void printf_matrix(float *m)
{
int i = 0;
int j = 0;
for (i = 0;i < 3;i++)
{
for (j = 0;j < 3;j++)
{
//printf("%f\t",*(m + i * 3 + j));
qDebug() << *(m + i * 3 + j);
}
}
}
//三维行列式的值
//m:3 * 3数组
double det(float *m)
{
double value = 0.0;
value = (m + 0)[0] * (m + 3)[1] * (m + 6)[2] + \
(m + 0)[1] * (m + 3)[2] * (m + 6)[0] + \
(m + 0)[2] * (m + 3)[0] * (m + 6)[1] - \
(m + 0)[1] * (m + 3)[0] * (m + 6)[2] - \
(m + 0)[2] * (m + 3)[1] * (m + 6)[0] - \
(m + 0)[0] * (m + 3)[2] * (m + 6)[1];
return value;
}
//将一个行列式的值赋给另一个
//src,dst:3 * 3数组
void copy_matrix(float *src,float *dst)
{
int i = 0;
int j = 0;
for (i = 0;i < 3;i++)
{
for (j = 0;j < 3;j++)
{
*(dst + i * 3 + j) = *(src + i * 3 + j);
}
}
}
//解方程
//m:方阵,3 * 3数组
//b:解
//fail:back -1
int solve(float *m,float *b,float *x)
{
float det_m;
float det_m_temp;
float m_temp[3][3];
int i = 0;
int j = 0;
det_m = det(m);
if (det_m == 0)
{
return -1;
}
for (j = 0;j < 3;j++)
{
//得到新的行列式
copy_matrix(m,m_temp[0]);
for (i = 0;i < 3;i++)
{
m_temp[i][j] = *(b + i);
}
det_m_temp = det(m_temp[0]);
//求解
x[j] = det_m_temp / det_m;
}
return 0;
}
//计算空间点到原点距离的平方
float d_p_square(float *p)
{
float d = 0;
int i = 0;
for (i = 0;i < 3;i++)
{
d += (*(p + i)) * (*(p + i));
}
return d;
}
_four_point_locate.h:(4点定位计算类)
#ifndef _FOUR_POINT_LOCATE_H
#define _FOUR_POINT_LOCATE_H
#include "public.h"
class _Four_Point_Locate
{
private:
//空间已知4点坐标
float p[4][3];
//空间已知4点距离
float d[4] ;
public:
_Four_Point_Locate();
//初始化空间4点坐标
//point:坐标,数组
//num:1-4
void set_point(float *point,int num);
//初始化空间4点距离
//distance:距离
//num:1-4
void set_distance(float distance,int num);
//计算未知点坐标
//p:计算后的返回值
//fail:back -1
int calc(float *point);
};
#endif // _FOUR_POINT_LOCATE_H
_four_point_locate.cpp
#include "_four_point_locate.h"
_Four_Point_Locate::_Four_Point_Locate()
{
}
//初始化空间4点坐标
//p:坐标,数组
//num:1-4
void _Four_Point_Locate::set_point(float *point,int num)
{
int j = 0;
for (j = 0;j < 3;j++)
{
p[num - 1][j] = point[j];
}
}
//初始化空间4点距离
//distance:距离
//num:1-4
void _Four_Point_Locate::set_distance(float distance,int num)
{
d[num - 1] = distance;
}
//计算未知点坐标
//p:计算后的返回值
//fail:back -1
int _Four_Point_Locate::calc(float *point)
{
//矩阵A
float A[3][3];
//矩阵B
float B[3];
int i = 0;
int j = 0;
//初始化B矩阵
for (i = 0;i < 3;i++)
{
B[i] = (d_p_square(p[i + 1]) - d_p_square(p[i]) - (d[i + 1] * d[i + 1] - d[i] * d[i])) / 2;
}
//初始化A矩阵
for (i = 0;i < 3;i++)
{
for (j = 0;j < 3;j++)
{
A[i][j] = p[i + 1][j] - p[i][j];
}
}
//计算未知点坐标
if (solve(A[0],B,point) < 0)
{
return -1;
}
return 0;
}
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>Widget</class>
<widget class="QWidget" name="Widget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>561</width>
<height>372</height>
</rect>
</property>
<property name="windowTitle">
<string>Widget</string>
</property>
<widget class="QPushButton" name="pushButton">
<property name="geometry">
<rect>
<x>60</x>
<y>270</y>
<width>75</width>
<height>23</height>
</rect>
</property>
<property name="text">
<string>计算</string>
</property>
</widget>
<widget class="QLineEdit" name="p1x">
<property name="geometry">
<rect>
<x>60</x>
<y>70</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="d1">
<property name="geometry">
<rect>
<x>360</x>
<y>70</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>210</x>
<y>20</y>
<width>141</width>
<height>21</height>
</rect>
</property>
<property name="font">
<font>
<pointsize>16</pointsize>
</font>
</property>
<property name="text">
<string>空间4点定位</string>
</property>
</widget>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>10</x>
<y>70</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第1点:</string>
</property>
</widget>
<widget class="QLabel" name="label_3">
<property name="geometry">
<rect>
<x>300</x>
<y>70</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLabel" name="label_4">
<property name="geometry">
<rect>
<x>10</x>
<y>110</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第2点:</string>
</property>
</widget>
<widget class="QLabel" name="label_5">
<property name="geometry">
<rect>
<x>300</x>
<y>110</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLabel" name="label_6">
<property name="geometry">
<rect>
<x>10</x>
<y>150</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第3点:</string>
</property>
</widget>
<widget class="QLabel" name="label_7">
<property name="geometry">
<rect>
<x>300</x>
<y>150</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLabel" name="label_8">
<property name="geometry">
<rect>
<x>10</x>
<y>190</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>第4点:</string>
</property>
</widget>
<widget class="QLabel" name="label_9">
<property name="geometry">
<rect>
<x>300</x>
<y>190</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>距离:</string>
</property>
</widget>
<widget class="QLineEdit" name="p2x">
<property name="geometry">
<rect>
<x>60</x>
<y>110</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="d2">
<property name="geometry">
<rect>
<x>360</x>
<y>110</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="p3x">
<property name="geometry">
<rect>
<x>60</x>
<y>150</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="d3">
<property name="geometry">
<rect>
<x>360</x>
<y>150</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="p4x">
<property name="geometry">
<rect>
<x>60</x>
<y>190</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="d4">
<property name="geometry">
<rect>
<x>360</x>
<y>190</y>
<width>113</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLabel" name="label_10">
<property name="geometry">
<rect>
<x>310</x>
<y>250</y>
<width>48</width>
<height>12</height>
</rect>
</property>
<property name="text">
<string>定位:</string>
</property>
</widget>
<widget class="QLineEdit" name="outx">
<property name="geometry">
<rect>
<x>350</x>
<y>270</y>
<width>113</width>
<height>20</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="outy">
<property name="geometry">
<rect>
<x>350</x>
<y>290</y>
<width>113</width>
<height>20</height>
</rect>
</property>
</widget>
<widget class="QLineEdit" name="outz">
<property name="geometry">
<rect>
<x>350</x>
<y>310</y>
<width>113</width>
<height>20</height>
</rect>
</property>
</widget>
<widget class="QLabel" name="label_11">
<property name="geometry">
<rect>
<x>320</x>
<y>270</y>
<width>21</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>x:</string>
</property>
</widget>
<widget class="QLabel" name="label_12">
<property name="geometry">
<rect>
<x>320</x>
<y>290</y>
<width>21</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>y:</string>
</property>
</widget>
<widget class="QLabel" name="label_13">
<property name="geometry">
<rect>
<x>320</x>
<y>310</y>
<width>21</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>z:</string>
</property>
</widget>
<widget class="QLineEdit" name="p1y">
<property name="geometry">
<rect>
<x>100</x>
<y>70</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p1z">
<property name="geometry">
<rect>
<x>140</x>
<y>70</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="p2y">
<property name="geometry">
<rect>
<x>100</x>
<y>110</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>-1</string>
</property>
</widget>
<widget class="QLineEdit" name="p2z">
<property name="geometry">
<rect>
<x>140</x>
<y>110</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p3y">
<property name="geometry">
<rect>
<x>100</x>
<y>150</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>1</string>
</property>
</widget>
<widget class="QLineEdit" name="p3z">
<property name="geometry">
<rect>
<x>140</x>
<y>150</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p4y">
<property name="geometry">
<rect>
<x>100</x>
<y>190</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
<widget class="QLineEdit" name="p4z">
<property name="geometry">
<rect>
<x>140</x>
<y>190</y>
<width>41</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources/>
<connections/>
</ui>
widget.h:
#ifndef WIDGET_H
#define WIDGET_H
#include <QWidget>
#include "public.h"
#include "_four_point_locate.h"
namespace Ui {
class Widget;
}
class Widget : public QWidget
{
Q_OBJECT
public:
explicit Widget(QWidget *parent = 0);
~Widget();
private:
Ui::Widget *ui;
_Four_Point_Locate four_point_locate;
private slots:
void on_pushButton_clicked();
};
#endif // WIDGET_H
widget.cpp:
#include "widget.h"
#include "ui_widget.h"
Widget::Widget(QWidget *parent) :
QWidget(parent),
ui(new Ui::Widget)
{
ui->setupUi(this);
}
Widget::~Widget()
{
delete ui;
}
void Widget::on_pushButton_clicked()
{
float point[3];
bool ok;
int flag = 0;
//获得坐标
point[0] = ui->p1x->text().toFloat(&ok);
point[1] = ui->p1y->text().toFloat(&ok);
point[2] = ui->p1z->text().toFloat(&ok);
four_point_locate.set_point(point,1);
point[0] = ui->p2x->text().toFloat(&ok);
point[1] = ui->p2y->text().toFloat(&ok);
point[2] = ui->p2z->text().toFloat(&ok);
four_point_locate.set_point(point,2);
point[0] = ui->p3x->text().toFloat(&ok);
point[1] = ui->p3y->text().toFloat(&ok);
point[2] = ui->p3z->text().toFloat(&ok);
four_point_locate.set_point(point,3);
point[0] = ui->p4x->text().toFloat(&ok);
point[1] = ui->p4y->text().toFloat(&ok);
point[2] = ui->p4z->text().toFloat(&ok);
four_point_locate.set_point(point,4);
//distance
four_point_locate.set_distance(ui->d1->text().toFloat(&ok),1);
four_point_locate.set_distance(ui->d2->text().toFloat(&ok),2);
four_point_locate.set_distance(ui->d3->text().toFloat(&ok),3);
four_point_locate.set_distance(ui->d4->text().toFloat(&ok),4);
//calc
flag = four_point_locate.calc(point);
if (flag < 0)
{
ui->outx->setText("fail");
ui->outy->setText("fail");
ui->outz->setText("fail");
}
else
{
ui->outx->setText(QString::number(point[0]));
ui->outy->setText(QString::number(point[1]));
ui->outz->setText(QString::number(point[2]));
}
}