程序员经验分享-hwhale

GPS定位算法（牛顿迭代+最小二乘

2023-05-16

第一步：准备数据与设置初始解

对于所有各颗可见卫星n，收集到它们在同一时刻的伪距测量值\rho ^{(n)}，计算测量值中的各项偏差、误差成分的矫正量\delta t^{(n)}、I^{(n)}（电离层延时）和T^{(n)}（对流层延时），然后计算误差校正后的伪距测量值

\rho_{c}^{n}=\rho ^n-\delta t^n-I^n-T^n

同时，对于所有各颗可见卫星n，根据它们分星历计算出经过地球自转校正后的卫星空间位置坐标\left ( x^{(n)},y^{(n)},z^{(n)} \right )。

在开始进行牛顿迭代之前，我们还需要给出接收机当前位置坐标的初始估计值x_{0}=[x_0,y_0,z_0]^T与接收机钟差初始估计值\delta t_{u,0}。

假如接收机在上一个定位时刻已经成功地解得定位结果，那么这次定位的初始估计值就自然可以采用上一个定位结果，包括接收机位置坐标与钟差值。如果我们还知道用户在上一个定位时刻的运动速度，那么据此推算出接收机当前位置的估计值也未尝不可。类似的，如果我们知道接收机适中的频漂，我们可以对钟差初始值进行类似的推算。这种情况对于接收机来说不仅最经常发生，而且通常也是最简单、最理想的一种。

然而，如果接收机在此前的近期一段时间内尚未实现定位，那么此刻对接收机来说是首次定位。对于首次定位，钟差初始值\delta t_{u,0}一般可以设置为0，而接收机坐标初始值x_{0}=[x_0,y_0,z_0]^T的估算问题可分为以下几种情况。

（1）接收机一般允许用户输入其所在的位置和时间。如果用户确信其输入的位置坐标是大致正确的，那么该输入值就可以作为接收机位置坐标的初始估计值。这种外界输入值的含义其实是相当广泛的，它还可以是在接收机启动前就存留在接收机记忆单元中的定位值，也可以是由无线通信网络提供的辅助信息。

（2）接收机此时应该已经对多颗可见卫星进行了跟踪、测量，并计算了它们的空间位置。这样，我们可以计算出所有这些可见卫星位置坐标的平均值，然后将此平均值在地面的投影作为接收机位置坐标的初始估计值。

（3）接收机初始位置的各个坐标分量也可全部简单地设置为零。即便从零出发牛顿迭代法一般也只需几次迭代循环就可以得到收敛。

第二步：非线性方程组线性化

我们用k代表当前历元正在进行的牛顿迭代次数，即k-1是在当前历元已经完成的迭代次数，而k=1代表第一次迭代。

在当前历元的第k次牛顿迭代中，方程组

\sqrt{(x^{(1)}-x)^2+(y^{(1)}-y)^2+(z^{(1)}-z)^2}+\delta t_u=\rho _{c}^{(1)}

\sqrt{(x^{(1)}-x)^2+(y^{(1)}-y)^2+(z^{(1)}-z)^2}+\delta t_u=\rho _{c}^{(2)}

\sqrt{(x^{(1)}-x)^2+(y^{(1)}-y)^2+(z^{(1)}-z)^2}+\delta t_u=\rho _{c}^{(N)}

式1 

的各个非线性方程可在[x_{k-1},\delta t_{u,k-1}]^T处线性化。以方程组中的第n个方程式为例，该方程式等号左边的第一项正是非线性函数r^{(n)}，而求函数r^{(n)}对x的偏导，得

\frac{\partial r^{(n)}}{\partial x} = \frac{-(x^{(n)}-x)}{\sqrt{(x^{(1)}-x)^2+(y^{(1)}-y)^2+(z^{(1)}-z)^2}}=\frac{-(x^{(n)}-x)}{r^{(n)}}

如图所示，

 图1 接收机指向卫星的观测矢量

r^{(n)}是卫星n在用户接收机处得观测矢量的长度，(x^{(n)}-x)是此观测矢量的X分量，于是\frac{-(x^{(n)}-x)}{r^{(n)}}就等于单位观测矢量的X分量。

类似地，可以求出函数r^{(n)}分别对y和z的偏导值，它们分别等于单位观测矢量的Y和Z分量的反向，即

 式2

式1中左侧已经是一个关于\delta t_{u}的线性函数，或者说各方程式等号左边对\delta t_{u}的偏导值为1.这样可以得到式1在\left [ x_{k-1},\delta t_{u,k-1} \right ]^{T}处线性化后的矩阵方程式

