algo

YAServerDlg.cpp

@@ -6278,53 +6278,78 @@ void CYAServerDlg::Test()
 
 	double dMapScale = 3.46;
 
+	_coordinate** cd = new _coordinate*[4];
+	cd[0] = NULL;
+	cd[1] = NULL;
+	cd[2] = NULL;
+	cd[3] = NULL;
+
+	_coordinate c1;
+	c1.tt = 448142133759;
+	cd[0] = &c1;
+
+	_coordinate c2;
+	c2.tt = 448142159656;
+	cd[1] = &c2;
+
+	_coordinate c3;
+	c3.tt = 448142221673;
+	cd[2] = &c3;
+
+	_coordinate c4;
+	c4.tt = 448142175733;
+	cd[3] = &c4;
+
+	//HeapSort(cd,4);
+	SelectSort(cd,4);
+
 	//36
 	ReceiveData* prd1 = new ReceiveData();	
-	prd1->reader_id = 202;
+	prd1->reader_id = 103;
 	prd1->antenna_id = 1;
-	prd1->rec_time_stamp = 630968127665;//283613846791;
-	prd1->x = 945.180000*dMapScale;
-	prd1->y = 449.500000*dMapScale;
+	prd1->rec_time_stamp = 906407856760;//283613846791;
+	prd1->x = 1106.340000*dMapScale;
+	prd1->y = 386.500000*dMapScale;
 	prd1->z = 0;
 	pRdm.insert(ReceiveDataMap::value_type(prd1->reader_id,*prd1));
 
 	//34
 	ReceiveData* prd2 = new ReceiveData();	
-	prd2->reader_id = 203;
+	prd2->reader_id = 104;
 	prd2->antenna_id = 1;
-	prd2->rec_time_stamp = 630968138568;//283613853712;//
-	prd2->x = 898.310000*dMapScale;
+	prd2->rec_time_stamp = 906407902417;//283613853712;//
+	prd2->x = 1106.340000*dMapScale;
 	prd2->y = 449.500000*dMapScale;
 	prd2->z = 0;
 	pRdm.insert(ReceiveDataMap::value_type(prd2->reader_id,*prd2));	
 
 	//35
 	ReceiveData* prd3 = new ReceiveData();	
-	prd3->reader_id = 204;
+	prd3->reader_id = 102;
 	prd3->antenna_id = 1;
-	prd3->rec_time_stamp = 630968161996;//283613846609;
-	prd3->x = 838.880000*dMapScale;
-	prd3->y = 449.5*dMapScale;
+	prd3->rec_time_stamp = 906407904144;//283613846609;
+	prd3->x = 1106.340000*dMapScale;
+	prd3->y = 320.4*dMapScale;
 	prd3->z = 0;
 	pRdm.insert(ReceiveDataMap::value_type(prd3->reader_id,*prd3));
 
 	ReceiveData* prd4 = new ReceiveData();	
-	prd4->reader_id = 201;
+	prd4->reader_id = 101;
 	prd4->antenna_id = 1;
-	prd4->rec_time_stamp = 630968199054;//283613846609;
-	prd4->x = 1041.890000*dMapScale;
-	prd4->y = 449.500000*dMapScale;
+	prd4->rec_time_stamp = 906407930087;//283613846609;
+	prd4->x = 1106.340000*dMapScale;
+	prd4->y = 285.2*dMapScale;
 	prd4->z = 0;
 	pRdm.insert(ReceiveDataMap::value_type(prd4->reader_id,*prd4));
 
-	ReceiveData* prd5 = new ReceiveData();	
-	prd5->reader_id = 205;
-	prd5->antenna_id = 1;
-	prd5->rec_time_stamp = 630968206079;//283613846609;
-	prd5->x = 804.000000*dMapScale;
-	prd5->y = 449.500000*dMapScale;
-	prd5->z = 0;
-	pRdm.insert(ReceiveDataMap::value_type(prd5->reader_id,*prd5));
+	//ReceiveData* prd5 = new ReceiveData();	
+	//prd5->reader_id = 205;
+	//prd5->antenna_id = 1;
+	//prd5->rec_time_stamp = 630968206079;//283613846609;
+	//prd5->x = 804.000000*dMapScale;
+	//prd5->y = 285.200000*dMapScale;
+	//prd5->z = 0;
+	//pRdm.insert(ReceiveDataMap::value_type(prd5->reader_id,*prd5));
 
 	//ReceiveData* prd5 = new ReceiveData();	
 	//prd5->reader_id = 205;
