[应用相关]

仿GPS的4星定位程序

[复制链接]
453|9
手机看帖
扫描二维码
随时随地手机跟帖
guanjiaer|  楼主 | 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
guanjiaer|  楼主 | 2018-10-11 13:10 | 显示全部楼层

界面:

0_133163135421n2.jpg


使用特权

评论回复
guanjiaer|  楼主 | 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


使用特权

评论回复
guanjiaer|  楼主 | 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;
}


使用特权

评论回复
guanjiaer|  楼主 | 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


使用特权

评论回复
guanjiaer|  楼主 | 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;
}


使用特权

评论回复
guanjiaer|  楼主 | 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>



使用特权

评论回复
guanjiaer|  楼主 | 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>

使用特权

评论回复
guanjiaer|  楼主 | 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


使用特权

评论回复
guanjiaer|  楼主 | 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]));
    }
}


使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

72

主题

3836

帖子

2

粉丝