---
# System prepended metadata

title: DATA Fusion Skill
tags: [機器人學]

---

DATA Fusion Skill
===
###### tags: `機器人學`
# Kalman Filter
![](https://i.imgur.com/QnLJodZ.png)
want to know: 
 - [ ] multi input 
 - [x] - [ ] how about drift

# EKF 
need Jacobian
我的策略：直接用微小誤差去微分 算出值

# UKF
![](https://i.imgur.com/bcvflJQ.png)

``` python
#UKF test
from scipy.linalg import sqrtm
import numpy as np
import sys
reload(sys)
sys.setdefaultencoding('utf-8')
import matplotlib.pyplot as plt
class UKF():
	def __init__(self):
		self.x = 0
		self.n_state = 0
		self.n_sig = 0
		self.y_measure = 0
		self.landa = 0
		self.alpha = 0
		self.beta = 0
		self.Q = 0
		self.R = 0
		self.p_x = 0
		self.p_yy = 0
		self.p_xy = 0
		self.W_m = 0
		self.W_c = 0
		self.n_sig = 0
	def param_update(self,n_state,n_output,landa,alpha,beta,Q,R,p_x):
		self.landa = landa
		self.alpha = alpha
		self.beta = beta
		self.Q = Q
		self.R = R
		self.p_x = p_x
		self.n_state = n_state
		self.n_sig = n_state*2+1
		self.n_output = n_output
		########### Get value of Weight ########3
		self.W_m = np.zeros(self.n_sig)
		for i in range (self.n_sig):
			if i == 0:
				self.W_m[i] = self.landa/(self.n_state+self.landa)
			else:
				self.W_m[i] = 1/(2*(self.n_state+self.landa))
		self.W_c= np.zeros(self.n_sig)
		for i in range (self.n_sig):
			if i == 0:
				self.W_c[i] = self.landa/((self.n_state+self.landa)+(1-self.alpha**2+self.beta))
			else:
				self.W_c[i] = 1/(2*(self.n_state+self.landa))
	def F(self,x):
		return np.array([x[0,:]+1,x[1,:]+2])
	def update(self,x,y_measure):
			#######################################
		###             predict X       #######
		#######################################
		
		######### unscentrate transform #########
		######### Get sigma of X        #########
		r = sqrtm(self.n_state*self.p_x)
		x = x.T
		X = x

		for i in range(self.n_state):
			X = np.vstack((X,x+r[i]))
			X = np.vstack((X,x-r[i]))
		X = X.T
		#print X
		####### Get prediction  of X(k) by X(k-1) ########
		X_pre=self.F(X)
		#print('X(k) is')
		#print(X_pre)
		####### Get mean of sigma             ########
		X_pre_mean = np.zeros((self.n_state,1))
		for i in range(self.n_state):
			for j in range(self.n_sig):
				X_pre_mean[i,0] += X_pre[i,j]*self.W_m[j]
		#print('X_pre_mean is')
		#print(X_pre_mean)
		####### X sigma minus X mean            ########
		X_diff = np.zeros((self.n_state,self.n_sig))
		for i in range(self.n_sig):
			X_diff.T[i] = X_pre.T[i]-X_pre_mean.T
		#print('X diff is')
		#print(X_diff)
		####### Get covarienc of X       ########
		p_k = np.zeros((self.n_state,self.n_state))
		for i, val in enumerate(np.array_split(X_diff, self.n_sig, 1)):
			p_k += val.dot(val.T)*self.W_c[i]
		p_k += self.Q
		#print('p_k is')
		#print(p_k)
		#######################################
		#####         Correction         ######
		#######################################

		##########   predict Y by X  ##########
		Y_pre = np.zeros((self.n_output,self.n_sig))
		Y_pre[0,:]=(X[0,:]**2+X_pre[1,:]**2)**0.5
		Y_pre[1,:]= np.arctan(X_pre[0,:]/X_pre[1,:])
		#print('Y_pre is')
		#print(Y_pre) 
		##########  get  Y mean  ##########
		Y_pre_mean = np.zeros((self.n_output,1)) 
		for i in range(self.n_output):
			for j in range(self.n_sig):
				Y_pre_mean[i,0] += self.W_m[j]*Y_pre[i,j]
		#print('Y_pre_mean is')
		#print(Y_pre_mean)
		####### Y sigma minus y mean            ########
		Y_pre_diff = np.zeros((self.n_output,self.n_sig))
		for i in range(self.n_sig):
			Y_pre_diff.T[i] = Y_pre.T[i]-Y_pre_mean.T
		#print('Y_pre diff is: \n'+str(Y_pre_diff))
		####### Get covarienc of y       ########
		p_yy = np.zeros((self.n_state,self.n_state))
		for i, val in enumerate(np.array_split(Y_pre_diff, self.n_sig, 1)):
			self.p_yy += val.dot(val.T)*self.W_c[i]
		#print('self.p_yy is')
		#print(self.p_yy)
		####### Get covarienc of xy       ########
		p_xy = np.zeros((self.n_state,self.n_state))
		for i, val in enumerate(zip(np.array_split(Y_pre_diff,self.n_sig,1),np.array_split(Y_pre_diff,self.n_sig,1))):
			p_xy += self.W_m[i]*val[0].dot(val[1].T)

		####### Get Gain       ########
		K_k = p_xy.dot(np.linalg.inv(p_yy+R))
		#print('K_k is:')
		#print K_k


		####### Result       ########
		x_final = X_pre_mean+K_k.dot(y_measure - Y_pre_mean)
		#print ('X final is')
		#print (x_final)

		self.p_x = p_k- K_k.dot(p_yy+R).dot(K_k.T)
		#print ('P final is:')
		#print (self.p_x)
		return x_final

if __name__ == '__main__':
	np.random.seed(1234)
	s1 = np.random.normal(0, 2, 201)
	s2 = np.random.normal(0, 2, 201)
	###############################################
	####                param           ###########
	###############################################

	########### State and OUTPUT measurement
	x = np.array([[0.001],[0.001]])
	n_state = np.size(x)
	for i in range (200):
		x = np.hstack((x,[[x[0,i]+1+s1[i]],[x[1,i]+2+s1[i]]]))
	print x
	x_shape = np.shape(x)
	n_sig = n_state*2+1
	n_output = 2
	landa = 0.
	alpha = 1e-3
	beta = 2.

	y = np.zeros((n_output,x_shape[1]))
	y[0,:]=(x[0,:]**2+x[1,:]**2)**0.5
	y[1,:]= np.arctan(x[0,:]/x[1,:])
	#print y
	######### Get preliminary covarience ########

	Q = np.array([[0.01,0],[0,0.01]])
	R = np.array([[1,0],[0,1]])
	p_x = np.array([[1.44,0.], [0., 2.89]])

	ukf1 = UKF()
	ukf1.param_update(n_state,n_output,landa,alpha,beta,Q,R,p_x)
	data_x1 = [0]
	data_x2 = [0] 
	data_origin1 = [0]
	data_origin2 = [0] 
	for i,var in enumerate(zip(np.array_split(x,201,1),np.array_split(y,201,1))):
		#print i
		if i > 0:
			#print var_last
			x_opt = ukf1.update(x_last,var[1])
			print 'haha',var[0] ,x_opt
			data_x1= np.append(data_x1,x_opt[0,0])
			data_x2= np.append(data_x2,x_opt[1,0])
			data_origin1= np.append(data_origin1,var[0][0])
			data_origin2= np.append(data_origin2,var[0][1])
			x_last = x_opt
		else: 
			x_last = var[0]
	plt.plot(range(201),data_origin1,label='x_origin1')
	plt.plot(range(201),data_x1,label='x1')
	plt.plot(range(201),data_origin2,label='x_origin2')
	plt.plot(range(201),data_x2,label='x2')
	#print data_x1
	#print data_x2
	plt.legend()
	plt.show()
 	

```
Result
![](https://i.imgur.com/aLIniH2.png)



