添加陀螺factor ，

Author: jack_yang

com_d.py

@@ -18,6 +18,7 @@ def com_init():
   com= serial.Serial('COM3',115200,timeout=0)
   return com  
 
+  
 
 def get_num(input):
   val=0
@@ -79,6 +80,7 @@ def get_data(com):
       time.sleep(0.0005)
     # time.sleep(0.001)
   data = com.readall()
+
   #return 
   #return hexShow(data)
   return np.append(decode_acc(data),[decode_gyro(data),decode_erula(data)])

ekf_filter.py

@@ -74,7 +74,7 @@ def __init__(self):
 		self.measure_dm =6
 		self.init_flag = 0
 
-		self.qq=0.045 ##
+		self.qq=0.0045 ##
 		self.qw=0.025
 
 
@@ -121,6 +121,7 @@ def __init__(self):
 		self.X=np.zeros(self.status_dm)
 		self.Y=np.zeros(self.measure_dm)
 		self.K=np.zeros((self.status_dm,self.measure_dm))
+		self.factor = 16.4*180.0/np.pi
 	def filter_init(self,gyro,accel):
 		nedVector = np.array([0, 0 , -1.0])
 		accelVector=np.zeros(3)
@@ -166,9 +167,9 @@ def filter_update(self,gyro,accel,dt=0.002):
 		halfdt =dt*0.5
 		### 0.5*t(w - wbias)
 		##self.X[4:6]存储的是w的
-		halfdx = halfdt * (gyro[0] - self.X[4])
-		halfdy = halfdt * (gyro[1] - self.X[5])
-		halfdz = halfdt * (gyro[2] - self.X[6])
+		halfdx = halfdt * (gyro[0]/self.factor - self.X[4])
+		halfdy = halfdt * (gyro[1]/self.factor - self.X[5])
+		halfdz = halfdt * (gyro[2]/self.factor - self.X[6])
 
 		neghalfdx = -halfdx
 		neghalfdy = -halfdy

madgwick_filter.py

@@ -1,7 +1,7 @@
 
 # -*- coding: utf-8 -*-
 from math  import sqrt,atan2,asin
-from numpy  import  pi
+from numpy  import  pi,array
 
 def Sqr(x):
 	return x**2
@@ -17,7 +17,7 @@ def qa_to_erula_angle(q):
 		num=1.0
 	PitchAngle = asin(num)
 	YawAngle = atan2(2.0*q1*q2 - 2.0*q0*q3,  2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
-	return [PitchAngle,RollAngle,YawAngle]
+	return array([PitchAngle,RollAngle,YawAngle])
 ###def madgwick_filter
 class madgwick_filter(object):
 	"""docstring for ClassName"""

main.py

@@ -12,39 +12,50 @@
 filter =mdf.madgwick_filter(0.4) 
 '''
 import ekf_filter as ekf
-filter =ekf.ekf_filter()
+filter1 = ekf.ekf_filter()
+
+import madgwick_filter as mdf
+filter2 =mdf.madgwick_filter(0.1) 
 
 
 import time as tm
 time_str = tm.strftime('%Y-%m-%d-%H-%M-%S',tm.localtime(tm.time()))
 file_name='arhs-'+time_str+'.txt'
 
-#fo = open(file_name, "wb")
+fo = open(file_name, "wb")
 while 1:
 	data = ser.get_data(com_port)
 	acc=data[:3]
 	gyro=data[3:6]
 	erula_cal=data[6:]
 	#print acc,gyro
 	#tm.sleep (1)
-	if (filter.init_flag ==0):
-		filter.filter_init(gyro,acc)
-		filter.init_flag=1	
+	if (filter1.init_flag ==0):
+		filter1.filter_init(gyro,acc)
+		filter1.init_flag=1	
 	else:
-		data=filter.filter_update(gyro,acc,0.001)
-	
+		data=filter1.filter_update(gyro,acc,0.001)
+	   
 		erual =data*180/np.pi
 		stra=' '
 		stra+=str(erual[0]) +' '
 		stra+=str(erual[1]) +' '
 		stra+=str(erual[2]) +' '
 
+
+
+		data=filter2.filter_update(gyro,acc)
+		erual =data*180/np.pi
+		stra+=str(erual[0]) +' '
+		stra+=str(erual[1]) +' '
+		stra+=str(erual[2]) +' '
+         
 		stra+=str(erula_cal[0]) +' '
 		stra+=str(erula_cal[1]) +' '
 		stra+=str(erula_cal[2]) +' '
 		stra+='\n'
 		print stra
-		#fo.write(stra);
+		fo.write(stra);
 
 
 

test.py

@@ -18,9 +18,11 @@
 # norm2 = normalize(x[:,np.newaxis], axis=0).ravel()
 # print np.all(norm1 == norm2)
 
-
 import numpy as np
-b=np.ones((7,6))
-c =np.ones((6,7))
-a = np.dot(b,c)
-print a.shape
+# b=np.ones((7,6))
+# c =np.ones((6,7))
+# a = np.dot(b,c)
+# print a.shape
+stra ="-0.094727,0.008301,1.003906,0.011706,-0.006385,0.007449"
+a=np.fromstring(stra, dtype=float, sep=', ')
+print a