最小二乘2D三点定位算法

最小二乘(2D)三点定位算法

$$(p_i-p_0{)}^T(p_i-p0)=r_i^2$$
$${\mathrm{argmin}}_{p_0}\Vert \mathrm{J}(p_0)\Vert .$$
$$\mathrm{J}(p_0)=\sum _{i=1}^N[(p_i-p_0{)}^T(p_i-p_0))-r_i^2{]}^2$$
2D 三点定位需要的beacon 点数大于等2.下面开始对公式进行推导:

$$\frac{\partial \mathrm{J}(p_0)}{\partial p_0}=a+B{p}_0+[2p_0{p}_0^T+(p_0{p}_0^T)I]c-p_0{p}_0^T{p}_0=0$$

$$a=\frac{1}{N}\sum _{i=1}^N(p_i{p}_i^T{p}_i-r_i^2{p}_i)$$
$$B=\frac{1}{N}[-2p_i{p}_i^T-(p_i^T{p}_i)I+r_i^{2}I]$$
$$c=\frac{1}{N}\sum _{i=1}^N{p}_i$$
$$p_0=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+Bc+2c{c}^{T}c$$
$$D=B+2c{c}^T+(c^{T}c)I$$
$$f+[D-(q^{T}q)Iq]=0$$
$$D-(q^{T}q)I=-\frac{2}{N}\sum _{i=1}^N{p}_i{p}_i^T+2c{c}^T$$
$$H=D-(q^{T}q)I$$
$$f+Hq=0$$
$$q=-H^{-1}f$$

#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;
}