式3

其中

 

式4

而-I_{x}^{(n)}(x_{(k-1)}) 代表r^{(n)}对x的偏导在x_{k-1}处的值，即

式5

 由G可见，雅可比矩阵G只是与各颗卫星相对于用户的几何位置有关，因而G通常被称为几何矩阵。

为了简化讨论，假设接收机时钟与GPS时间同步，而且该方程组的牛顿迭代法秩序迭代一次就得到收敛。考虑式3中的每一个方程式，其等号左边是单位观测矢量的反向与差值的内积，其中\left ( \Delta x,\Delta y,\Delta z \right )是用户在相邻两个观测时刻之间从A点运动到B点的坐标变化量。因为与接收机到卫星n的距离r^{(n)}相比，用户位移量\left ( \Delta x,\Delta y,\Delta z \right )

与单位观测矢量反向的内积等于此位移在观测矢量反向上的投影。因此式3中的每一个方程式所代表的含义为：用户位移量在卫星观测反方向上的投影，等于由此位移引起的卫星与用户之间的距离变化量。伪距定位的过程，实际上是依据接收机到各个卫星的距离变化量来反推用户的运动位移矢量。 

图2 用户位移在卫星观测方向上的投影

 式3中的每个方程式关于接收机钟差的系数均为1，这是因为接收机钟差式各个卫星距离测量值中的公共偏差部分，它在定位计算中实际上是吸收向量b中各元素的平均值。

第3步：求解线性方程组

这一步的任务是利用最小二乘法求解GPS伪距定位线性矩阵方程式。套用最小二乘的求解公式，可得到最小二乘解为

 如果各个卫星测量值的方差以及权重已被确定，那么也可以运用加权最小二乘法来求解。为了提高定位精度，一部分GPS接收机事实上采用加权最小二乘法作为GPS定位方程的求解算法。

第四步：更新非线性方程组的根

更新后的接收机坐标x_{k}和钟差值\delta t_{u,k}为：

 第五步：判断牛顿迭代的收敛性

若牛顿迭代已经收敛到了所需要的精度，则牛顿迭代法可终止循环运算，并将当前这一次的迭代计算后的更新值作为接收机此时可的定位、定时结果；否则，k值增1，并返回至第二步再重复进行一次牛顿迭代计算。判断牛顿迭代是否收敛，一般可检查此次迭代计算得到的位移向量\Delta x的长度\left \| \Delta x \right \|或者的值是否已经小到一个预先设定的门限值。

牛顿迭代法有时会收敛到一个远离地球的天际一端。如果以上伪距定位计算给出的解与常识明显不符，那么GPS定位算法必须返回第一步，并恰当地调整接收机位置初始估计值，然后重新进行一次完成的牛顿迭代运算，使最后收敛至一个在地球表面附近的合理值为止。

 

 

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

