觀測模型
偽距觀測方程
偽距觀測值代表衛星Satellite和接收機Receiver之間粗略的距離信息由\(P^s_{r,k}\),其中S代表衛星，r代表接收機，k代碼第k顆衛星。它由用戶接收到信號的時間\(t_r(t)\)和衛星發射信號的時間\(t^s(t-\tau^s_r)\)相減再乘以光速得到的，\(\tau^s_r\)為信號傳播時間，公式為
\[P^s_r=c*[t_r(t)-t^s(t-\tau^s_r)]+e^s_r(t) \]
\(e^s_r(t)\)：偽距觀測噪聲
考慮到接收機時間誤差 \(t_r(t)=t+\Delta t_r\)
考慮到衛星鍾誤差 \(t^s(t-\tau^s_r)=t-\tau^s_r+\Delta t^s\)
信號傳播時間 \(\tau^s_r=\tau^s_{r,real}+dcb_r+dcb^s=\frac{1}{c}*(\rho^s_r+I^s_r+T^s_r+dm^s_r)+dcb_r+dcb^s\)
綜合上述得到偽距觀測方程
| \(P^s_r=\rho^s_r+c\Delta t_r-c\Delta t^s+c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t)\) |
| ######################################################################################## |
| \(P^s_r\)：偽距觀測值，已知變量，可通過接收機測量獲得 |
| \(\rho^s_r\)：衛地距，實際衛星和接收機的距離 |
| \(\Delta t_r\)：接收機鍾差，\(c\Delta t_r=\Delta T_r\) |
| \(\Delta t^s\)：衛星鍾差, \(c\Delta t^s=\Delta T^s\) |
| \(dcb_r\)：接收機硬件延遲, \(c*dcb_r=Dcb_r\) |
| \(dcb^s\)：衛星硬件延遲，\(c*dcb^s=Dcb^s\) |
| \(I^s_r\)：電離層誤差 |
| \(T^s_r\)：對流層誤差 |
| \(dm^s_r\)：多路徑誤差 |
| \(e^s_r(t)\)：偽距觀測噪聲 |
多普勒觀測方程
對偽距觀測方程進行時間求導有
\[{P^s_r}'={\rho^s_r}'+{c\Delta t_r}'-{c\Delta t^s}'+(c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t))' \]
由於電離層誤差，對流層誤差和硬件延遲的時間導數很小可忽略不計，統一在測量誤差項中，化簡有多普勒觀測方程
| \(D^s_r={\rho^s_r}'+{c\Delta t_r}'-{c\Delta t^s}'+{e^s_r(t)}'\) |
| ######################################################################################## |
| \(D^s_r={P^s_r}'\): 多普勒測量值，已知變量，可通過基帶多普勒頻移測量值獲得 |
| \({\Delta t_r}'\)：接收機鍾漂,\({c\Delta t_r}'={\Delta T_r}'\) |
| \({\Delta t^s}'\)：衛星鍾漂,\({c\Delta t^s}'={\Delta T^s}'\) |
| \({e^s_r(t)}'\)：多普勒觀測誤差 |
載波相位觀測方程
載波相位觀測值為衛星和接收機之間特定頻率的載波相位之差，實際中，信號接收機只能測量接收機和衛星的載波相位差的小數部分，整數部分無法確定。
\[{\varphi}^{s}_r(t)={\varphi}_r(t)-{\varphi}^s(t-\tau^s_r)+N^s_r+e^s_r(t) \]
考慮到接收機初始相位\({\varphi}_r(0)\)，衛星初始相位\({\varphi}^s(0)\)和傳播時間
接收機信號相位\({\varphi}_r(t)=freq*(t+\Delta t_r)+{\varphi}_r(0)\)
衛星信號相位\({\varphi}^s(t-\tau^s_r)=freq*(t-\tau^s_r+\Delta t^s)+{\varphi}^s(0)\)
整理有
\[{\varphi}^{s}_r(t)=freq*(\tau^s_r+\Delta t_r-\Delta t^s)+[{\varphi}_r(0)-{\varphi}^s(0)]+N^s_r+e^s_r(t) \]
又有波長\(\lambda=c/freq\),方程兩邊同時乘以\(\lambda\)
\[\lambda{\varphi}^{s}_r(t)=c(\tau^s_r+\Delta t_r-\Delta t^s)+\lambda[{\varphi}_r(0)-{\varphi}^s(0)]+\lambda N^s_r+\lambda e^s_r(t) \]
\(\lambda{\varphi}^{s}_r(t)=L^{s}_r(t)\)為衛星和接收的距離，則得到載波相位觀測方程
| \(L^{s}_r(t)=\rho^s_r+c\Delta t_r-c\Delta t^s+c[dcb_r+dcb^s]+I^s_r+T^s_r+dm^s_r+\lambda[{\varphi}_r(0)-{\varphi}^s(0)]+\lambda N^s_r+e^s_r(t)\) |
| ######################################################################################## |
| \(L^{s}_r(t)\)：載波相位觀測值 |
| \(\rho^s_r\)：衛地距，實際衛星和接收機的距離 |
| \(\Delta t_r\)：接收機鍾差，\(c\Delta t_r=\Delta T_r\) |
| \(\Delta t^s\)：衛星鍾差, \(c\Delta t^s=\Delta T^s\) |
| \(dcb_r\)：接收機硬件延遲, \(c*dcb_r=Dcb_r\) |
| \(dcb^s\)：衛星硬件延遲，\(c*dcb^s=Dcb^s\) |
| \(I^s_r\)：電離層誤差 |
| \(T^s_r\)：對流層誤差 |
| \(dm^s_r\)：多路徑誤差 |
| \({\varphi}_r(0)\)：接收機初始相位 |
| \({\varphi}^s(0)\)：衛星初始相位 |
| \(N^s_r\)：整周模糊度 |
| \(e^s_r(t)\)：載波相位觀測誤差 |
偽距定位原理
根據偽距觀測方程 \(P^s_r=\rho^s_r+c\Delta t_r-c\Delta t^s+c[Dcb_r+Dcb^s]+I^s_r+T^s_r+dm^s_r+e^s_r(t)\)
忽略大氣層，多路徑，硬件延遲和測量誤差等因素，方程簡化為\(P^s_r=\rho^s_r+\Delta T_r\)
假設衛星k位置為\((x_k,y_k,z_k)\),接收機位置為\((x_r,y_r,z_r)\),則有
\[{P_k}^s_r=\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}+\Delta T_r=f_k(x_r,y_r,z_r,\Delta T_r) \]
當有N顆衛星時，則有偽距觀測量方程組
\[\left\{\begin{matrix} {P_1}^s_r \\ {P_2}^s_r \\ {P_3}^s_r \\ \vdots \\ {P_N}^s_r \end{matrix} \right\} = \left\{\begin{matrix} \sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}+\Delta T_r \\ \sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}+\Delta T_r \\ \sqrt{(x_3-x_r)^2+(y_3-y_r)^2+(z_3-z_r)^2}+\Delta T_r \\ \vdots \\ \sqrt{(x_N-x_r)^2+(y_N-y_r)^2+(z_N-z_r)^2}+\Delta T_r \end{matrix} \right\} = \left\{\begin{matrix} f_1(x_r,y_r,z_r,\Delta T_r)\\ f_2(x_r,y_r,z_r,\Delta T_r)\\ f_3(x_r,y_r,z_r,\Delta T_r)\\ \vdots \\ f_N(x_r,y_r,z_r,\Delta T_r) \end{matrix} \right\} \]
該非線性方程可用最小二乘進行求解
最小二乘求解偽距定位方程
偽距觀測量方程組方程組 \(b=A(X_r)\) 向量\(X_r=(x_r,y_r,z_r,\Delta T_r)\) 接收機坐標和接收機鍾差
\(b=({P_1}^s_r, {P_2}^s_r, {P_3}^s_r, \cdots {P_N}^s_r)^T\)
\(A(X_r) = \left\{\begin{matrix} \sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}+\Delta T_r \\ \sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}+\Delta T_r \\ \sqrt{(x_3-x_r)^2+(y_3-y_r)^2+(z_3-z_r)^2}+\Delta T_r \\ \vdots \\ \sqrt{(x_N-x_r)^2+(y_N-y_r)^2+(z_N-z_r)^2}+\Delta T_r \end{matrix} \right\}\)
根據如下非線性最小二乘方法求解方法，\(f(X_r)=A(X_r)-b\) 進行求解 \(min||f(X_r)||\)
\[\begin{cases} X_{k+1}=X_k+\Delta X \\ \Delta X=[J(X)^TJ(X)^{-1}]*J(X)^T*f(X) \end{cases}\]
其中 \(J(X)\)為Jacobian矩陣，當有n顆衛星時表示為
\[J(X_r)=\left\{\begin{matrix} \frac{\partial f_1(x_1)}{\partial x} & \frac{\partial f_1(x_2)}{\partial x} & \cdots & \frac{\partial f_1(x_m)}{\partial x} \\ \frac{\partial f_2(x_1)}{\partial x} & \frac{\partial f_2(x_2)}{\partial x} & \cdots & \frac{\partial f_2(x_m)}{\partial x} \\ \vdots\\ \frac{\partial f_n(x_1)}{\partial x} & \frac{\partial f_n(x_2)}{\partial x} & \cdots & \frac{\partial f_n(x_m)}{\partial x} \\ \end{matrix} \right\} = \left\{\begin{matrix} \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\partial x_r} & \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\partial y_r} & \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\partial z_r} & \frac{\partial f_1(x_r,y_r,z_r,\Delta T_r)}{\Delta T_r} \\ \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\partial x_r} & \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\partial y_r} & \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\partial z_r} & \frac{\partial f_2(x_r,y_r,z_r,\Delta T_r)}{\Delta T_r} \\ \vdots\\ \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\partial x_r} & \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\partial y_r} & \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\partial z_r} & \frac{\partial f_n(x_r,y_r,z_r,\Delta T_r)}{\Delta T_r} \\ \end{matrix} \right\} \]
\[J(x_r,y_r,z_r,\Delta T_r)=\left\{\begin{matrix} \frac{-(x_1-x_r)}{\sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}} & \frac{-(y_1-y_r)}{\sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}} & \frac{-(z_1-z_r)}{\sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}} & 1 \\ \frac{-(x_2-x_r)}{\sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}} & \frac{-(y_2-y_r)}{\sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}} & \frac{-(z_2-z_r)}{\sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}} & 1 \\ \vdots\\ \frac{-(x_n-x_r)}{\sqrt{(x_n-x_r)^2+(y_1-y_r)^2+(z_n-z_r)^2}} & \frac{-(y_n-y_r)}{\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}} & \frac{-(z_n-z_r)}{\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}} & 1 \end{matrix} \right\} \]
設\(R_n=\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}\) 則
\[J(x_r,y_r,z_r,\Delta T_r)= \left\{\begin{matrix} \frac{(x_r-x_1)}{R_1} & \frac{(y_r-y_1)}{R_1} & \frac{(z_r-z_1)}{R_1} & 1 \\ \frac{(x_r-x_2)}{R_2} & \frac{(y_r-y_2)}{R_2} & \frac{(z_r-z_2)}{R_2} & 1 \\ \vdots\\ \frac{(x_r-x_n)}{R_n} & \frac{(y_r-y_n)}{R_n} & \frac{(z_r-z_n)}{R_n} & 1 \end{matrix} \right\} \]
得到\(J(X_r)\)后 在計算增量\(\Delta x\)
\(\Delta X=[J(X_r)^TJ(X_r))^{-1}]*J(X_r))^T*f(X_r)\)
\(\Delta X=[J(X_r)^TJ(X_r))^{-1}]*J(X_r))^T*[A(X_r)-b]\)
值得指出的是 這里\(J(X_r)\)只和衛星和接收機位置相關，稱為幾何矩陣，\(A(X_r)-b\)為測量偽距和計算的衛地距之差，稱為偽距殘差
迭代方程，直到收斂為止
\(\Delta X=[J(X_r)^TJ(X_r)^{-1}]*J(X_r)^T \left\{\begin{matrix} \sqrt{(x_1-x_r)^2+(y_1-y_r)^2+(z_1-z_r)^2}+\Delta T_r-{P_1}^s_r\\ \sqrt{(x_2-x_r)^2+(y_2-y_r)^2+(z_2-z_r)^2}+\Delta T_r-{P_2}^s_r\\ \vdots \\ \sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}+\Delta T_r-{P_n}^s_r \end{matrix} \right\}\)
\(X_{k+1}=X_r+\Delta X\)
多普勒定速原理
有多普勒觀測方程 \(D^s_r={\rho^s_r}'+{c\Delta t_r}'-{c\Delta t^s}'+{e^s_r(t)}'\)
假設衛星k位置為\((x_k,y_k,z_k)\),接收機位置為\((x_r,y_r,z_r)\)
假設衛星k速度為\((\frac{\partial x_k}{\partial t},\frac{\partial y_k}{\partial t},\frac{\partial z_k}{\partial t})=(vx_k,vy_k,vz_k)\),接收機速度為\((\frac{\partial x_r}{\partial t},\frac{\partial y_r}{\partial t},\frac{\partial z_r}{\partial t})=(vx_r,vy_r,vz_r)\)
其中
\[{\rho^s_r}'=\frac{\partial \sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}{\partial t}\\ =\frac{x_k-x_r}{\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}*(\frac{\partial x_k}{\partial t} - \frac{\partial x_r}{\partial t}) + \\ \frac{y_k-y_r}{\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}*(\frac{\partial y_k}{\partial t} - \frac{\partial y_r}{\partial t}) + \\ \frac{z_k-z_r}{\sqrt{(x_k-x_r)^2+(y_k-y_r)^2+(z_k-z_r)^2}}*(\frac{\partial z_k}{\partial t} - \frac{\partial z_r}{\partial t}) \]
設\(R_n=\sqrt{(x_n-x_r)^2+(y_n-y_r)^2+(z_n-z_r)^2}\) 則
\[{\rho^s_r}'=\frac{x_k-x_r}{R_k}*vx_k+\frac{y_k-y_r}{R_k}*vy_k+\frac{z_k-z_r}{R_k}*vz_k-\frac{x_k-x_r}{R_k}*vx_r-\frac{y_k-y_r}{R_k}*vy_r-\frac{z_k-z_r}{R_k}*vz_r \]
並代入多普勒觀測方程為
\[D^s_r=\frac{x_k-x_r}{R_k}*vx_k+\frac{y_k-y_r}{R_k}*vy_k+\frac{z_k-z_r}{R_k}*vz_k-\frac{x_k-x_r}{R_k}*vx_r-\frac{y_k-y_r}{R_k}*vy_r-\frac{z_k-z_r}{R_k}*vz_r+{c\Delta t_r}'-{c\Delta t^s}'+{e^s_r(t)}' \]
將接收機速度項放到左邊，多普勒測量值放到右邊，
\[(\frac{x_r-x_k}{R_k},\frac{y_r-y_k}{R_k},\frac{z_r-z_k}{R_k},1)*\left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} = D^s_r+(\frac{x_r-x_k}{R_k},\frac{y_r-y_k}{R_k},\frac{z_r-z_k}{R_k}) *\left\{\begin{matrix} vx_k\\ vy_k\\ vz_k \end{matrix} \right\}+{\Delta T^s}'-{e^s_r(t)}'\]
上述右邊公式中，衛星速度和衛星鍾差可通過星歷計算獲得。
設
\((\frac{x_r-x_k}{R_k},\frac{y_r-y_k}{R_k},\frac{z_r-z_k}{R_k},1)=I_k\)
當有n顆衛星進行定位時，誤差項\({e^s_r(t)}'\)不略不計，有方程組
\[\left\{\begin{matrix} \frac{(x_r-x_1)}{R_1} & \frac{(y_r-y_1)}{R_1} & \frac{(z_r-z_1)}{R_1} & 1 \\ \frac{(x_r-x_2)}{R_2} & \frac{(y_r-y_2)}{R_2} & \frac{(z_r-z_2)}{R_2} & 1 \\ \vdots\\ \frac{(x_r-x_n)}{R_n} & \frac{(y_r-y_n)}{R_n} & \frac{(z_r-z_n)}{R_n} & 1 \end{matrix} \right\} \left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} = \left\{\begin{matrix} {D_1}^s_r+(\frac{x_r-x_1}{R_1},\frac{y_r-y_1}{R_1},\frac{z_r-z_1}{R_1},1)*(vx_1,vy_1,vz_1,{\Delta T^s}')^T\\ {D_2}^s_r+(\frac{x_r-x_2}{R_2},\frac{y_r-y_2}{R_2},\frac{z_r-z_2}{R_2},1)*(vx_2,vy_2,vz_2,{\Delta T^s}')^T\\ \vdots\\ {D_n}^s_r+(\frac{x_r-x_n}{R_n},\frac{y_r-y_n}{R_n},\frac{z_r-z_n}{R_n},1)*(vx_n,vy_n,vz_n,{\Delta T^s}')^T\\ \end{matrix} \right\} \]
\[\left\{\begin{matrix} I_1 \\ I_2 \\ \vdots\\ I_n \end{matrix} \right\} \left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} = \left\{\begin{matrix} {D_1}^s_r+I_1*(vx_1,vy_1,vz_1,{\Delta T^s}')^T\\ {D_2}^s_r+I_2*(vx_2,vy_2,vz_2,{\Delta T^s}')^T\\ \vdots\\ {D_n}^s_r+I_n*(vx_n,vy_n,vz_n,{\Delta T^s}')^T\\ \end{matrix} \right\} \]
這里方程左邊的常量項和偽距定位方程中的幾何矩陣\(J(X_r)\)完全相同，可用最小二乘求解上述方程
最小二乘求解多普勒定速方程
將上述的多普勒定速方程組簡寫為
\[AX_r=B \]
采用最小二乘求解\(X_r=(vx_r,vy_r,vz_r,{\Delta T_r}')\)，\(min{||f(X_r)||}^2=min{||A*X_r-B||}^2\)
\[f(X_r)=A*X_r-B= \left\{\begin{matrix} I_1 \\ I_2 \\ \vdots\\ I_n \end{matrix} \right\} \left\{\begin{matrix} vx_r\\ vy_r\\ vz_r\\ \Delta T_r' \end{matrix} \right\} - \left\{\begin{matrix} {D_1}^s_r+I_1*(vx_1,vy_1,vz_1,{\Delta T^s}')^T\\ {D_2}^s_r+I_2*(vx_2,vy_2,vz_2,{\Delta T^s}')^T\\ \vdots\\ {D_n}^s_r+I_n*(vx_n,vy_n,vz_n,{\Delta T^s}')^T\\ \end{matrix} \right\} \]
\[ f(X_r) = \left\{\begin{matrix} I_1*(vx_r-vx_1,vy_r-vy_1,vz_r-vz_1,\Delta T_r'-{\Delta T^s}')^T - {D_1}^s_r\\ I_2*(vx_r-vx_2,vy_r-vy_2,vz_r-vz_2,\Delta T_r'-{\Delta T^s}')^T - {D_2}^s_r\\ \vdots\\ I_n*(vx_r-vx_n,vy_r-vy_n,vz_r-vz_n,\Delta T_r'-{\Delta T^s}')^T - {D_n}^s_r\\ \end{matrix} \right\} \]
\(f(X_r)\)為多普勒殘差, 求導得雅克比矩陣，\(J(X_r)=\frac{\partial(A*X_r-B)}{\partial X_r}=A\)
迭代如下公式直到收斂，即可求出\((vx_r,vy_r,vz_r,{\Delta T_r}')\)
\[\begin{cases} {X_r}_{k+1}={X_r}_{k}+\Delta X_r \\ \Delta X_r=[A^TA^{-1}]*A^T*f(X_r) \end{cases}\]
