`
ydbc
  • 浏览: 766728 次
  • 性别: Icon_minigender_1
  • 来自: 大连
文章分类
社区版块
存档分类
最新评论

仿GPS的4星定位程序

 
阅读更多

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


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



分享到：
| 利用subview 大法 改变UIPageControl的颜 ...
评论
发表评论

您还没有登录,请您登录后再发表评论

相关推荐

    matlab_gps 定位算法仿真程序 导航定位解算原理仿真

    在IT领域，GPS（全球定位系统）定位算法的仿真与导航解算是至关重要的技术，尤其在自动驾驶、航空航天、地理信息系统（GIS）等应用中。MATLAB作为一种强大的数值计算和仿真平台，是进行这类算法开发的理想工具。下面...

    XUANX.rar_GPS_gps选星_可见星_选星_选星 MATLAB

    在GPS接收机的设计和优化过程中，"选星"是一个关键步骤，它涉及到如何从众多可见卫星中选择出信号质量最好、最稳定的四颗来定位。本程序“XUANX.rar”就是一个针对这一过程的MATLAB模拟工具，旨在帮助理解并优化GPS...

    Matlab GPS精度因子分析，4颗卫星，有画出三维图(GDOP PDOP VDOP TDOP)

    在GPS（全球定位系统）中，精度因子是一个关键概念，用于评估定位的精度。本项目通过Matlab进行GPS精度因子的分析，特别是关注了GDOP（几何精度因子）、PDOP（位置精度因子）、VDOP（垂直精度因子）和TDOP（时间精度...

    提高卫星导航定位精度和实时性的选星算法

    为了确保定位精度，通常需要从多个可见卫星中选择几何分布最佳的卫星组合（至少四颗卫星），这就是所谓的“选星”问题。传统选星算法包括最佳几何精度因子法、最大矢端四面体体积法、最大正交投影法、综合法等，这些...

    本人做的产生GPS32颗星的CA码,共包含2个文件

    全球定位系统（GPS）是现代导航和定位技术中不可或缺的一部分，它通过轨道上的卫星向地面发送信号，使用户能够准确地确定自己的地理位置。为了实现这一功能，每颗GPS卫星都会发射一种特殊的二进制序列，称为CA码...

    GPS卫星自主定轨

    首先，GPS（全球定位系统）是一个由美国建立并维护的全球卫星导航系统，通过多颗卫星向地面用户提供精确的时间和位置信息。自主定轨是指利用GPS卫星的观测数据，通过计算确定或更新卫星的精确轨道位置的过程。这对于...

    O2O订餐系统仿饿了么网站源码+支付宝接口+手机站

    4. 位置服务：集成GPS定位，方便用户查找附近餐厅，提供送餐范围查询。 四、商家打印订单功能 商家打印订单是订餐系统与线下实体店交互的关键环节。该功能一般通过API与打印机硬件设备进行通信，实现订单信息的自动...

    基于GPS的低轨卫星简化解析法轨道预报.pdf

    在实际应用中，星载GPS接收机的定位板输出卫星的位置、速度等信息，轨道板软件则负责接收这些数据，进行轨道改进和预报。由于定位板可能出现的不稳定、电离层闪烁或GPS信号失锁等问题，轨道板需要具备在数据中断时...

    naviga090205.rar_捷联惯导_组合导航算法_车载导航_车载惯导_车载组合导航

    组合导航则是将多种导航技术（如GPS、磁罗盘、星敏感器等）融合，以提高整体导航性能。车载组合导航系统通常结合了捷联惯导与全球定位系统（GPS），两者互补，既能发挥GPS的高精度，又能利用SINS的连续性。当GPS信号...

    Android最新资料以及源码

    自绘视图View实现gps绘画、监听gps状态、监听gps位置变化、以及搜星。 3、Android 程序锁源码 Android 程序锁，共六个目标文件，锁屏用的程序。 4、Android 手机屏幕保护源码 共3个目标文件（后台服务实现），...

    vc++ 应用源码包_1

    主要文件就4个，非常适合学习，详细见源码。 OPENG开发的示例代码c++版 演示了OpenG的使用方法，内含几个实例，一个实例就3个文件。 p2p vb实例。 p2p+technology文档。 P2P视频技术源码（含开发文档） PcShare...

    行业分类-设备装置-一种低速接近航天器轨迹安全评价方法.zip

    先进的导航技术如星敏感器、激光雷达和全球定位系统（GPS）在低速接近中起着关键作用。 5. 接近策略：低速接近策略通常包括多个阶段，如初始接近、捕获、停泊和最终对接。每个阶段都有特定的安全阈值和操作规程，以...

    vc++ 应用源码包_2

    主要文件就4个，非常适合学习，详细见源码。 OPENG开发的示例代码c++版 演示了OpenG的使用方法，内含几个实例，一个实例就3个文件。 p2p vb实例。 p2p+technology文档。 P2P视频技术源码（含开发文档） PcShare...

    vc++ 应用源码包_6

    主要文件就4个，非常适合学习，详细见源码。 OPENG开发的示例代码c++版 演示了OpenG的使用方法，内含几个实例，一个实例就3个文件。 p2p vb实例。 p2p+technology文档。 P2P视频技术源码（含开发文档） PcShare...

    vc++ 应用源码包_5

    主要文件就4个，非常适合学习，详细见源码。 OPENG开发的示例代码c++版 演示了OpenG的使用方法，内含几个实例，一个实例就3个文件。 p2p vb实例。 p2p+technology文档。 P2P视频技术源码（含开发文档） PcShare...

    vc++ 应用源码包_3

    主要文件就4个，非常适合学习，详细见源码。 OPENG开发的示例代码c++版 演示了OpenG的使用方法，内含几个实例，一个实例就3个文件。 p2p vb实例。 p2p+technology文档。 P2P视频技术源码（含开发文档） PcShare...

Magicbox
Global site tag (gtag.js) - Google Analytics