TOA（到达时间）定位的核心是通过测量信号从目标到多个基站的传播时间，将其转换为距离信息，并利用几何关系解算目标位置。 本文给出具体的定位模型建模、位置解算（多种方法）、CRLB计算步骤及公式推导
文章目录
- 通用模型建立
- 非线性方程组构建
- 线性化处理（最小二乘法）
- 最大似然估计（ML）
- 高斯-牛顿迭代法
- 误差分析（克拉美罗界，CRLB）
- 关键公式总结
- 应用扩展
通用模型建立
假设条件：
- 已知
N
N
N个基站坐标(
x
i
,
y
i
)
(x_i, y_i)
(xi,yi)（i
=
1
,
2
,
.
.
.
,
N
i=1,2,…,N
i=1,2,…,N）。 - 目标位置为未知参数
(
x
,
y
)
(x, y)
(x,y)。 - 信号传播速度为光速
c
c
c（空中场景通常取c
=
3
×
10
8
m/s
c=3 \\times 10^8 \\, \\text{m/s}
c=3×108m/s）。
距离模型： 信号到达时间
t
i
t_i
ti对应的距离为：
r
i
=
c
⋅
t
i
=
(
x
−
x
i
)
2
+
(
y
−
y
i
)
2
+
n
i
r_i = c \\cdot t_i = \\sqrt{(x – x_i)^2 + (y – y_i)^2} + n_i
ri=c⋅ti=(x−xi)2+(y−yi)2
+ni 其中
n
i
n_i
ni为测量噪声（通常假设为高斯白噪声），实际距离为
d
i
=
(
x
−
x
i
)
2
+
(
y
−
y
i
)
2
d_i = \\sqrt{(x – x_i)^2 + (y – y_i)^2}
di=(x−xi)2+(y−yi)2
。
非线性方程组构建
每个基站对应一个圆方程：
(
x
−
x
i
)
2
+
(
y
−
y
i
)
2
=
r
i
2
(
i
=
1
,
2
,
.
.
.
,
N
)
(x – x_i)^2 + (y – y_i)^2 = r_i^2 \\quad (i=1,2,…,N)
(x−xi)2+(y−yi)2=ri2(i=1,2,…,N) 由于噪声存在，方程需通过优化方法求解目标位置
(
x
,
y
)
(x, y)
(x,y)。
线性化处理（最小二乘法）
将非线性方程转化为线性方程组以减少计算复杂度：
展开方程： 对第
i
i
i个基站：
x
2
−
2
x
i
x
+
x
i
2
+
y
2
−
2
y
i
y
+
y
i
2
=
r
i
2
x^2 – 2x_i x + x_i^2 + y^2 – 2y_i y + y_i^2 = r_i^2
x2−2xix+xi2+y2−2yiy+yi2=ri2
消去二次项： 选取第1个基站为参考，其余方程减去参考方程：
−
2
(
x
i
−
x
1
)
x
−
2
(
y
i
−
y
1
)
y
=
r
i
2
−
r
1
2
−
(
x
i
2
+
y
i
2
−
x
1
2
−
y
1
2
)
-2(x_i – x_1)x – 2(y_i – y_1)y = r_i^2 – r_1^2 – (x_i^2 + y_i^2 – x_1^2 – y_1^2)
−2(xi−x1)x−2(yi−y1)y=ri2−r12−(xi2+yi2−x12−y12) 令
K
i
=
x
i
2
+
y
i
2
K_i = x_i^2 + y_i^2
Ki=xi2+yi2，则方程简化为：
−
2
(
x
i
−
x
1
)
x
−
2
(
y
i
−
y
1
)
y
=
r
i
2
−
r
1
2
−
(
K
i
−
K
1
)
-2(x_i – x_1)x – 2(y_i – y_1)y = r_i^2 – r_1^2 – (K_i – K_1)
−2(xi−x1)x−2(yi−y1)y=ri2−r12−(Ki−K1)
矩阵形式： 对(N-1)个方程，构造线性方程组：
A
X
=
b
\\mathbf{A} \\mathbf{X} = \\mathbf{b}
AX=b 其中：
A
=
[
−
2
(
x
2
−
x
1
)
−
2
(
y
2
−
y
1
)
⋮
⋮
−
2
(
x
N
−
x
1
)
−
2
(
y
N
−
y
1
)
]
,
X
=
[
x
y
]
,
b
=
[
r
2
2
−
r
1
2
−
(
K
2
−
K
1
)
⋮
r
N
2
−
r
1
2
−
(
K
N
−
K
1
)
]
\\mathbf{A} = \\begin{bmatrix} -2(x_2 – x_1) & -2(y_2 – y_1) \\\\ \\vdots & \\vdots \\\\ -2(x_N – x_1) & -2(y_N – y_1) \\end{bmatrix}, \\quad \\mathbf{X} = \\begin{bmatrix} x \\\\ y \\end{bmatrix}, \\quad \\mathbf{b} = \\begin{bmatrix} r_2^2 – r_1^2 – (K_2 – K_1) \\\\ \\vdots \\\\ r_N^2 – r_1^2 – (K_N – K_1) \\end{bmatrix}
A=
−2(x2−x1)⋮−2(xN−x1)−2(y2−y1)⋮−2(yN−y1)
,X=[xy],b=
r22−r12−(K2−K1)⋮rN2−r12−(KN−K1)
最小二乘解：
X
^
=
(
A
T
A
)
−
1
A
T
b
\\mathbf{\\hat{X}} = (\\mathbf{A}^T \\mathbf{A})^{-1} \\mathbf{A}^T \\mathbf{b}
X^=(ATA)−1ATb
最大似然估计（ML）
假设噪声
n
i
n_i
ni服从均值为0、方差为
σ
i
2
\\sigma_i^2
σi2的高斯分布，构建似然函数：
P
(
r
∣
x
)
=
∏
i
=
1
N
1
2
π
σ
i
e
−
(
r
i
−
d
i
)
2
2
σ
i
2
P(\\mathbf{r} | \\mathbf{x}) = \\prod_{i=1}^N \\frac{1}{\\sqrt{2\\pi}\\sigma_i} e^{-\\frac{(r_i – d_i)^2}{2\\sigma_i^2}}
P(r∣x)=i=1∏N2π
σi1e−2σi2(ri−di)2 取对数似然函数并忽略常数项：
L
(
x
)
=
−
∑
i
=
1
N
(
r
i
−
d
i
)
2
2
σ
i
2
L(\\mathbf{x}) = -\\sum_{i=1}^N \\frac{(r_i – d_i)^2}{2\\sigma_i^2}
L(x)=−i=1∑N2σi2(ri−di)2 优化目标为最小化残差平方和：
x
^
=
arg
min
x
∑
i
=
1
N
(
r
i
−
d
i
)
2
σ
i
2
\\hat{\\mathbf{x}} = \\arg \\min_{\\mathbf{x}} \\sum_{i=1}^N \\frac{(r_i – d_i)^2}{\\sigma_i^2}
x^=argxmini=1∑Nσi2(ri−di)2
高斯-牛顿迭代法
当测量噪声较大时，需通过迭代优化求解非线性最小二乘问题：
x
(
0
)
=
[
x
0
,
y
0
]
T
\\mathbf{x}^{(0)} = [x_0, y_0]^T
x(0)=[x0,y0]T。
f
i
(
x
)
=
r
i
−
(
x
−
x
i
)
2
+
(
y
−
y
i
)
2
f_i(\\mathbf{x}) = r_i – \\sqrt{(x – x_i)^2 + (y – y_i)^2}
fi(x)=ri−(x−xi)2+(y−yi)2，雅可比矩阵为：
J
=
[
∂
f
1
∂
x
∂
f
1
∂
y
⋮
⋮
∂
f
N
∂
x
∂
f
N
∂
y
]
,
∂
f
i
∂
x
=
x
−
x
i
d
i
,
∂
f
i
∂
y
=
y
−
y
i
d
i
\\mathbf{J} = \\begin{bmatrix} \\frac{\\partial f_1}{\\partial x} & \\frac{\\partial f_1}{\\partial y} \\\\ \\vdots & \\vdots \\\\ \\frac{\\partial f_N}{\\partial x} & \\frac{\\partial f_N}{\\partial y} \\end{bmatrix}, \\quad \\frac{\\partial f_i}{\\partial x} = \\frac{x – x_i}{d_i}, \\quad \\frac{\\partial f_i}{\\partial y} = \\frac{y – y_i}{d_i}
J=
∂x∂f1⋮∂x∂fN∂y∂f1⋮∂y∂fN
,∂x∂fi=dix−xi,∂y∂fi=diy−yi
x
(
k
+
1
)
=
x
(
k
)
−
(
J
T
W
J
)
−
1
J
T
W
f
(
x
(
k
)
)
\\mathbf{x}^{(k+1)} = \\mathbf{x}^{(k)} – (\\mathbf{J}^T \\mathbf{W} \\mathbf{J})^{-1} \\mathbf{J}^T \\mathbf{W} \\mathbf{f}(\\mathbf{x}^{(k)})
x(k+1)=x(k)−(JTWJ)−1JTWf(x(k)) 其中
W
\\mathbf{W}
W为权重矩阵（通常取噪声方差的倒数）。
误差分析（克拉美罗界，CRLB）
定位误差的理论下限可通过Fisher信息矩阵（FIM）计算：
F
I
M
=
∑
i
=
1
N
1
σ
i
2
[
(
x
−
x
i
)
2
d
i
2
(
x
−
x
i
)
(
y
−
y
i
)
d
i
2
(
x
−
x
i
)
(
y
−
y
i
)
d
i
2
(
y
−
y
i
)
2
d
i
2
]
\\mathbf{FIM} = \\sum_{i=1}^N \\frac{1}{\\sigma_i^2} \\begin{bmatrix} \\frac{(x – x_i)^2}{d_i^2} & \\frac{(x – x_i)(y – y_i)}{d_i^2} \\\\ \\frac{(x – x_i)(y – y_i)}{d_i^2} & \\frac{(y – y_i)^2}{d_i^2} \\end{bmatrix}
FIM=i=1∑Nσi21
di2(x−xi)2di2(x−xi)(y−yi)di2(x−xi)(y−yi)di2(y−yi)2
CRLB为：
Var
(
x
^
)
≥
[
F
I
M
−
1
]
11
,
Var
(
y
^
)
≥
[
F
I
M
−
1
]
22
\\text{Var}(\\hat{x}) \\geq [\\mathbf{FIM}^{-1}]_{11}, \\quad \\text{Var}(\\hat{y}) \\geq [\\mathbf{FIM}^{-1}]_{22}
Var(x^)≥[FIM−1]11,Var(y^)≥[FIM−1]22
关键公式总结
r
i
=
(
x
−
x
i
)
2
+
(
y
−
y
i
)
2
+
n
i
r_i = \\sqrt{(x – x_i)^2 + (y – y_i)^2} + n_i
ri=(x−xi)2+(y−yi)2+ni 。
X
^
=
(
A
T
A
)
−
1
A
T
b
\\mathbf{\\hat{X}} = (\\mathbf{A}^T \\mathbf{A})^{-1} \\mathbf{A}^T \\mathbf{b}
X^=(ATA)−1ATb 。
应用扩展
- 双向测距（TW-TOA）：通过往返时间消除时钟同步误差，公式为：
T
prop
=
1
2
(
T
round
−
T
reply
)
T_{\\text{prop}} = \\frac{1}{2}(T_{\\text{round}} – T_{\\text{reply}})
Tprop=21(Tround−Treply) 适用于无严格时钟同步的场景。参考：TWR定位(Two Way Ranging,双向定位)，原理解析、公式推导、参考文献、解算方法等 - 混合定位：结合AOA或RSS提升定位精度。参考：【MATLAB例程】AOA定位、AOA与TOA混合定位，二维环境下的对比，基站（锚点数量）自适应调整，附完整的MATLAB代码
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
