1 准备知识
rtklib中相对定位部分使用扩展卡尔曼滤波实现。所以,要真正搞懂rtklib中载波相位差分定位的部分,最好先看一下kalman滤波的知识(当然点开这篇文章,想必对GNSS领域的domain knowledge是已经很熟悉的了_)。不需要很精通,以笔者10几年断断续续卡尔曼滤波相关的工作经验来看,我觉得卡尔曼滤波最好能理解以下内容
:
- 会应用卡尔曼滤波五公式解决基本问题
- 感性的认识到卡尔曼滤波的本质其实是模型与量测的加权
- 能知道什么是时间更新,什么是量测更新
- 给定状态与量测能自己建立模型,即求
A阵和
C阵(有的书上叫
F阵和
H 阵)
关于最后这一点,其实每个行业或者业务领域需要的domain knowledge千差万别,有些很简单比如估计个温度,电量等等,稍微复杂一点的比如本篇文章中的ekf,当然这个复杂是相对的,其与组合导航领域的数据融合来说真是小巫见大巫了。
2 状态向量与观测向量的选取
卡尔曼滤波中第一步也是最重要的便是选择状态向量和观测向量。只要这两组向量确定出来了,那么卡尔曼滤波模型就“基本”确定了。
2.1 状态向量
量测向量
x的定义如下:
x=[rr,vr,B1,B2,B5]
其中,
rr,vr分别为三维位置向量和速度向量。
Bi是m维单差模糊度,m是观测到的卫星个数。即:
Bi=[Brb,i1,Brb,i2,...,Brb,im]
使用单差模糊度是为了规避历元间参考卫星可能改变的问题。
2.2 量测向量
量测向量
y的定义如下:
y=[ϕ1,ϕ2,ϕ5,P1,P2,P5]
其中,
ϕi,Pi分别为双差载波相位和双差伪距。
3 定义卡尔曼滤波模型
3.1 时间更新模型
以下为运动学方程,比较简单,就是位置是上一时刻的位置加速度乘以时间间隔,速度认为不变,单差模糊度恒定不变。
rr(k+1)=rr(k)+vr(k)∗dt
vr(k+1)=vr(k)
Bi(k+1)=Bi(k)
其中
dt为历元间隔,通过以上关系我们很容易得到状态转移矩阵
A
A=⎣⎡I3,303,303m,3I3,3∗dtI3,303m,303,3m03,3mI3m,3m⎦⎤
从上面的结果可以看出A是一个常值矩阵,这个是很不错的,这不仅省去了线性化的工作。更有意思的是如果量测矩阵也是一个常值矩阵,那么这个系统就是一个线性定常系统,对于线性定常系统卡尔曼滤波的增益矩阵(通常记为
K阵)无需在线计算,可以提前计算出来,系统中直接应用即可。
线性定常系统的
K阵最后收敛为一个常值矩阵,不熟悉卡尔曼滤波的可以通过卡尔曼滤波五公式自己体会。
3.2 量测更新模型
量测模型略复杂,从下边的式子可以容易看出是非线性的,所以系统并不是线性定常系统。
ϕrbjk=ρrbjk+λ(Brbj−Brbk)
Prbjk=ρrbjk
若记
y=h(x),则我们需要求解h的雅克比矩阵。
观测模型可以通过matlab或者python的sympy推导出来,以下以四颗卫星为例给出结果。
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2
rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2
rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2
rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2
rz−zs4000000000000000000000000000000000000000000000000000000l1l1l1000000000000000−l1000000000000000000−l1000000000000000000−l1000000000000000000l2l2l2000000000000000−l2000000000000000000−l2000000000000000000−l2000000000000000000l5l5l5000000000000000−l5000000000000000000−l5000000000000000000−l5000000000⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
可以抽象成以下形式,
C=
⎣⎢⎢⎢⎢⎢⎢⎡−DE−DE−DE−DE−DE−DE000000λ1D000000λ2D000000λ5D000⎦⎥⎥⎥⎥⎥⎥⎤
其中,
D=
⎣⎢⎢⎡11...1−10...00−1...0............00...−1⎦⎥⎥⎤
4 小结
到此,想必您对rtkpos的关键算法已经“很”了解了,当然其中还有很多细节,比如共视星选取,周跳探测,不同动态模型下的F阵或者A阵选取,Q阵和R阵的确定,整周模糊度的确定算法等,这些细节将会接下来结合代码详细讲解。