@@ -6586,8 +6611,8 @@ void CYAServerDlg::deal_card_msg( Card* card, bool is_hist /*= false*/ )
 				CString strLog = _T("");
 
 				wchar_t* p_card_id = CFunctions::c2wc(card->card_id.c_str());
-				strLog.Format(_T("r: %d-%d, t: %d, l:%d, tt: %I64u, ft: %I64u, x1: %f, y1: %f, z1: %f, ct: %d, v: %f, cardid:%s"),
-					dist->reader_id, dist->antenna_id + 1, card->sync_num, card->p_reader->sync_level, dist->tt, card->flying_time,  dist->x, dist->y, dist->z,card->time_stamp, card->v, p_card_id/*CFunctions::c2wc(card->card_id.c_str())*/);
+				strLog.Format(_T("r: %d-%d, t: %d, l:%d, tt: %I64u, ft: %I64u, x1: %f, y1: %f, z1: %f, ct: %d, cardid:%s"),
+					dist->reader_id, dist->antenna_id + 1, card->sync_num, card->p_reader->sync_level, dist->tt, card->flying_time,  dist->x, dist->y, dist->z,card->time_stamp, p_card_id/*CFunctions::c2wc(card->card_id.c_str())*/);
 				writeErrorLog(_T("DIST_S"), strLog, true);
 				//strLog = _T("");
 				strLog.ReleaseBuffer(strLog.GetLength());
@@ -6599,12 +6624,12 @@ void CYAServerDlg::deal_card_msg( Card* card, bool is_hist /*= false*/ )
 					card->last_y = card->y;
 					card->last_z = card->z;
 
-					strLog.Format(_T("cardid:%s, cx: %f, cy: %f, cz: %f, clx: %f, cly: %f, ct: %d"), p_card_id /*CFunctions::c2wc(card->card_id.c_str())*/, card->x, card->y, card->z, card->last_locate.x, card->last_locate.y, card->time_stamp_cal);
+					strLog.Format(_T("cardid:%s, cx: %f, cy: %f, cz: %f, clx: %f, cly: %f, v: %f, ct: %d"), p_card_id /*CFunctions::c2wc(card->card_id.c_str())*/, card->x, card->y, card->z, card->last_locate.x, card->last_locate.y, card->v, card->time_stamp_cal);
 					writeErrorLog(_T("DIST_S"), strLog, false);
 					strLog.Empty();
 
 					strLog = _T("");
-					strLog.Format(_T("%d,%f,%f,%f,%f"),card->sync_num,card->origin_locate.x,card->origin_locate.y,card->x, card->y);
+					strLog.Format(_T("%d,%f,%f,%f,%f,f: %f,s: %f"),card->sync_num,card->origin_locate.x,card->origin_locate.y,card->x, card->y,card->m_dFirstDiff,card->m_dSecondDiff,card->time_stamp_cal);
 					writeErrorLog(_T("KALMAN_S"),strLog,false);
 				}
 				if(p_card_id){

classdef.cpp

@@ -65,6 +65,8 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 	pReaderPathMap = NULL;
 	pTdoaReaderPathMap = NULL;
 	last_s_locate_reader[0] = last_s_locate_reader[1] = -1;
+	m_syncNumList.clear();
+	m_dFirstDiff = m_dSecondDiff = 0.0;
 
 	last_locate.tt = 0;
 	last_locate.a = last_locate.antenna_id = last_locate.reader_id = last_locate.t = 0;
@@ -491,7 +493,8 @@ void Card::get_coordinate( int cnt )
 		return;
 	}
 
-	HeapSort(p_dists_locate,k);
+	//HeapSort(p_dists_locate,k);
+	SelectSort(p_dists_locate,k);
 
 	ReceiveDataMap pRdm;
 	pRdm.clear();
@@ -539,7 +542,105 @@ void Card::get_coordinate( int cnt )
 		this->origin_locate.y = p->posy / (this->map_scale*1.0);
 
 		double interval_time = this->p_reader->reader_interval_time;
