仿GPS的4星定位程序

1万 · 2018-10-11 13:10

环境：

主机: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,则有解.然后用克莱姆法则求解出未知点坐标.

下面是测试程序.

---------------------作者：jdh99 来源：CSDN 原文：https://blog.csdn.net/jdh99/article/details/7349771?utm_source=copy

1万 · 2018-10-11 13:10

界面:

1万 · 2018-10-11 13:11

源程序:

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

1万 · 2018-10-11 13:11

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[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;
}

1万 · 2018-10-11 13:12

_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

1万 · 2018-10-11 13:40

_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 = (d_p_square(p[i + 1]) - d_p_square(p) - (d[i + 1] * d[i + 1] - d * d)) / 2;
      }

      //初始化A矩阵
      for (i = 0;i < 3;i++)
      {
            for (j = 0;j < 3;j++)
            {
                     A[j] = p[i + 1][j] - p[j];
            }
      }

      //计算未知点坐标
      if (solve(A[0],B,point) < 0)
      {
         return -1;
      }

      return 0;
}

1万 · 2018-10-11 13:40

widget.ui:(界面)

<?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>

1万 · 2018-10-11 13:41

  </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>

1万 · 2018-10-11 13:41

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

1万 · 2018-10-11 13:42

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]));
}
}

仿GPS的4星定位程序

涓涓之细流