Kalman Filter for Euler Angles and Position data
I had to implement my own filter, Nexus does not have options for a real time filter (although we use 7 markers on the HMD, Nexus 2 in kinematic fit mode will use all available markers on the segment to compute the best fit position and orientation, so the more markers the smoother the result). I'll share the basics here, in case anyone is interested. First, I want to give credit where it is due, most of my code is adapted from this example: http://arxiv.org/ftp/arxiv/papers/1204/1204.0375.pdf
My model is basically to assume that at each iteration, the orientation and position has not changed from the last iteration. This is a really simple model and easy to implement and has some nice characteristics (no overshoot or ripple). I tuned the filter's smoothing and time-lag response by altering the model and measurement noise covariance parameters (not sure if this is the legitimate way to tune a Kalman filter).
Here is the code:
Code:
import viz
import oculus
import pykalman
import numpy as np
#this is setup to filter six states, or variables: <X,Y,Z,Yaw,Pitch,Roll>
global X #k-1 timestep state estimate initiated as zeros
X = np.array([[0],[0],[0],[0],[0],[0]],dtype=np.float)
global P #state covariance prediction
P = np.diag((0.01,0.01,0.01,0.01,0.01,0.01))
global A #system model matrix
A = np.eye(6,dtype=np.float)
xshape = X.shape
dq = 0.35#noise covariance of euler angles in the model
dqq = 0.5#noise covariance of position data in the model
global Q #process noise covariance
Q = np.array([[dq,0,0,0,0,0],[0,dq,0,0,0,0],[0,0,dq,0,0,0],[0,0,0,dqq,0,0],[0,0,0,0,dqq,0],[0,0,0,0,0,dqq]],dtype=np.float)
global B #system input matrix
B = np.eye(xshape[0])
du = 0.0001#measurement noise covariance of euler angles
duu = 0.0001#noise covariance of position measurements
global U #measurement noise covariance
U = np.array([[du],[du],[du],[duu],[duu],[duu]])
global Y #measurement array
Y = np.array([[0],[0],[0],[0],[0],[0]],dtype=np.float)
yshape = Y.shape
global H #measurement matrix
H = np.eye(yshape[0])
global R #measurement covariance
R = np.eye(yshape[0])
def kf_predict(X,P,A,Q,B,U):#predict the current iterations' state
X = np.dot(A, X) + np.dot(B, U)
P = np.dot(A, np.dot(P, A.T)) + Q
return(X,P)
def gauss_pdf(X, M, S):
mshape = M.shape
xshape = X.shape
if mshape[1] == 1:
DX = X - np.tile(M, xshape[1])
E = 0.5 * np.sum(DX * (np.dot(np.linalg.inv(S), DX)), axis=0)
E = E + 0.5 * mshape[0] * np.log(2 * np.pi) + 0.5 * np.log(np.linalg.det(S))
P = np.exp(-E)
elif xshape[1] == 1:
DX = tile(X, mshape[1])- M
E = 0.5 * np.sum(DX * (np.dot(np.linalg.inv(S), DX)), axis=0)
E = E + 0.5 * mshape[0] * np.log(2 * np.pi) + 0.5 * np.log(np.linalg.det(S))
P = np.exp(-E)
else:
DX = X-M
E = 0.5 * np.dot(DX.T, np.dot(np.linalg.inv(S), DX))
E = E + 0.5 * mshape[0] * np.log(2 * np.pi) + 0.5 * np.log(np.linalg.det(S))
P = np.exp(-E)
return (P[0],E[0])
def kf_update(X,P,Y,H,R):#use the predicted state and current measurement to determine the current iterations' state
IM = np.dot(H, X)
IS = R + np.dot(H, np.dot(P, H.T))
K = np.dot(P, np.dot(H.T, np.linalg.inv(IS)))
X = X + np.dot(K, (Y-IM))
P = P - np.dot(K, np.dot(IS, K.T))
LH = gauss_pdf(Y, IM, IS)
return (X,P,K,IM,IS,LH)
#program loop running in real time
while ~stop:
global X
global P
global A
global Q
global B
global U
global Y
global H
global R
#Y = ?? update your measurement array
#example, Y.itemset(0,Yaw)
#run it through the filter
(X,P) = kf_predict(X,P,A,Q,B,U)
(X,P,K,IM,IS,LH) = kf_update(X,P,Y,H,R)
#now use the filtered states in X to update the display
navigationNode.setEuler(-1*float(X[2])*180/math.pi,float(X[0])*180/math.pi,float(X[1])*180/math.pi)#kalman filtered
navigationNode.setPosition([-1*float(X[3]),float(X[5]),-1*float(X[4])]) #Nexus is Right handed coordinates, Vizard is Left handed...
|