-		double deltaT = (this->sync_num - this->m_nLastLocateT)*interval_time;
+
+		double deltaT = 0.0;
+		double cvx = 0;
+		double cvy = 0;
+		double cv  = 0;
+
+		double cx = 0;
+		double cy = 0;
+		double cz = 0;
+
+		cx = p->posx / (this->map_scale*1.0);
+		cy = p->posy / (this->map_scale*1.0);
+		cz = p->posz / (this->map_scale*1.0);
+
+		if(this->m_nLastLocateT == 0){
+			m_syncNumList.push_back(this->sync_num);
+		}
+		else{
+			list<unsigned long long>::iterator it = m_syncNumList.begin();
+			deltaT = (this->sync_num - *it)*interval_time;
+			if(deltaT - 2.0 > 0 ){
+				m_syncNumList.pop_front();
+				m_syncNumList.push_back(this->sync_num);
+
+				cvx = (cx - this->last_locate.x)/deltaT;
+				cvy = (cy - this->last_locate.y)/deltaT;
+				cvx *= this->map_scale;
+				cvy *= this->map_scale;
+
+				cv  = sqrt(pow(cvx,2) + pow(cvy,2));
+
+				double avx = (cvx - last_vx) / deltaT;
+				double avy = (cvy - last_vy) / deltaT;
+				double av = sqrt(pow(avx,2) + pow(avy,2));
+				//车卡的加速度
+				switch(this->card_type){
+				case CT_PERSON:
+					if(av > 3){
+						this->x = this->last_locate.x;
+						this->y = this->last_locate.y;
+						return;
+					}
+					break;
+				case CT_VEHICLE:
+					if(av > 5){
+						//保留上次结果
+						this->x = this->last_locate.x;
+						this->y = this->last_locate.y;
+						//this->m_nLastLocateT = this->sync_num;
+						return;
+					}
+					break;
+				}
+				this->last_vx = cvx;
+				this->last_vy = cvy;	
+			}
+		}
+
+		//if(deltaT != 0){
+		//	cvx = (cx - this->last_locate.x)/deltaT;
+		//	cvy = (cy - this->last_locate.y)/deltaT;
+		//	cvx *= this->map_scale;
+		//	cvy *= this->map_scale;
+
+		//	/*if(vx>=0){
+		//	nSign = 1;
+		//	}
+		//	else{
+		//	nSign = -1;
+		//	}*/
+		//	cv  = sqrt(pow(cvx,2) + pow(cvy,2));
+		//	//v *= nSign;
+
+		//	//last_vx = last_vy = 0.0;
+		//	double avx = (cvx - last_vx) / deltaT;
+		//	double avy = (cvy - last_vy) / deltaT;
+		//	double av = sqrt(pow(avx,2) + pow(avy,2));
+		//	//车卡的加速度
+		//	switch(this->card_type){
+		//	case CT_PERSON:
+		//		if(av > 3){
+		//			this->x = this->last_locate.x;
+		//			this->y = this->last_locate.y;
+		//			return;
+		//		}
+		//		break;
+		//	case CT_VEHICLE:
+		//		if(av > 5){
+		//			//保留上次结果
+		//			this->x = this->last_locate.x;
+		//			this->y = this->last_locate.y;
+		//			//this->m_nLastLocateT = this->sync_num;
+		//			return;
+		//		}
+		//		break;
+		//	}
+		//	this->last_vx = cvx;
+		//	this->last_vy = cvy;		
+		//}
 
 		if(m_nFilterType == FILTER_KALMAN){
 			////通过卡尔曼滤波处理
@@ -568,60 +669,60 @@ void Card::get_coordinate( int cnt )
 				this->m_pKalmanFilter->m_bFlag = true;
 				this->m_pKalmanFilter->m_nCounts++;
 
-				double vx = 0;
-				double vy = 0;
-				double v  = 0;
-
-				x = p->posx / (this->map_scale*1.0);
-				y = p->posy / (this->map_scale*1.0);
-				z = p->posz / (this->map_scale*1.0);
-
-				if(deltaT != 0){
-					vx = (this->x - this->last_locate.x)/deltaT;
-					vy = (this->y - this->last_locate.y)/deltaT;
-					vx *= this->map_scale;
-					vy *= this->map_scale;
-
-					/*if(vx>=0){
-						nSign = 1;
-					}
-					else{
-						nSign = -1;
-					}*/
-					v  = sqrt(pow(vx,2) + pow(vy,2));
-					//v *= nSign;
-
-					//last_vx = last_vy = 0.0;
-					double avx = (vx - last_vx) / deltaT;
-					double avy = (vy - last_vy) / deltaT;
-					double av = sqrt(pow(avx,2) + pow(avy,2));
-					//车卡的加速度
-					switch(this->card_type){
-					case CT_PERSON:
-						if(av > 3){
-							this->x = this->last_locate.x;
-							this->y = this->last_locate.y;
-							return;
-						}
-						break;
-					case CT_VEHICLE:
-						if(av > 0.5){
-							//保留上次结果
-							this->x = this->last_locate.x;
-							this->y = this->last_locate.y;
-							//this->m_nLastLocateT = this->sync_num;
-							return;
-						}
-						break;
-					}
-					this->last_vx = vx;
-					this->last_vy = vy;		
-				}
-
-				this->m_pKalmanFilter->m_pCar->z(0,0) = this->x;
-				this->m_pKalmanFilter->m_pCar->z(1,0) = vx;
-				this->m_pKalmanFilter->m_pCar->z(2,0) = this->y;
-				this->m_pKalmanFilter->m_pCar->z(3,0) = vy;
+				//double vx = 0;
+				//double vy = 0;
+				//double v  = 0;
+
+				//x = p->posx / (this->map_scale*1.0);
+				//y = p->posy / (this->map_scale*1.0);
+				//z = p->posz / (this->map_scale*1.0);
+
+				//if(deltaT != 0){
+				//	vx = (this->x - this->last_locate.x)/deltaT;
+				//	vy = (this->y - this->last_locate.y)/deltaT;
+				//	vx *= this->map_scale;
+				//	vy *= this->map_scale;
+
+				//	/*if(vx>=0){
+				//		nSign = 1;
+				//	}
+				//	else{
+				//		nSign = -1;
+				//	}*/
+				//	v  = sqrt(pow(vx,2) + pow(vy,2));
+				//	//v *= nSign;
+
+				//	//last_vx = last_vy = 0.0;
+				//	double avx = (vx - last_vx) / deltaT;
+				//	double avy = (vy - last_vy) / deltaT;
+				//	double av = sqrt(pow(avx,2) + pow(avy,2));
+				//	//车卡的加速度
+				//	switch(this->card_type){
+				//	case CT_PERSON:
+				//		if(av > 3){
+				//			this->x = this->last_locate.x;
+				//			this->y = this->last_locate.y;
+				//			return;
+				//		}
+				//		break;
+				//	case CT_VEHICLE:
+				//		if(av > 0.5){
+				//			//保留上次结果
+				//			this->x = this->last_locate.x;
+				//			this->y = this->last_locate.y;
+				//			//this->m_nLastLocateT = this->sync_num;
+				//			return;
+				//		}
+				//		break;
+				//	}
+				//	this->last_vx = vx;
+				//	this->last_vy = vy;		
+				//}
+
+				this->m_pKalmanFilter->m_pCar->z(0,0) = cx;
+				this->m_pKalmanFilter->m_pCar->z(1,0) = cvx;
+				this->m_pKalmanFilter->m_pCar->z(2,0) = cy;
+				this->m_pKalmanFilter->m_pCar->z(3,0) = cvy;
 
 				if(this->m_pKalmanFilter->m_nCounts == 1){
 					//第一次直接赋值
@@ -691,8 +792,7 @@ void Card::get_coordinate( int cnt )
 			this->m_pKalmanFilter->m_pCar->t = this->m_nLastLocateT = this->sync_num;
 			this->v = nSign * sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,0),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,0),2))*this->map_scale;
 