GPS定位算法（牛顿迭代+最小二乘 的相关文章

  • J-001 Jetson硬件电路设计概论--NANO & XAVIER NX

    Jetson硬件设计概论 1 Jetson板卡简介2 Jetson产品组合3 模组对比4 电路分析4 1 电源设计4 1 1 原理图设计4 1 2 引脚描述4 1 3 电源框图4 1 4 上电时序 4 2 USB设计4 2 1 USB 2

  • J-002 Jetson电路设计之电源设计--NANO && XAVIER NX

    Jetson电源设计 1 电源说明1 1 电源和系统引脚描述1 2 电源控制框图详情 2 上电的时许2 1 框图分析2 2 上电时序 3 GND引脚 1 电源说明 Jetson NANO和XAVIER NX核心板的电源为DC 5V 1 1

  • J-004 Jetson电路设计之HDMI设计--NANO && XAVIER NX

    HDMI电路设计 1 简介2 框图介绍3 原理图介绍 1 简介 NANO amp XAVIER NX提供一路HDMI接口 xff0c DP接口与HDMI是兼容的 xff0c 可用于扩展一路HDMI 其中引脚说明 PIN名称描述方向类型63H

  • 树莓派入坑第一天——系统烧录以及SSH登录问题

    1 首先下载镜像https www raspberrypi org downloads xff0c Raspbian系统是树莓派官方推荐的系统 xff0c 解压出img镜像文件 注意树莓派3B可能不支持老版本镜像 2 下载一个格式化SD卡的

  • c++学习之路

    3 19 内存分区模型 内存四区意义 不同区域存放的数据 xff0c 赋予不同的生命周期 给我们更大的灵活编程 程序exe 运行前分为 代码区和全局区 xff1b 运行后分为 栈区和堆区 1 代码区 存放CPU执行的机器指令 存放函数体的二

  • 写程序的步骤

    xff08 1 xff09 xff1a 一 要把实际问题提取为数学问题 相当于数学中的建模 抽象问题具体化 二 把其分解为若干个小的函数 并明白每个小函数怎样实现其功能 同时注意功能函数与主函数间的数据交互问题 三 作出 流程图 xff0c

  • Conda_安装库失败：Collecting package metadata (current_repodata.json): failed

    具体原因 xff1a update repo信息时网络有问题 于是就出现了污染 解决方法 xff1a conda clean i 然后重新随便install一个库 会重新下载repo信息 xff0c 问题解决

  • scrapy爬虫实战——抓取NBA吧的内容

    scrapy爬虫 步骤1 进入虚拟环境2 测试爬取页面3 进入开发者模式4 剥离页面中的数据5 在pycharm中码代码scrapy框架的目录 xff08 之前创建虚拟环境自动搭建 xff09 nba py源码详解 6 Debug第一步 点

  • ubuntu系统版本查询命令方法

    目录 一 使用命令 xff1a cat proc version 查看 二 使用命令 xff1a uname a 查看 三 使用命令 xff1a lsb release a 查看 四 使用命令 xff1a hostnamectl 查看 五

  • python解析xml文件（解析、更新、写入）

    Overview 这篇博客内容将包括对XML文件的解析 追加新元素后写入到XML xff0c 以及更新原XML文件中某结点的值 使用的是python的xml dom minidom包 xff0c 详情可见其官方文档 xff1a xml do

  • 一阶微分方程

    传送门https jingyan baidu com article 8065f87fb7f0652331249822 html 1 可分离变量的微分方程解法 一般形式 g y dy 61 f x dx 直接解得 g y dy 61 f x

  • C#多线程--信号量(Semaphore)

    Semaphore 是负责协调各个线程 以保证它们能够正确 合理的使用公共资源 也是操作系统中用于控制进程同步互斥的量 Semaphore常用的方法有两个WaitOne 和Release xff0c Release 的作用是退出信号量并返回

  • 【总结】C++工程师学习路线|推荐视频|推荐书籍

    前言 由于博主秋招拿到的offer有限 xff0c 经过对比 xff0c 决定转到C 43 43 开发技术栈 xff0c 此篇文章用于规划自己今后的成长路线并分享给大家 学习路线 C 43 43 语言本身 xff1a 我们可以将这个部分分为

  • 面试被问到的promise总结

    promise all的使用 promise all可以将多个promise实例包装成一个新的promise实例 xff0c 并且返回的值也不相同 xff0c 成功使 xff0c promise返回的值是一个结果数组 xff0c 而失败的话

  • ROS节点，消息，话题，服务的介绍

    整理结合机器人操作系统 xff08 ros xff09 浅析和网址http wiki ros org cn NODE node几乎是无处不在 xff0c 这个东西相当于可执行文件 xff0c 目前我更愿意把它当做cpp文件 xff0c 通过

  • vue实现表格的更多查询功能

    场景一 xff1a 一行足够显示完所有的查询条件 场景二 xff1a 需要多行才能显示完所有的查询条件 1 首先创建一个按钮组件SearchButton lt template gt lt el form inline class 61 3

  • FreeROTS原理学习笔记

    前言 xff1a 这仅是一篇学习笔记记录 xff0c 无指导意义 想详细了解的人 可看CSDN博主 zhzht19861011 的原创文章 FreeROTS系统 xff1a 使用习惯 xff1a 1 一般来说 xff0c 都是利用下载好的例

  • RuntimeError: dataset.make_initializable_iterator is not supported when eager execution is enabled.

    这是由于代码的接口更改 xff0c 无法正常连接数据集 xff0c 即新版本接口变了 需要按照第4章的数据集部分 xff0c 改一下数据集接口

  • 基于Android 的串口工具类

    欢迎使用串口通讯 xff0c 首先说明下我这里使用的是RS485通讯 xff0c 采用的是半双工通讯 xff0c 所以收和发不能同时操作需要发送等待一段时间来接收完数据在发送其他指令了 xff0c 这里顺便在说下RS232 xff0c 它采

  • ROS安装步骤

    ROS xff08 Robot Operating System xff09 起源于2007年斯坦福大学人工智能实验室与WillowGarage公司的个人机器人项目 xff0c 其后被Willow Garage公司开源和发展 xff0c 目

随机推荐

热门标签