20161009

Filter/KalmanFilter.cpp

@@ -56,9 +56,11 @@ void CKalmanFilter::Initial(int t)
 /*
  * 没有定位成功时调用
 */
-void CKalmanFilter::Predict()
+void CKalmanFilter::Predict(int t)
 {
 	//predict
+	m_pCar->A(0,1) = t;
+	m_pCar->A(2,3) = t;
 	m_pCar->x = m_pCar->A * m_pCar->x;
 	m_pCar->P = m_pCar->A * m_pCar->P *(m_pCar->A).adjoint() + m_pCar->Q;
 }
@@ -66,10 +68,10 @@ void CKalmanFilter::Predict()
 /*
  * 定位成功调用
 */
-void CKalmanFilter::Predict_Correct()
+void CKalmanFilter::Predict_Correct(int t)
 {
 	//predict
-	Predict();
+	Predict(t);
 	//correct
 	Eigen::MatrixXd K = m_pCar->P * m_pCar->H.adjoint() * (m_pCar->H * m_pCar->P *m_pCar->H.adjoint() + m_pCar->R).inverse();
 	m_pCar->x = m_pCar->x + K * (m_pCar->z - m_pCar->H * m_pCar->x);

Filter/KalmanFilter.h

@@ -4,7 +4,7 @@
 
 #include <Eigen/Dense>
 //#include "Car.cpp"
-using namespace Eigen;
+//using namespace Eigen;
 
 //ÓÃÓÚ¿¨¶ûÂüÂ˲¨
 const int READER_NUMS = 4;
@@ -27,8 +27,8 @@ public:
 	~CKalmanFilter();
 public:
 	void Initial(int t);
-	void Predict();
-	void Predict_Correct();
+	void Predict(int t);
+	void Predict_Correct(int t);
 };
 
 class Car

YAServer.vcxproj

@@ -46,7 +46,7 @@
     <OutDir>$(SolutionDir)..\..\output\$(Configuration)\bin\</OutDir>
     <IntDir>$(SolutionDir)..\IntDir\$(PlatformName)\$(Configuration)\$(ProjectName)\</IntDir>
     <IncludePath>$(BOOST)\;..\common\;..\thirdparty\include\socket.io-client-cpp\src\;..\thirdparty\include\rapidjson\include\;..\thirdparty\include\websocketpp\;..\thirdparty\include\mysql\;..\thirdparty\include\iocp\common\Src\;$(IncludePath)</IncludePath>
-    <LibraryPath>$(BOOSTLIB);..\thirdparty\lib\mysql\;$(LibraryPath)</LibraryPath>
+    <LibraryPath>..\lib\Debug\;$(BOOSTLIB);..\thirdparty\lib\mysql\;$(LibraryPath)</LibraryPath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
     <LinkIncremental>false</LinkIncremental>

YAServerDlg.cpp

@@ -1625,6 +1625,10 @@ void CYAServerDlg::parse_data_receive(BYTE *pData, int nLen, DWORD dwConnID )
 
 void CYAServerDlg::parse_package_data( BYTE* DataBuffer, int nLen, DWORD dwConnID )
 {
+	if(DataBuffer == NULL){
+		return;
+	}
+
 	CString strBuf;
 	formatByteArray(strBuf, DataBuffer, nLen);
 	writeErrorLog(_T("RAW_S"), strBuf, false);

classdef.cpp

@@ -271,6 +271,21 @@ Card::~Card(void)
 		m_pKalmanFilter = NULL;
 	}
 #endif
+
+	if(p_dists){
+		delete[] p_dists;
+		p_dists = NULL;
+	}
+
+	if(p_dists_locate){
+		delete[] p_dists_locate;
+		p_dists_locate = NULL;
+	}
+
+	if(p3_anchors){
+		delete[] p3_anchors;
+		p3_anchors = NULL;
+	}
 }
 
 void Card::get_coordinate_2d( int cnt )
@@ -629,6 +644,7 @@ void Card::get_coordinate( int cnt )
 	ReceiveDataMap pRdm;
 	pRdm.clear();
 	int t = 0;
+	double synmNumTime = (this->sync_num - this->last_locate.tt)*0.2;
 	bool bRet = false;
 	//for(it;it!=_dists.front()->mp_dist.end();it++){
 	for(int i=0;i<k;i++){
@@ -643,10 +659,10 @@ void Card::get_coordinate( int cnt )
 				bRet = true;
 				break;
 			}
-			if(prd->rec_time_stamp == 806418238616){
+			/*if(prd->rec_time_stamp == 806418238616){
 				t = 55;
 				t = p_dists_locate[i]->t;
-			}
+			}*/
 			pRdm.insert(ReceiveDataMap::value_type(prd->reader_id,*prd));
 		}		
 	}
@@ -671,6 +687,7 @@ void Card::get_coordinate( int cnt )
 			//this->m_pKalmanFilter->m_nCounts++;
 		}else{
 			this->m_pKalmanFilter->m_bFlag = true;
+			this->m_pKalmanFilter->m_nCounts++;
 			long long deltaT = this->t - this->last_locate.t;
 			double vx = 0;
 			double vy = 0;
@@ -697,9 +714,9 @@ void Card::get_coordinate( int cnt )
 			}else{
 				//只有三次以上才允许使用kalman滤波以下的函数
 				if(this->m_pKalmanFilter->m_bFlag){
-					this->m_pKalmanFilter->Predict_Correct();
+					this->m_pKalmanFilter->Predict_Correct(synmNumTime);
 				}else{
-					this->m_pKalmanFilter->Predict();
+					this->m_pKalmanFilter->Predict(synmNumTime);
 				}
 			}
 		}
@@ -707,6 +724,8 @@ void Card::get_coordinate( int cnt )
 		this->last_locate.x = this->x = this->m_pKalmanFilter->m_pCar->x(0,1);
 		this->last_locate.y = this->y = this->m_pKalmanFilter->m_pCar->x(2,1);
 		this->last_locate.z = this->z;
+		this->last_locate.t = this->t;
+		this->last_locate.tt = this->sync_num;
 		this->v = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,1),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,1),2));
 #else
 		//最新通过算法算出的结果

classdef.h

@@ -28,7 +28,7 @@
 #define ALGORITHM_TYPE_TDOA
 
 //Â˲¨¹¦ÄÜ¿ªÆô
-//#define FILTER_KALMAN
+#define FILTER_KALMAN
 
 #include <string>
 #include <ctime>

locate_algorithm.cpp

@@ -930,7 +930,12 @@ POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
 				r = LocateAlgorithm::GetPos(pRP,dist,i);
 				//ÎÞ½â»ò½âÎÞЧ
 
-				if(r == NULL || r->nCount == 0){
+				if(r == NULL ){
+					continue;
+				}
+				if(r->nCount == 0){
+					delete r;
+					r = NULL;
 					continue;
 				}
 

stdafx.h

@@ -3,5 +3,5 @@
 #define _USE_MFC
 #define _DETECT_MEMORY_LEAK
 
-//#include <Eigen/Dense>
+#include <Eigen/Dense>
 #include <GeneralHelper.h>