最小二乘(2D)三点定位算法
(
p
i
−
p
0
)
T
(
p
i
−
p
0
)
=
r
i
2
(p_i - p_0)^T(p_i - p0) = r_i^2
(pi−p0)T(pi−p0)=ri2
arg
min
p
0
∥
J
(
p
0
)
∥
.
\mathop{\arg\min}_{\ p_0} \ \ \| \mathrm{J} (\ p_0)\|.
argmin p0 ∥J( p0)∥.
J
(
p
0
)
=
∑
i
=
1
N
[
(
p
i
−
p
0
)
T
(
p
i
−
p
0
)
)
−
r
i
2
]
2
\mathrm{J} (\ p_0) = \sum_{i=1}^{N}[(p_i-p_0)^T(p_i-p_0)) - r_i^2]^2
J( p0)=i=1∑N[(pi−p0)T(pi−p0))−ri2]2
2D 三点定位需要的beacon 点数大于等2.下面开始对公式进行推导:
∂ J ( p 0 ) ∂ p 0 = a + B p 0 + [ 2 p 0 p 0 T + ( p 0 p 0 T ) I ] c − p 0 p 0 T p 0 = 0 \frac{\partial \mathrm{J} (\ p_0)}{\partial p_0} = a + Bp_0 + [2p_0p_0^T + (p_0p_0^T)I]c-p_0p_0^Tp_0 = 0 ∂p0∂J( p0)=a+Bp0+[2p0p0T+(p0p0T)I]c−p0p0Tp0=0
a
=
1
N
∑
i
=
1
N
(
p
i
p
i
T
p
i
−
r
i
2
p
i
)
a = \frac{1}{N}\sum_{i=1}^{N}(p_ip_i^Tp_i - r_i^2p_i)
a=N1i=1∑N(pipiTpi−ri2pi)
B
=
1
N
[
−
2
p
i
p
i
T
−
(
p
i
T
p
i
)
I
+
r
i
2
I
]
B = \frac{1}{N}[-2p_ip_i^T -(p_i^Tp_i)I + r_i^2I]
B=N1[−2pipiT−(piTpi)I+ri2I]
c
=
1
N
∑
i
=
1
N
p
i
c = \frac{1}{N}\sum_{i=1}^{N}p_i
c=N1i=1∑Npi
p
0
=
q
+
c
p_0 = q+c
p0=q+c
(
a
+
B
c
+
2
c
c
T
c
)
+
{
B
q
+
[
2
c
c
T
+
(
c
T
c
)
I
]
q
}
−
q
q
T
q
=
0
(a+Bc+2cc^Tc) +\{Bq+[2cc^T+(c^Tc)I]q\} - qq^Tq =0
(a+Bc+2ccTc)+{Bq+[2ccT+(cTc)I]q}−qqTq=0
f
=
a
+
B
c
+
2
c
c
T
c
f = a+Bc+2cc^Tc
f=a+Bc+2ccTc
D
=
B
+
2
c
c
T
+
(
c
T
c
)
I
D = B+2cc^T+(c^Tc)I
D=B+2ccT+(cTc)I
f
+
[
D
−
(
q
T
q
)
I
q
]
=
0
f +[D-(q^Tq)Iq]=0
f+[D−(qTq)Iq]=0
D
−
(
q
T
q
)
I
=
−
2
N
∑
i
=
1
N
p
i
p
i
T
+
2
c
c
T
D-(q^Tq)I = -\frac{2}{N}\sum_{i=1}^{N}p_ip_i^T+ 2cc^T
D−(qTq)I=−N2i=1∑NpipiT+2ccT
H
=
D
−
(
q
T
q
)
I
H = D-(q^Tq)I
H=D−(qTq)I
f
+
H
q
=
0
f + Hq=0
f+Hq=0
q
=
−
H
−
1
f
q = -H^{-1}f
q=−H−1f
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main()
{
double r1 = 12.1;
double r2 = 9.1;
double r3 = 5.9;
double r4 = 9.8;
Mat p1 = (Mat_<double>(2,1)<< -7.7, 6.3);
Mat p2 = (Mat_<double>(2,1)<< 7.6, 7.1);
Mat p3 = (Mat_<double>(2,1)<< 7.6, 12.8);
Mat p4 = (Mat_<double>(2,1)<< -7.7, 16.9);
Mat I = (Mat_<double>(2,2)<< 1, 0, 0, 1);
//a
Mat a = (p1*p1.t()*p1 - r1*r1*p1 +
p2*p2.t()*p2 - r2*r2*p2 +
p3*p3.t()*p3 - r3*r3*p3 +
p4*p4.t()*p4 - r4*r4*p4)/4;
//B
Mat B = (( -2*p1*p1.t() -Mat((p1.t()*p1)).at<double>(0,0)*I + r1*r1*I )+
( -2*p2*p2.t() -Mat((p2.t()*p2)).at<double>(0,0)*I + r2*r2*I )+
( -2*p3*p3.t() -Mat((p3.t()*p3)).at<double>(0,0)*I + r3*r3*I) +
( -2*p4*p4.t() -Mat((p4.t()*p4)).at<double>(0,0)*I + r4*r4*I))/4;
//c
Mat c = (p1 + p2 + p3 + p4)/4;
//f
Mat f = a + B*c +2*c*c.t()*c; // (2,1)
//D
Mat D = B + 2*c*c.t() + Mat(c.t()*c).at<double>(0,0)*I; //(2,2)
//H
Mat H = -(p1*p1.t() + p2*p2.t() + p3*p3.t() + p4*p4.t())/2 + 2*c*c.t(); //(2,2)
//q
Mat q = -1*H.inv()*f;
// result p0
Mat p0 = c+q;
}