用户到卫星的距离可以先简单理解成通过光速×时间差得到
而用户测得的是包含各种误差影响在内的距离, 称之为伪距。
这是伪距
是用户机钟差,加上用户三维坐标,共四个未知量,所以需要至少同时看到四颗卫星才能实现定位。
设用户坐标是(X,Y,Z),三颗卫星位置根据星历可得是已知量,设为(Xi,Yi,Zi)
则用户对第i个卫星的伪距为:
后面的一串是各种误差,比如电离层影响等等,这些都根据模型可提前求得,算作已知量。
写完整:
提前写明,最终求解这个四元方程的方法是 迭代法
因此对于(X,Y,Z)会需要一个初值,可以理解为近似值,但实际你随便取无影响,只会对迭代系数有一点点的影响。
我们把这个初值定为(X0,Y0,Z0),令
然后将一开始伪距的表达式泰勒一阶展开,这里是三元的泰勒展开,anyway只有套用三元的一阶展开公式就行
三元函数一阶泰勒展开公式
得到(5)(左图ΔX=X-X0带入了)
后引入这个ρ01,类似到ρ04,带入(5)并求偏导
可得 ,再带入(5),(5)就可用偏导表示
完整版
记,记
(6)写成矩阵表示:,因此
最终可解得ΔX,ΔY,ΔZ,CΔtu。通过更新用户坐标
如果超过四颗卫星的信息,前面都一样,相当于方程数多于未知量个数,此时只需要在最后加上最小二乘法的处理就可以得到最优解了。
考虑到涉及到的矩阵运算比较多,所以选用MATLAB来计算