kalman

Filter/Filter.cpp

@@ -1,12 +0,0 @@
-#include "stdafx.h"
-#include "Filter.h"
-
-
-CFilter::CFilter(void)
-{
-}
-
-
-CFilter::~CFilter(void)
-{
-}

Filter/Filter.h

@@ -1,12 +0,0 @@
-#pragma once
-class CFilter
-{
-public:
-	CFilter(void);
-	~CFilter(void);
-};
-
-class CKalmanFilter:public CFilter{
-public:
-	//void kalmanIntial(Card, int t)
-};

Filter/KalmanFilter.cpp

@@ -0,0 +1,73 @@
+//#include "stdafx.h"
+#include "KalmanFilter.h"
+
+CKalmanFilter::CKalmanFilter()
+{
+	m_pCar = NULL;
+	if(m_pCar == NULL){
+		m_pCar = new Car;
+	}
+}
+CKalmanFilter::~CKalmanFilter()
+{
+	if(m_pCar){
+		delete m_pCar;
+		m_pCar = NULL;
+	}
+}
+/*
+ * 函数名：Initial
+ *
+ * param:
+ *		car
+ *		t
+ *
+ * return:
+ *		无
+ *
+*/
+void CKalmanFilter::Initial(int t)
+{
+	m_pCar->A.setIdentity(READER_NUMS, READER_NUMS);
+	m_pCar->A(0, 1) = t;
+	m_pCar->A(2, 3) = t;
+	m_pCar->x.setZero(READER_NUMS, 1);
+	m_pCar->P.setZero(READER_NUMS, READER_NUMS);
+	m_pCar->P(0, 0) = 8000;
+	m_pCar->P(1, 1) = 10;
+	m_pCar->P(2, 2) = 8000;
+	m_pCar->P(3, 3) = 10;
+	m_pCar->Q.setZero(READER_NUMS, READER_NUMS);
+	m_pCar->Q(0, 0) = 2.5;
+	m_pCar->Q(1, 1) = 0.1;
+	m_pCar->Q(2, 2) = 2.5;
+	m_pCar->Q(3, 3) = 0.1;
+
+	m_pCar->H.setIdentity(OBSERVATION_NUMS, OBSERVATION_NUMS);
+	m_pCar->z.setZero(OBSERVATION_NUMS, 1);
+	m_pCar->R.setIdentity(OBSERVATION_NUMS, OBSERVATION_NUMS);
+	m_pCar->t = 0;
+}
+
+/*
+ * 没有定位成功时调用
+*/
+void CKalmanFilter::Predict()
+{
+	//predict
+	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;
+}
+
+/*
+ * 定位成功调用
+*/
+void CKalmanFilter::Predict_Correct()
+{
+	//predict
+	Predict();
+	//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);
+	m_pCar->P = m_pCar->P - K * m_pCar->H * m_pCar->P.adjoint();
+}

Filter/KalmanFilter.h

@@ -0,0 +1,48 @@
+//#pragma once
+#ifndef KALMAN_FILTER_H
+#define KALMAN_FILTER_H
+
+
+#include "../global.h"
+
+//用于卡尔曼滤波
+const int READER_NUMS = 4;
+const int OBSERVATION_NUMS = 4;
+
+class Car;
+
+class CKalmanFilter{
+public:
+	Car* m_pCar;
+public:
+	bool m_bFlag;	//定位标志位
+	int  m_nCounts; //定位成功数
+public:
+	CKalmanFilter();
+	~CKalmanFilter();
+public:
+	void Initial(int t);
+	void Predict();
+	void Predict_Correct();
+};
+
+class Car
+{	
+public:
+	//内存对齐
+	//EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	
+	int t;
+	Eigen::Matrix<double, READER_NUMS, READER_NUMS> A;
+	//滤波结果保存，0 - 3 分别代表x位置坐标 x 方向速度 y坐标 y速度
+	Eigen::Matrix<double, READER_NUMS, 1> x;
+	Eigen::Matrix<double, READER_NUMS, READER_NUMS> P;
+	Eigen::Matrix<double, READER_NUMS, READER_NUMS> Q;
+
+	Eigen::Matrix<double, OBSERVATION_NUMS, OBSERVATION_NUMS> H;
+	Eigen::Matrix<double, OBSERVATION_NUMS, OBSERVATION_NUMS> R;
+	//原始定位结果，0 - 3 分别代表x位置坐标 x 方向速度 y坐标 y速度
+	Eigen::Matrix<double, OBSERVATION_NUMS, 1> z;
+};
+
+#endif

SyncTime/Common.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #define MAX_TIME_DELAY_NUM	65536
 #define TEN_BILLION			10000000000
 #define ROOT_ANCHOR_NUM		6913

SyncTime/SyncHelper.cpp

@@ -1,4 +1,5 @@
 #include"SyncHelper.h"
