server/algorithm/tdoa/taylor_wls_tdoa.h

#ifndef TAYLOR_WLS_TDOA_H
#define TAYLOR_WLS_TDOA_H

#include <Eigen/Dense>
#include <SyncTime/Position.h>
#include <memory>
#include <limits>

//#include "classdef.h"
//#include <unordered_map>
#include "../base/base_data_struct.h"
#include "../Functions/Functions.h"

namespace Tdoa{
	using namespace HostServer;
	//第三方矩阵库
	using namespace Eigen;

	const double DEFAULT_ITERATIVE_ACCURACY = 0.5;			// 默认迭代精度
	const unsigned int DEFAULT_ITERATIVE_TIMES = 20;		// 默认迭代次数

	class TaylorWls{
	public:
		TaylorWls(){
			is_initial = false;
			this->accuracy = DEFAULT_ITERATIVE_ACCURACY;
			this->iterator_times = DEFAULT_ITERATIVE_TIMES;
		}
		TaylorWls(double accuracy,unsigned int times){
			is_initial = false;
			this->accuracy = accuracy;
			this->iterator_times = times;
		}
		~TaylorWls(){}
	private:
		unsigned int iterator_times;	// 迭代次数
		bool is_initial;		// 是否初始化了初始坐标
		double accuracy;		// 迭代精度

		Position pos;			// 迭代函数的输出坐标
		Position iter_pos;		// 迭代函数的输入坐标
	private:
		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign> anchors;		// 分站坐标
		Matrix<unsigned long long,Eigen::Dynamic,1,Eigen::DontAlign> T;				// 分站时间戳

	private:
		//计算m的广义逆矩阵
		Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>& pinv(Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>& m);
		//生成符合正太分布的随机数
		double gauss_rand(double mu, double sigma);
		double gauss_rand();

	public:
		double get_accuracy(){return accuracy;};
		void   set_accuracy(const double & accuracy){this->accuracy = accuracy;};

		Position& get_position(){return pos;};
		void set_position(const Position& p){
			pos = p;
		};

		unsigned int get_iterative_times(){return iterator_times;};
		void set_iterative_times(unsigned int times){this->iterator_times = times;}

		// 计算坐标
		Position& calc_position(std::shared_ptr<algorithm::base::ReceiveDataUnorderedMap>& pRdm);
		// taylor+加权最小二乘算法
		Position& taylor_wls(Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::DontAlign>& anchors,Matrix<unsigned long long,Eigen::Dynamic,1,Eigen::DontAlign>& T,const Position& pos,unsigned int size);

		// 初始化迭代的初始坐标
		void init_position(const Position& pos){
			this->iter_pos = pos;
			this->pos = pos;
			is_initial = true;
		};

		int init_params(std::shared_ptr<algorithm::base::ReceiveDataUnorderedMap> pRdm);	//除了第一次定位以外，每次调用算法都会初始化分站坐标和tt
	};
};

#endif