-			int nv = (int)(v*1000);
-			this->v = nv*1.0/1000.0;
+			this->v = cv*3.6;
 		}else{
 			//TRACE(_T("no kalman . \n"));
 			//最新通过算法算出的结果
@@ -706,17 +806,7 @@ void Card::get_coordinate( int cnt )
 				this->y = p->posy / (this->map_scale*1.0);
 				this->z = p->posz / (this->map_scale*1.0);
 
-				double vx = 0,vy = 0,v = 0;
-				if(deltaT != 0){
-					vx = (this->x - this->last_locate.x)/deltaT;
-					vy = (this->y - this->last_locate.y)/deltaT;
-					vx *= this->map_scale;
-					vy *= this->map_scale;
-
-					v  = sqrt(pow(vx,2) + pow(vy,2));
-				}
-
-				this->v = this->v*3.6;
+				this->v = cv*3.6;
 
 				this->last_locate.x = this->x;
 				this->last_locate.y = this->y;

classdef.h

@@ -316,12 +316,18 @@ struct POS{
 	int nFirstReader;
 	int nSecondReader;
 
+	//精度参考
+	double dFirstDiff;
+	double dSecondDiff;
+
 	POS& operator=(POS& tmp){
 		nFirstReader = tmp.nFirstReader;
 		nSecondReader = tmp.nSecondReader;
 		posx = tmp.posx;
 		posy = tmp.posy;
 		posz = tmp.posz;
+		dFirstDiff = tmp.dFirstDiff;
+		dSecondDiff = tmp.dSecondDiff;
 		pos_radius = tmp.pos_radius;
 	}
 };
@@ -624,6 +630,8 @@ public:
 	_coordinate** p_dists_locate;
 	_coordinate last_locate;
 	_coordinate origin_locate;
+	double m_dFirstDiff;
+	double m_dSecondDiff;
 	
 	DistQueMap _dists;
 	int time_stamp_max; // 最大时间戳，即需要计算定位的时间戳
@@ -681,6 +689,7 @@ public:
 	//bool is_init_kalman;
 	CKalmanFilter* m_pKalmanFilter;
 	//int m_max_stamp; // 最大的时间戳，即最近采集的数据
+	list<unsigned long long> m_syncNumList;
 
 	int last_s_locate_reader[2];	//上一次定位成功的分站信息
 

locate_algorithm.cpp

@@ -840,6 +840,8 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 	pos->posy = 0;
 	pos->posz = 0;
 	pos->pos_radius = 0;
+	pos->dFirstDiff = 0.0;
+	pos->dSecondDiff = 0.0;
 
 	double a = 0;
 	double ref_dist = 0.0;   //两分站之间无路径的距离差
@@ -1089,6 +1091,14 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 		for(int i = 0;i<res_idx;i++){//2
 			double d_tmp = sqrt(pow(res[i].x - tmp_reader[0].x,2) + pow(res[i].y - tmp_reader[0].y,2)) - sqrt(pow(res[i].x - tmp_reader[1].x,2) + pow(res[i].y - tmp_reader[1].y,2));
 			double d_diff = abs(abs(d_tmp) - abs(ref_dist));
+			switch(i){
+			case 0:
+				pos->dFirstDiff = d_diff;
+				break;
+			case 1:
+				pos->dSecondDiff = d_diff;
+				break;
+			}
 			if(d_diff < d){
 				d = d_diff;
 				r_idx = i;
@@ -1461,7 +1471,7 @@ bool LocateAlgorithm::CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScal
 		}	
 	}
 
-	//if(nSize == 3){
+	//if(nSize >= 3){
 	//	//先找出未参与定位的分站
 	//	ReceiveDataMap::iterator f_it = pRdm->end();
 	//	ReceiveDataMap::iterator it = pRdm->begin();
@@ -1551,3 +1561,21 @@ void SwapElement(_coordinate* a,_coordinate*b)
 	*b = tmp;
 }
 
+void SelectSort(_coordinate** pCoordinateArray,int nLen)
+{  
+    for (int i=0; i<nLen; i++)  
+    {  
+        int k = i;  
+        unsigned long long key = pCoordinateArray[i]->tt;  
+        for (int j=i+1; j<nLen; j++)  
+        {  
+            if (pCoordinateArray[j]->tt < key) 
+            {  
+                k = j;  
+                key = pCoordinateArray[j]->tt;  
+            }  
+        }  
+        if (k!=i)  
+            SwapElement(pCoordinateArray[i], pCoordinateArray[k]);  
+    }  
+}  

locate_algorithm.h

@@ -30,6 +30,8 @@ void HeapSort(_coordinate** pCoordinateArray,int nLen);
 void BuildMaxHeap(_coordinate** pCoordinateArray,int nLen);
 void AdjustMaxHeap(_coordinate** pCoordinateArray,int n,int nHeapSize);
 void SwapElement(_coordinate* a,_coordinate*b);
+void SelectSort(_coordinate** pCoordinateArray,int nLen);
+
 /*
  * ¶¨Î»Ëã·¨
  *