+
 long long HostServer::SyncHelper::parseTime(const char * c)
 {
     long long ttt0 = (((long long)(c[0]))<<32)  & (0x000000FF00000000);

SyncTime/SyncManager.cpp

@@ -1,6 +1,7 @@
 #include"SyncManager.h"
 #include <deque>
-#include <Eigen/Dense>
+//#include <Eigen/Dense>
+#include "../global.h"
 
 HostServer::SyncManager::SyncManager()
 {

YAServer.vcxproj

@@ -164,6 +164,8 @@ xcopy "$(SolutionDir)..\config.ini" "D:\0a-share\$(Configuration)\" /Y /C /D /S<
     <ClInclude Include="constdef.h" />
     <ClInclude Include="DbSettingDlg.h" />
     <ClInclude Include="def.h" />
+    <ClInclude Include="Filter\KalmanFilter.h" />
+    <ClInclude Include="global.h" />
     <ClInclude Include="LogSetting.h" />
     <ClInclude Include="log_def.h" />
     <ClInclude Include="MysqlConnPool.h" />
@@ -222,12 +224,19 @@ xcopy "$(SolutionDir)..\config.ini" "D:\0a-share\$(Configuration)\" /Y /C /D /S<
       <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">NotUsing</PrecompiledHeader>
       <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">NotUsing</PrecompiledHeader>
     </ClCompile>
-    <ClCompile Include="classdef.cpp" />
+    <ClCompile Include="classdef.cpp">
+      <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">NotUsing</PrecompiledHeader>
+    </ClCompile>
     <ClInclude Include="locate_algorithm.h">
       <FileType>CppCode</FileType>
     </ClInclude>
     <ClCompile Include="DbSettingDlg.cpp" />
-    <ClCompile Include="locate_algorithm.cpp" />
+    <ClCompile Include="Filter\KalmanFilter.cpp">
+      <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">NotUsing</PrecompiledHeader>
+    </ClCompile>
+    <ClCompile Include="locate_algorithm.cpp">
+      <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">NotUsing</PrecompiledHeader>
+    </ClCompile>
     <ClCompile Include="LogSetting.cpp" />
     <ClCompile Include="MysqlConnPool.cpp" />
     <ClCompile Include="stdafx.cpp">
@@ -243,8 +252,12 @@ xcopy "$(SolutionDir)..\config.ini" "D:\0a-share\$(Configuration)\" /Y /C /D /S<
       <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">NotUsing</PrecompiledHeader>
     </ClCompile>
     <ClCompile Include="SysSetting.cpp" />
-    <ClCompile Include="YAServer.cpp" />
-    <ClCompile Include="YAServerDlg.cpp" />
+    <ClCompile Include="YAServer.cpp">
+      <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">NotUsing</PrecompiledHeader>
+    </ClCompile>
+    <ClCompile Include="YAServerDlg.cpp">
+      <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">NotUsing</PrecompiledHeader>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ResourceCompile Include="YAServer.rc" />

YAServer.vcxproj.filters

@@ -214,6 +214,12 @@
     <ClInclude Include="SyncTime\SyncManager.h">
       <Filter>synctime</Filter>
     </ClInclude>
+    <ClInclude Include="global.h">
+      <Filter>头文件</Filter>
+    </ClInclude>
+    <ClInclude Include="Filter\KalmanFilter.h">
+      <Filter>Filter</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="YAServer.cpp">
@@ -303,6 +309,9 @@
     <ClCompile Include="SyncTime\SyncManager.cpp">
       <Filter>synctime</Filter>
     </ClCompile>
+    <ClCompile Include="Filter\KalmanFilter.cpp">
+      <Filter>Filter</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ResourceCompile Include="YAServer.rc">

YAServerDlg.cpp

@@ -1,7 +1,6 @@
 
 // YAServerDlg.cpp : ʵÏÖÎļþ
 //
-
 #include "stdafx.h"
 #include "YAServer.h"	
 #include "YAServerDlg.h"
@@ -1648,7 +1647,7 @@ void CYAServerDlg::parse_package_data( BYTE* DataBuffer, int nLen, DWORD dwConnI
 		}
 	case CHAR_TIMESYNC:
 		{
-			parse_data_reader_synctime(DataBuffer, nCurPos);
+			parse_data_reader_synctime(DataBuffer ,nLen ,nCurPos);
 			break;
 		}
 	case CHAR_ADHOC:

classdef.cpp

@@ -56,6 +56,10 @@ Card::Card( string cardid, int cardtype, double z_offset, double offset_x /*= 12
 	is_init_kalman = false;
 	pReaderPathMap = NULL;
 	pTdoaReaderPathMap = NULL;
+
+	m_pKalmanFilter = new CKalmanFilter();
+	m_pKalmanFilter->Initial(1);
+	m_pKalmanFilter->m_bFlag = false;
 }
 
 Card::Card( void )
@@ -257,6 +261,10 @@ int Card::get_effictive_dist_count( int offset /*= 0*/ )
 
 Card::~Card(void)
 {
+	if(m_pKalmanFilter){
+		delete m_pKalmanFilter;
+		m_pKalmanFilter = NULL;
+	}
 }
 
 void Card::get_coordinate_2d( int cnt )
@@ -663,37 +671,61 @@ void Card::get_coordinate( int cnt )
 	if(pTdoaReaderPathMap->size() > 0 && pRdm.size() > 1){
 		p = LocateAlgorithm::Pos(&pRdm,*pTdoaReaderPathMap);
 
-		if(p->posx == INVALID_COORDINATE || p->posy == INVALID_COORDINATE || p->posz == INVALID_COORDINATE){
-			/*p->posx = this->last_x;
-			p->posy = this->last_y;
-			p->posz = this->last_z;*/
-			p->posx = this->last_locate.x;
-			p->posy = this->last_locate.y;
-			p->posz = this->last_locate.z;
-			return;
-		}
-
+		//最新通过算法算出的结果
 		this->x = p->posx / (this->map_scale*1.0);
 		this->y = p->posy / (this->map_scale*1.0);
 		this->z = p->posz / (this->map_scale*1.0);
 
-		this->last_locate.x = this->x;
-		this->last_locate.y = this->y;
+		////通过卡尔曼滤波处理
+		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;
+		double v  = 0;
+
+		if(deltaT != 0){
+			vx = (this->x - this->last_locate.x)/deltaT;
+			vy = (this->y - this->last_locate.y)/deltaT;
+			v  = sqrt(pow(this->x - this->last_locate.x,2) + pow(this->x - this->last_locate.x,2))/deltaT;
+		}
+
+		if(this->m_pKalmanFilter->m_nCounts == 1){
+			this->m_pKalmanFilter->m_pCar->z(0,1) = this->x;
+			this->m_pKalmanFilter->m_pCar->z(1,1) = vx;
+			this->m_pKalmanFilter->m_pCar->z(2,1) = this->y;
+			this->m_pKalmanFilter->m_pCar->z(3,1) = vy;
+			//第一次直接赋值
+			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
+		}else if(this->m_pKalmanFilter->m_nCounts == 2){
+			//两次处理
+			this->m_pKalmanFilter->m_pCar->z(1, 1) = (this->m_pKalmanFilter->m_pCar->z(0, 1) - this->m_pKalmanFilter->m_pCar->x(0, 1))/deltaT;
+			this->m_pKalmanFilter->m_pCar->z(3, 1) = (this->m_pKalmanFilter->m_pCar->z(2, 1) - this->m_pKalmanFilter->m_pCar->x(2, 1))/deltaT;
+			this->m_pKalmanFilter->m_pCar->x = this->m_pKalmanFilter->m_pCar->z;
+		}else{
+			//只有三次以上才允许使用kalman滤波以下的函数
+			if(this->m_pKalmanFilter->m_bFlag){
+				this->m_pKalmanFilter->Predict_Correct();
+			}else{
+				this->m_pKalmanFilter->Predict();
+			}
+		}
+
+		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->v = sqrt(pow(this->m_pKalmanFilter->m_pCar->x(1,1),2) + pow(this->m_pKalmanFilter->m_pCar->x(3,1),2));
+		/*this->last_locate.x = this->x;
+		this->last_locate.y = this->y;
+		this->last_locate.z = this->z;*/
 
 		//this->v = sqrt(pow(this->x - this->last_x,2) + pow(this->y - this->last_y,2) + pow(this->z - this->last_z,2))*this->map_scale;
-		this->v = 0;
+		//this->v = 0;
 		this->z = 0;
 		this->a = 0;
 	}
-	//this->map_scale = 0.02;
 
-	/*this->x = p->posx / (this->map_scale*1.0);
-	this->y = p->posy / (this->map_scale*1.0);
-	this->v = 0;
-	this->z = 0;
-	this->a = 0;*/
-		
 #else
 
 #endif

classdef.h

@@ -32,6 +32,7 @@
 #include <deque>
 //#include "winsock2.h"
 #include <kalman\locate.h>
+#include "Filter\KalmanFilter.h"
 
 #define MIN(x,y) (x < y ? x : y)
 #define MAX(x,y) (x > y ? x : y)
@@ -553,7 +554,7 @@ public:
 	_coordinate** p_dists;
 	_coordinate** p_dists_locate;
 	_coordinate last_locate;
-
+	
 	deque<DistQueItem*> _dists;
 	int time_stamp_max; // 最大时间戳，即需要计算定位的时间戳
 	//int get_pos_state() const { return pos_state; }
@@ -603,6 +604,7 @@ public:
 	//Point2* p2_anchors;
 	Point3* p3_anchors;
 	//bool is_init_kalman;
+	CKalmanFilter* m_pKalmanFilter;
 	//int m_max_stamp; // 最大的时间戳，即最近采集的数据
 	bool is_anchor_changed;
 	void get_coordinate();

global.cpp

@@ -0,0 +1,2 @@
+#pragma once
+#include <Eigen/Dense>

global.h

@@ -0,0 +1,3 @@
+#pragma once
+#include <Eigen/Dense>
+//using namespace Eigen;

locate_algorithm.cpp

@@ -1,4 +1,4 @@
-#include "stdafx.h"
+//#include "stdafx.h"
 #include "locate_algorithm.h"
 #include <math.h>
 #include <algorithm>