From b4572dc1cddb8d489d4ec6c63c825e1e91e90fd1 Mon Sep 17 00:00:00 2001
From: Aaryansh7 <aaryanshmb7@gmail.com>
Date: Sat, 25 Apr 2020 22:21:08 +0530
Subject: [PATCH] added kalman filter

---
 controls/scripts/check_kf_filter.py |  43 +++++++++++
 controls/scripts/kalman_filter.py   | 112 ++++++++++++++++++++++++++++
 controls/scripts/kalman_filter.pyc  | Bin 0 -> 4319 bytes
 3 files changed, 155 insertions(+)
 create mode 100755 controls/scripts/check_kf_filter.py
 create mode 100755 controls/scripts/kalman_filter.py
 create mode 100644 controls/scripts/kalman_filter.pyc

diff --git a/controls/scripts/check_kf_filter.py b/controls/scripts/check_kf_filter.py
new file mode 100755
index 0000000..61910d8
--- /dev/null
+++ b/controls/scripts/check_kf_filter.py
@@ -0,0 +1,43 @@
+#! /usr/bin/python
+print("node started")
+
+import numpy as np
+import rospy
+from geometry_msgs.msg import Pose
+from underwater_sensor_msgs.msg import DVL 
+
+
+import kalman_filter
+
+rospy.init_node('check', anonymous=True)
+
+print("imported kalman_filter")
+
+X=np.zeros((6,1))
+U=np.ones(1)
+
+cur_vel=np.array([0.0,0.0,0.0])
+cur_pos=np.array([0.0,0.0,0.0])
+def vel_callback(msg):
+		cur_vel =np.array([msg.bi_x_axis, msg.bi_y_axis, msg.bi_z_axis])
+					
+			
+def pose_callback(msg):
+		cur_pos=np.array([msg.position.x, msg.position.y, msg.position.z])
+
+
+
+def main():
+	sub_pose=rospy.Subscriber('g500/pose',Pose,pose_callback,queue_size=1)
+	sub_vel=rospy.Subscriber('g500/dvl',DVL,vel_callback,queue_size=1)
+	kf=kalman_filter.Kalman_filter(X,U,cur_vel,cur_pos)
+	print("sensor data")
+	print(cur_pos)
+	kf.print_data()
+	print("result")
+	print(kf.kf_main())
+
+while not rospy.is_shutdown():
+	print("entering main")
+	main()
+	rospy.Rate(100).sleep()
diff --git a/controls/scripts/kalman_filter.py b/controls/scripts/kalman_filter.py
new file mode 100755
index 0000000..6e5c3d2
--- /dev/null
+++ b/controls/scripts/kalman_filter.py
@@ -0,0 +1,112 @@
+#!/usr/bin/python
+
+from numpy import *
+from numpy.linalg import inv
+from numpy.linalg import det
+import numpy as np
+
+
+class Kalman_filter:
+	def __init__(self,X,U,cur_vel,cur_pos):
+		self.X=X
+		self.row_main=6
+		self.col_main=1
+		self.cur_vel=cur_vel
+		self.cur_pos=cur_pos
+		self.dt=0.01
+		self.loop_iter=50
+
+		###initialization of state matrices
+		self.P=np.eye(self.X.shape[0])
+		#set covariance in diagonal elements of P(by default setting it to 0)
+		for i in range(6):
+			self.P[i][i]=0
+
+		self.A=np.eye(self.X.shape[0])
+		for i in range(3):
+			self.A[i][i+3]=self.dt
+
+		#set covariance in diagonal elements of Q(by defualt setting is 0)
+		self.Q=np.eye(self.X.shape[0])
+		for i in range(6):
+			self.Q[i][i]=0
+
+		self.B=np.ones((6,1))
+		for i in range(3):
+			self.B[i][0]=0.5*self.dt*self.dt
+			self.B[i+3][0]=self.dt
+
+		self.U=U
+
+		###Measurement matrices
+		self.Y=np.zeros((6,1))
+		for i in range(3):
+			self.Y[i][0]=self.cur_pos[i]
+			self.Y[i+3][0]=self.cur_vel[i]
+
+		self.H=self.A
+
+		self.R=np.eye(self.X.shape[0])
+
+	
+	def print_data(self):
+		print("checking of correct sensor data")
+		print(self.Y)
+	
+
+	#velocity subscriber callback
+	def vel_callback(self,msg):
+		self.cur_vel =np.array([msg.bi_x_axis, msg.bi_y_axis, msg.bi_z_axis])
+					
+			
+	#postion subscriber callback	
+	def pose_callback(self,msg):
+		self.cur_pos=np.array([msg.position.x, msg.position.y, msg.position.z])
+
+	#Algorithm of Kalman_filter
+	def kf_predict(self):
+		for i in range(6):
+			self.B[i]=self.B[i]*self.U
+		self.X=np.dot(self.A,self.X)+self.B
+		self.P=np.dot(self.A,np.dot(self.P,self.A.T)) + self.Q
+		return(self.X,self.P)		
+
+	def kf_update(self):
+		IM=np.dot(self.H,self.X)
+		IS=self.R+np.dot(self.H,np.dot(self.P,self.H.T))
+		K=np.dot(self.P,np.dot(self.H.T,inv(IS)))
+		self.X=self.X+np.dot(K,(self.Y-IM))
+		self.P=self.P-np.dot(K,np.dot(IS,K.T))
+		LH=self.gauss_main(self.Y,IM,IS)
+		return (self.X,self.P,K,IM,IS,LH)
+
+	def gauss_main(self,X,M,S):
+		if M.shape[1]==1:
+			DX=X-np.tile(M,X.shape[1])
+			E=0.5*np.sum(DX*(np.dot(inv(S),DX)),axis=0)
+			E=E+0.5*M.shape[0]*log(2-pi) + 0.5*log(det(S))
+			P=np.exp(-E)
+
+		elif X.shape[1]==1:
+			DX=np.tile(X,M.shape[1])-M
+			E=0.5*np.sum(DX*(np.dot(inv(S),DX)),axis=0)
+			E=E+0.5*M.shape[0]*log(2-pi) + 0.5*log(det(S))
+			P=np.exp(-E)
+
+		else:
+			DX=X-M
+			E=0.5*np.dot(DX.T,dot(inv(S),DX))
+			E=E+0.5*M.shape[0]*log(2-pi) + 0.5*log(det(S))
+			P=np.exp(-E)
+
+		return(P[0],E[0])
+
+	
+
+	def kf_main(self):
+		for i in range(self.loop_iter):
+			(self.X,self.P)=self.kf_predict()
+			(self.X,self.P,K,IM,IS,LH)=self.kf_update()
+
+		return (self.X)
+
diff --git a/controls/scripts/kalman_filter.pyc b/controls/scripts/kalman_filter.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..617d0d0a06ab41ccd182bbea1ddb55cb3e9154c3
GIT binary patch
literal 4319
zcmc&%ZEqA+6h3!myX}@%3SDTc1q#Nv^0q>VCRnW^h9c776azHkWSE_9r`@;COkrEw
zPw*p*pZ#KD{6{AE8~mX0i$2e}`vy@yt#P|^_MAKSo_p?d-frFBCddE0{KGe)bUrSA
zKgN^Z2C3j{q$YBgoFg?Obw}15F_c}EU0HLjoa4;3oK!}nGAfn4JQH~;@?Z^ePF71}
z&=~)PPsxDGNX}Z8o5qdDeW4O%E7>GQZw0k_(D2veS{5ZeBf0b%p7ah#giQ@i#da_a
z29t4VFB#b5Dx+3#Bu@*;bd6jaqrn_0h-d8#N1!>6hrgfVNpp_KzL9+qUr`~0Z5<r}
zb(I0lDWltRj&4gPFWIPUj`8g#z6Dw__ta_}m-btd9is6Vx&ClQ4zO-S=Z?!hCgNG#
z-jI9$u46*l=T%4E>NrUq+|1I6t{OOIW^wzcSE-tOg*lU5J&W7FTOF@SWsF<$kcXvx
zYtV=f^&q1iJBrpE{W+|9aK^>HjXFmA<~Y86$=utw_)aNyN2F5dZgf=I%Yz$T?Vc9!
z6E2+l=nFf*GlS#(UB1UIm<@bj|MV-IRAi8?mc|hcD=}juW*k<(E&hDt*S%*;@nsI-
zk(lvZB`5NHsf56h{`{B4V|c5%a6e;$B+Vy&J%}4Bgw2{2M)4-xN&LrA4FO>JRx{10
zx{_rD{M4GwmLDTabg<FNXhgIdWsKl-BWOjs%N>=Ipi#B*Cp!9Nmcy)OBT7}grt~YF
zx)&wbh$nrZbXloaA^^H7jcV&2mwQDLn&<H(IxjAP6D27#w!G1-N97<$c7sN`Q4V$<
z`%ltxnuO&niL+*-95x$S(yU=DOyX9SmbV5gz{S?C0^|E}BhGxEzDVEW7N#)x=gvFR
z=9n{Yrp>%dddxWrddkc@1()2cS#TztkjLsin&n3@fF!)J%#aZLs>=!UN_q-3+=#-h
zxKUkbt}lemB#FXoA&nYoGg+tvS)dinJC2E#n<Iy3xCrpntpqXRbI&V~QsOUW#Dp}J
z4+r!_+&;r$ke^02kxrn<1<;&S-EKOe+Ay|>#frMtJH&O2oghhqT@A5EvETNCcAR#l
z-4~=il{5e}$UUB3!?2!KhmXPpv?Cw<FsRiY1>x3J$SIF5|A4|J3=g8vfxsaO78jJs
zJ{)KfeK<6t35R!rTE;+-dvO*cR0&vH>8{c}4gLP{z0;U5{Ijz)!iK1K_$3T<4^Nwj
zL$7sx2ccc&`Mdq)v>a%?wo#$g(h)7o8YYX|lPK-3f`ibp4FqjPi!s*BuvDU-4b15C
z-C~bchXtKA1-r(+%WFFrm30RpM@wxv)ynD}hU`s~6iGA;S;{KS%$wo}BinloL;=6+
zF=Yn;f1M(p;iufzy5CBoN*rdISj<AvqH@MuF<MH9IP0Rm{{u`X<ba`t4*BMEq0hbW
zwm;LNjmT8MF@NU38JIdU!0RM51awLCUyJsEUPKVz9gSARwZq6PmC<!}tf#3r7e>OR
z-F;^{)ml?fkEs&w<;sEHa+pOnKENPyus$d2`CcI*MCw709`R;K)P1w$=tJ)v5=N|6
z&5SFm!A_cL-zyPNTSgcrH*afiySb{Kzoo(#%fojz+bNvA(?WTTK8KLfR6*0YWG<LD
z%{g!(&%w&j^Y<?}1rXTOESp27Z8?(sNL*kp{f{pg`x>5@1%|jj@+HtiG-ydi$%PMi
zo66ClMO<Dh;EXBqHit5;il`+gbHz;WvWS3LtSHV<9nunhQWZLeWz-B0j>sX59OR{O
z#`G=e80wjhmFmX6m9YAMG<3HURs$v=@N%*_&b!A;SP`z_fwT{30Wl%RkCrOBdc|J)
zz~S?JNpC=GiT2{hC@pELV1>5Di-D5GwMaW*x>L8gz>wFGTC=Kw+=@M}*0HGF(!?lf
z>tP$!w1J02ZYy2YHP`Q}@R_F4(DT4MhwWDJq$fdG>Q0!ldEZRqZq?!3aZ|#`l)Xt2
zpD{dI{vLv9ud)iUM0EbIxz;UyC?X<XBbK|>$hQS$M8~y3*eZo_ls^<TWDA_TJtcZu
zcY8I4o7$!^PVOv8uSIz8LbUf8MUl<HyGU}L<N`?<Wa!xGEgdz4S9v6pq{Kzcaa4|y
zqm8a}k8K3?$oIAG`F_1w*{MPF-oRvUp5zS4+a!82o=5%SeE_a+wsJ$dVS6rAvj(Q@
zw|P@ITgVqi3c12)p-`BydlGfM9X59AxRdiPrrgC^+z4t_ThSTMw)|7$e`Bn|xM+d<
Oh}$UA<7Ntf6Yk$?D;q2T

literal 0
HcmV?d00001