def buildInformationMatrix(data, N, num_landmarks, motion_noise, measurement_noise):
# Set the dimension of the filter
dim = 2 * (N + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for n in range(len(data)):
# n is the index of the robot pose in the matrix/vector
measurement = np.array(data[n][0])
motion = np.array(data[n][1])
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
lid=measurement[i][0]
measurements=measurement[i][1:]
m = (N + lid)
# update the information maxtrix/vector based on the measurement
setSubmatrix(Omega,Xi,n,m,1.0 / measurement_noise,measurements/measurement_noise)
# update the information maxtrix/vector based on the robot motion
# next pose id
pid=motion[0]
# delta motino from pose n to pose pid
dMotion=motion[1:]
m=pid # m is always next there is not closeloop
setSubmatrix(Omega,Xi,n,m,1.0/motion_noise,dMotion/motion_noise)
return Omega,Xi
def update(self, measurement):
# measurement matrix
Z = matrix([[measurement[0]], [measurement[1]]])
# measurement transition matrix
H = matrix([[1., 0., 0., 0., 0.], [0., 1., 0., 0., 0.]])
# measurement transition matrix in Jacobian
H_J = matrix([
[1., 0., 0., 0., 0.],
[0., 1., 0., 0., 0.],
])
# measurement noise
R = matrix([[0.1, 0.], [0., 0.1]])
# identity matrix
I = matrix([[1., 0., 0., 0., 0.], [0., 1., 0., 0., 0.],
[0., 0., 1., 0., 0.], [0., 0., 0., 1., 0.],
[0., 0., 0., 0., 1.]])
y = Z - (H * self.x
) # calculate difference in measurement and prediction
S = H_J * self.P * H_J.transpose() + R # calculate Kalman Gain
K = self.P * H_J.transpose() * S.inverse() # calculate Kalman Gain
self.x = self.x + (K * y) # update x
self.P = (I - (K * H_J)) * self.P # update P
def test_forOf(self):
js_code = Unittest('for (elem of [1, 2]) {\n\t5\n}', 'js')
py_code = Unittest('for elem in [1, 2]:\n\t5', 'py')
java_code = Unittest('for (GenericType elem : {1, 2}) {\n\t5;\n}',
'java',
is_input=False)
matrix.matrix(self, [py_code, js_code, java_code])
def test_basic_arrow_anon_functions(self):
js_code = Unittest('let a = () => {\n\t1\n}', 'js')
js_input = Unittest('let a = function() {\n\t1\n}',
'js',
is_output=False)
py_code = Unittest('a = lambda: 1', 'py')
matrix.matrix(self, [js_code, js_input, py_code])
def more_general_fit(y, t1, t2, eps=None):
'''
This routine will fit a function with multiple parameters.
Fits for y = alpha + beta * t1 + gamma * t2
returns our three fit parameters and a covariance matrix
'''
n = len(y)
Y = matrix(y)
X = []
if eps != None:
Err = matrix(eps)
for i in range(n):
X.append([1, t1[i], t2[i]])
X = matrix(X)
Xdagger = X.transpose()
XdagX = Xdagger * X
theta = XdagX.inverse() * Xdagger
theta *= Y
return theta.elements[0], theta.elements[1], theta.elements[2], (
XdagX.inverse())
def test_none(self):
py_code = Unittest('print(None)', 'py')
js_code = Unittest('console.log(null)', 'js')
java_code = Unittest('System.out.println(null);',
'java',
is_input=False)
matrix.matrix(self, [py_code, js_code, java_code])
def test_log_arr_to_code(self):
py_code = Unittest('print([1, 2, 3])', "py")
java_output = Unittest(
'System.out.println(Arrays.toString(new int[] {1, 2, 3}));',
'java',
is_input=False)
matrix.matrix(self, [py_code, java_output])
def test_indent_for_of(self):
py_code = Unittest(
'for j in [1, 2]:\n\tfor k in [3, 4]:\n\t\tj\n\t\tk', 'py')
js_code = Unittest(
'for (j of [1, 2]) {\n\tfor (k of [3, 4]) {\n\t\tj\n\t\tk\n\t}\n}',
'js')
matrix.matrix(self, [py_code, js_code])
def test_for_range_all_args_neg(self):
js_code = Unittest('for (let i = -25; i > -50; i -= 5) {\n\t5\n}',
'js')
py_code = Unittest('for i in range (-25, -50, -5):\n\t5', 'py')
java_code = Unittest('for (int i = -25; i > -50; i -= 5) {\n\t5;\n}',
'java')
matrix.matrix(self, [py_code, js_code, java_code])
def test_arrow_anon_functions_complex(self):
js_code = Unittest('let a = (x, y, z) => {\n\t1\n}', 'js')
js_input = Unittest('let a = function(x, y, z) {\n\t1\n}',
'js',
is_output=False)
py_code = Unittest('a = lambda x, y, z: 1', 'py')
matrix.matrix(self, [js_code, js_input, py_code])
def __init__(self,
x_matrix=matrix([[]]),
P_matrix=matrix([[]]),
time_interval=1):
self.x = x_matrix
self.P = P_matrix
self.dt = time_interval
def test_function_multi_line_body(self):
js_code = Unittest(
'function test(x, y, z) {\n\tconsole.log(x)\n\tconsole.log(y)\n\tconsole.log(z)\n}',
'js')
py_code = Unittest(
'def test(x, y, z):\n\tprint(x)\n\tprint(y)\n\tprint(z)', 'py')
matrix.matrix(self, [py_code, js_code])
def test_for_range_increment_one(self):
js_code = Unittest('for (let i = 0; i < 10; i += 1) {\n\t5\n}', 'js')
py_code = Unittest('for i in range (0, 10, 1):\n\t5', 'py')
java_code = Unittest('for (int i = 0; i < 10; i += 1) {\n\t5;\n}',
'java')
matrix.matrix(self, [py_code, js_code, java_code])
def online_slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise):
# Set the dimension of the filter
dim = 2 * (1 + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for k in range(len(data)):
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (1 + measurement[i][0])
# update the information maxtrix/vector based on the measurement
for b in range(2):
Omega.value[b][b] += 1.0 / measurement_noise
Omega.value[m+b][m+b] += 1.0 / measurement_noise
Omega.value[b][m+b] += -1.0 / measurement_noise
Omega.value[m+b][b] += -1.0 / measurement_noise
Xi.value[b][0] += -measurement[i][1+b] / measurement_noise
Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise
#expand the information matrix and vector ny one new position
list = [0, 1] + range(4, dim+2)
Omega = Omega.expand(dim+2, dim+2, list, list)
Xi = Xi.expand(dim+2, 1, list, [0])
# update the information maxtrix/vector based on the robot motion
for b in range(4):
Omega.value[b][b] += 1.0 / motion_noise
for b in range(2):
Omega.value[b ][b+2] += -1.0 / motion_noise
Omega.value[b+2][b ] += -1.0 / motion_noise
Xi.value[b ][0] += -motion[b] / motion_noise
Xi.value[b+2][0] += motion[b] / motion_noise
# now factor out the previous pose
newlist = range(2, len(Omega.value))
a = Omega.take([0, 1], newlist)
b = Omega.take([0, 1])
c = Xi.take([0, 1], [0])
Omega = Omega.take(newlist) - a.transpose() * b.inverse() * a
Xi = Xi.take(newlist, [0]) - a.transpose() * b.inverse() * c
# compute best estimate
mu = Omega.inverse() * Xi
return mu, Omega
def test_function_no_args(self):
js_code = Unittest('function test() {\n\t2\n}', 'js')
py_code = Unittest('def test():\n\t2', 'py')
java_code = Unittest('public void test() {\n\t2;\n}',
'java',
is_output=False)
matrix.matrix(self, [py_code, js_code, java_code])
def test_java_func_no_arg_no_body(self):
java_input = Unittest('public void test() {}', 'java', is_output=False)
java_output = Unittest(
'class Test {\n\tpublic unknown unknown test() {\n\t\t\n\t}\n}',
'java',
is_input=False)
matrix.matrix(self, [java_input, java_output])
def test_function_one_arg(self):
js_code = Unittest('function test(x) {\n\tconsole.log(x)\n}', 'js')
py_code = Unittest('def test(x):\n\tprint(x)', 'py')
java_code = Unittest(
'class Test {\n\tpublic unknown unknown test(CustomType x) {\n\t\tSystem.out.println(x);\n\t}\n}',
'java',
is_input=False)
matrix.matrix(self, [py_code, js_code, java_code])
def test_arrays(self):
py_code = Unittest('print([[1, 3], [3, 4]])', 'py')
js_code = Unittest('console.log([[1, 3], [3, 4]])', 'js')
java_code = Unittest(
'System.out.println(Arrays.toString(new int[][] {{1, 3}, {3, 4}}));',
'java',
is_input=False)
matrix.matrix(self, [py_code, js_code, java_code])
def test_complicated_function(self):
js_code = Unittest(
'function test(x, y, z) {\n\tconsole.log(x)\n\tconsole.log(y)\n\tconsole.log(z)\n\treturn x + y + z\n}',
'js')
py_code = Unittest(
'def test(x, y, z):\n\tprint(x)\n\tprint(y)\n\tprint(z)\n\treturn x + y + z',
'py')
matrix.matrix(self, [py_code, js_code])
def test_indent_func_if_while_for(self):
py_code = Unittest(
'def test():\n\tif (1):\n\t\twhile (2):\n\t\t\tfor j in [3, 4]:\n\t\t\t\t5',
'py')
js_code = Unittest(
'function test() {\n\tif (1) {\n\t\twhile (2) {\n\t\t\tfor (j of [3, 4]) {\n\t\t\t\t5\n\t\t\t}\n\t\t}\n\t}\n}',
'js')
matrix.matrix(self, [py_code, js_code])
def test_for_range_without_let(self):
js_code = Unittest('for(i=0; i<10; i+=1){\n\t5\n}',
'js',
is_output=False)
py_code = Unittest('for i in range (0, 10, 1):\n\t5',
'py',
is_input=False)
matrix.matrix(self, [js_code, py_code])
def test_indent_if(self):
py_code = Unittest(
'if (x == True):\n\tif (y == True):\n\t\tprint("y and x are true")',
'py')
js_code = Unittest(
'if (x == true) {\n\tif (y == true) {\n\t\tconsole.log("y and x are true")\n\t}\n}',
'js')
matrix.matrix(self, [py_code, js_code])
def test_for_range_without_let_java(self):
java_code = Unittest('for(i=0; i<10; i+=1){\n\t5;\n}',
'java',
is_output=False)
py_code = Unittest('for $$E1$$ in range ($$E0$$, 10, 1):\n\t5',
'py',
is_input=False)
matrix.matrix(self, [py_code, java_code])
def loadViewMatrix(self, x, y, z, center):
M = matrix([[x.x, x.y, x.z, 0], [y.x, y.y, y.z, 0], [z.x, z.y, z.z, 0],
[0, 0, 0, 1]])
O = matrix([[1, 0, 0, -center.x], [0, 1, 0, -center.y],
[0, 0, 1, -center.z], [0, 0, 0, 1]])
self.View = M @ O
def test_indent_for_range(self):
py_code = Unittest(
'for j in range (0, 10, 1):\n\tfor k in range (20, 30, 1):\n\t\tj\n\t\tk',
'py')
js_code = Unittest(
'for (let j = 0; j < 10; j += 1) {\n\tfor (let k = 20; k < 30; k += 1) {\n\t\tj\n\t\tk\n\t}\n}',
'js')
matrix.matrix(self, [py_code, js_code])
def test_if(self):
js_code = Unittest('if (1) {\n\tconsole.log("This is true")\n}', 'js')
py_code = Unittest('if (1):\n\tprint("This is true")', 'py')
bash_code = Unittest('if [[ 1 ]]; then\n\techo "This is true"\nfi',
'bash',
is_input=False)
java_code = Unittest(
'if (1) {\n\tSystem.out.println("This is true");\n}', 'java')
matrix.matrix(self, [py_code, js_code, java_code, bash_code])
def test_java_multiple_args(self):
java_input = Unittest('public void test(int x, String s, int y) {1;}',
'java',
is_output=False)
java_output = Unittest(
'class Test {\n\tpublic unknown unknown test(CustomType x, CustomType s, CustomType y) {\n\t\t1;\n\t}\n}',
'java',
is_input=False)
matrix.matrix(self, [java_input, java_output])
def test_java_func_arg(self):
java_input = Unittest('public void test(int x) {1;}',
'java',
is_output=False)
java_output = Unittest(
'class Test {\n\tpublic unknown unknown test(CustomType x) {\n\t\t1;\n\t}\n}',
'java',
is_input=False)
matrix.matrix(self, [java_input, java_output])
def test_for_with_update_expression_plus(self):
js_code = Unittest('for (let i = 0; i <= 10; i++) {\n\t5\n}',
'js',
is_output=False)
py_code = Unittest('for i in range (0, 11, 1):\n\t5', 'py')
java_code = Unittest('for (int i = 0; i <= 10; i++) {\n\t5;\n}',
'java',
is_output=False)
matrix.matrix(self, [py_code, js_code, java_code])
def test_for_with_update_expression_minus(self):
js_code = Unittest('for (let i = 20; i >= -5; i--) {\n\t5\n}',
'js',
is_output=False)
py_code = Unittest('for i in range (20, -4, -1):\n\t5', 'py')
java_code = Unittest('for (int i = 20; i >= -5; i--) {\n\t5;\n}',
'java',
is_output=False)
matrix.matrix(self, [py_code, js_code, java_code])
def get_matrix( self, order=3 ):
self.__create_matrix()
m = self.matrix
if order==3:
return matrix(data=(m[0][0], m[0][1], m[0][2],
m[1][0], m[1][1], m[1][2],
m[2][0], m[2][1], m[2][2],))
if order==4:
return matrix(data=(m[0][0], m[0][1], m[0][2], 0.0,
m[1][0], m[1][1], m[1][2], 0.0,
m[2][0], m[2][1], m[2][2], 0.0,
0.0,0.0,0.0,1.0))
raise Exception("Get matrix of unsupported order")
def _test(self, measurements, initial_xy, expected_x, expected_P):
ndkf = NDKalmanFilter()
dt = 0.1
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
x, P = ndkf.run2(measurements=measurements,
x=x, P=hw.P, u=u, F=hw.F, H=hw.H, R=hw.R, I=hw.I)
self.assertAlmostEqual(expected_x, x.value, places=0)
self.assertAlmostEqual(expected_P, P.value, places=4)
def slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise):
# Set the dimension of the filter
dim = 2 * (N + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for k in range(len(data)):
# n is the index of the robot pose in the matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (N + measurement[i][0])
# update the information maxtrix/vector based on the measurement
for b in range(2):
Omega.value[n+b][n+b] += 1.0 / measurement_noise
Omega.value[m+b][m+b] += 1.0 / measurement_noise
Omega.value[n+b][m+b] += -1.0 / measurement_noise
Omega.value[m+b][n+b] += -1.0 / measurement_noise
Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise
Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise
# update the information maxtrix/vector based on the robot motion
for b in range(4):
Omega.value[n+b][n+b] += 1.0 / motion_noise
for b in range(2):
Omega.value[n+b ][n+b+2] += -1.0 / motion_noise
Omega.value[n+b+2][n+b ] += -1.0 / motion_noise
Xi.value[n+b ][0] += -motion[b] / motion_noise
Xi.value[n+b+2][0] += motion[b] / motion_noise
# compute best estimate
mu = Omega.inverse() * Xi
return mu
def eye(n, d=None):
""" Creates an identity TT-matrix"""
c = _matrix.matrix()
c.tt = _vector.vector()
if d is None:
n0 = _np.asanyarray(n, dtype=_np.int32)
c.tt.d = n0.size
else:
n0 = _np.asanyarray([n] * d, dtype=_np.int32)
c.tt.d = d
c.n = n0.copy()
c.m = n0.copy()
c.tt.n = (c.n) * (c.m)
c.tt.r = _np.ones((c.tt.d + 1,), dtype=_np.int32)
c.tt.get_ps()
c.tt.alloc_core()
for i in xrange(c.tt.d):
c.tt.core[
c.tt.ps[i] -
1:c.tt.ps[
i +
1] -
1] = _np.eye(
c.n[i]).flatten()
return c
def get_factor(x,y,n,m): c=[] b=[] for k in range(m): c.append([0]) b.append([]) for i in range(n): c[k][0]+=x[i]**k*y[i] for l in range(m): b[k].append(0) for i in range(n): b[k][l]+=x[i]**(k+l) A=matrix.matrix(b) B=matrix.matrix(c) F=bum.simp_it(A,B,0.001) return F
def filter(x,P):
for n in range(len(measurements)):
#measurement update
Z = matrix([measurements[n]]) #doesnt give proper results ...error can be here
k = H*x
Z.show()
y = Z.transpose() - k
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K*y)
P = (I - (K*H)) * P
#prediction
x = (F * x) + u
P = F * P * F.transpose()
print 'x= '
x.show()
print 'P= '
P.show()
def __calculate_matrix_M(sefl, matr):
"""M = D(A), M * (x(k + 1) - x() / tau(k + 1)) + Ax = b"""
matr_m = matrix([])
matr_m.generate_e(matr.cnt_row())
for row in range(matr.cnt_row()):
matr_m.set_elem(row, row, matr.get(row, row))
return matr_m
def extended_kalman_filter(z, x, u, P, F_fn, x_fn, H, R):
"""
Applies extended kalman filter on system
z -> measurement
x -> last state
u -> control vector
P -> covariances
F_fn -> Function that returns F matrix for given 'x'
x_fn -> Updates 'x' using the non-linear derivatives
H -> Measurement matrix
R -> Measurement covariance
"""
I = identity_matrix(x.dimx)
# prediction
F = F_fn(x)
x = x_fn(x) + u
P = F * P * F.transpose()
# measurement update
Z = matrix([z])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
return x, P
def image_synthesis(objects, observer):
tr = rotate(translate(matrix(1), observer.p[:3]), (0, 0, 0), observer[1:])
objects = [obj.transform(tr) for obj in objects]
for ob in objects:
print(ob.points)
return [
obj.polygons for obj in objects if all([all([p.x > d, -Ex <= p.z <= Ex, -Ey <= p.y <= Ey]) for p in obj.points])
]
def _scale(self, mx, scalar):
sx = matrix([[]])
sx.zero(mx.dimx, mx.dimy)
for i in range(mx.dimx):
for j in range(mx.dimy):
sx.value[i][j] = mx.value[i][j] * scalar
return sx
def DBG_Mahalanobis(): from numpy import cov,matrix,linalg #from matrix import matrix from math import sqrt A = [1,3,4,5] B = [3,4,5,6] X = matrix([A]) Y = matrix([B]) cov_matrix=[] for i in range(len(A)): tmp = [] for j in range(len(A)): tmp_cov = cov([A[i],B[j]]) tmp.append(float(tmp_cov)) cov_matrix.append(tmp) cov_matrix = matrix(cov_matrix) C = cov_matrix print X print Y print linalg.det(C)
def metrykaMahalanobisa(self, A, B): from numpy import cov from matrix import matrix cov_matrix=[] for i in cov(A,B): tmp = [] for j in i: tmp.append(j) cov_matrix.append(tmp) cov_matrix = matrix(cov_matrix) print cov_matrix pass #usun to jak cos tu wstawisz
def __init__(self, env):
self.env = env
self.boat_id = self.env.create_boat()
#self.believed_location = self.env.boats[self.boat_id].location # Initial believed location
self.believed_location = 0.0, 0.0
self.believed_heading = 0.0
self.believed_speed = 0.0
self.prev_believed_location = None
self.measured_location = (0.0, 0.0)
self.measured_heading = 0.0
self.measured_rudder = 0.0
self.measured_speed = 0.0
self.relative_wind_angle = 0.0
self.mark_state = environment.mark_state(2, 0)
self.replan = True
self.last_cross_track_error = 0.0
self.int_cross_track_error = 0.0
# Kalman filter variables
self.kalman_u = matrix.matrix([[0.0], [0.0], [0.0], [0.0]]) # external motion
self.kalman_P = matrix.matrix([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0.1, 0.], [0., 0., 0., 10.]]) # initial uncertainty
self.kalman_F = matrix.matrix([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]) # next state function
self.kalman_H = matrix.matrix([[1., 0., 0., 0.], [0., 1., 0., 0.]]) # measurement function
self.kalman_R = matrix.matrix([[0.01, 0.], [0., 0.01]]) # measurement uncertainty
self.kalman_I = matrix.matrix([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # identity matrix
self.igor_target_tack = 1
def kalman(self):
# Convert polar coordinates to cartesian
c = utilsmath.polar_to_cartesian(self.believed_location)
v = utilsmath.polar_to_cartesian((self.believed_speed, self.believed_heading))
m = utilsmath.polar_to_cartesian(self.measured_location)
x = matrix.matrix([[c[0]], [c[1]], [v[0]], [v[1]]])
# prediction
x = (self.kalman_F * x) + self.kalman_u
self.kalman_P = self.kalman_F * self.kalman_P * self.kalman_F.transpose()
# measurement update
Z = matrix.matrix([[m[0]], [m[1]]])
y = Z - (self.kalman_H * x)
S = self.kalman_H * self.kalman_P * self.kalman_H.transpose() + self.kalman_R
K = self.kalman_P * self.kalman_H.transpose() * S.inverse()
x = x + (K * y)
self.kalman_P = (self.kalman_I - (K * self.kalman_H)) * self.kalman_P # prediction
self.believed_location = (utilsmath.cartesian_to_polar((x.value[0][0], x.value[1][0])))
def read_matrix(n,m,f):
i=0
c=[]
while (i<n):
try:
lst = [float(k) for k in f().split()]
if (len(lst)!=m):raise Exception("бла-бла-бла не то кол-во элементов")
except:
if (f!=raw_input):
print("неверный формат файла")
sys.exit()
print("ошибка в последней строчке")
else:
c.append(lst)
i+=1
return matrix.matrix(c)
def queryEntireVolume():
# open("001.csv")
# first line = width
# sec line = height
# third line = depth
# fourth line start with loop
# volume = {"x":"y":"z":"v"}
# volume = dict()
#
#lines = [line.rstrip('\n') for line in open('000.csv')] # this should be a list of the lines without new line character
width = 0
heigth = 0
depth = 0
f = open('/opt/whitematter/data/csv/000.csv', 'r')
# s = open("/var/log/scidbdebug.txt", 'a')
x = 0
y = 0
z = 0
counter = 0
for line in f:
if counter ==0:
width = int(line)###MAYBE DO line.rstrip('\n') for all the line usages
counter = 1
elif counter ==1:
height = int(line)
counter = 2
elif counter ==2:
depth = int(line)
counter = 3
volume = matrix(width, height, depth)
else:
volume[x,y,z] = int(line)
z = (z+1) % depth
if z == 0:
y = (y+1) % height
if y==0 and z==0:
x = (x+1) % width
#counter+=1
#s.write("counter is " + str(counter))
#s.write("\n")
return volume
def measure(self, Z, x, H, R, P, I=None):
"""
Z measurement
H measurement function
R measurement noise
K Kalman gain
P uncertainty covariance
"""
Z = matrix([Z]) # assume measurements are passed in as lists
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
if not I:
I = self._eye(x.dimx) #matrix([[1., 0.], [0., 1.]]) # identity matrix
P = (I - (K * H)) * P
return x, P
def filter(x, P):
for n in range(len(measurements)):
# measurement update
Z = matrix([[measurements[n]]])
y = Z.transpose() - H * x
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - K * H) * P
# prediction
x = F * x + u
P = F * P * F.transpose()
print 'x= '
x.show()
print 'P= '
P.show()
def filter(x, P):
for n in range(len(measurements)):
# prediction
x = (F * x) + u
P = F * P * F.transpose()
# measurement update
Z = matrix([measurements[n]])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
print "x= "
x.show()
print "P= "
P.show()
def __init__(self):
self.measurement = None
self.x = matrix([[0.0] for i in range(5)])
self.f = lambda x: matrix(
[[x[0] + x[3] * cos(x[2] + x[4])], [x[1] + x[3] * sin(x[2] + x[4])], [x[2] + x[4]], [x[3]], [x[4]]]
)
self.h = lambda x: matrix([[x[0]], [x[1]]])
# [ 1, 0, -s*sin(dt + t), cos(dt + t), -s*sin(dt + t)]
# [ 0, 1, s*cos(dt + t), sin(dt + t), s*cos(dt + t)]
# [ 0, 0, 1, 0, 1]
# [ 0, 0, 0, 1, 0]
# [ 0, 0, 0, 0, 1]
self.F = lambda x: matrix(
[
[1.0, 0.0, -x[3] * sin(x[2] + x[4]), cos(x[2] + x[4]), -x[3] * sin(x[2] + x[4])],
[0.0, 1.0, x[3] * cos(x[2] + x[4]), sin(x[2] + x[4]), x[3] * cos(x[2] + x[4])],
[0.0, 0.0, 1.0, 0.0, 1.0],
[0.0, 0.0, 0.0, 1.0, 0.0], # th = th+dth all others invariants , f is linear, F is const
[0.0, 0.0, 0.0, 0.0, 1.0],
]
)
self.H = lambda x: matrix([[1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0]])
self.P = matrix(
[
[1000.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1000.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1000.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1000.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1000.0],
]
)
self.R = matrix([[15.0, 0.0], [0.0, 15.0]]) # set noise to 5
self.I = matrix([[1 if i == j else 0.0 for j in range(5)] for i in range(5)])
def use_seidel(self, matr, vect, precision, iterations, silent, interval):
"""Main method of class seidel, returns solution of Ax=b,
params: A --> matr, b --> vect, precision --> effect on number of steps,
iterations --> max number of steps"""
if not matr.is_square():
raise Exception('Coefficient matrix is not square')
if matr.cnt_row() != vect.size():
raise Exception('Different size of matrix and vector')
dim = vect.size()
matr, vect = self.__transform(matr, vect)
upper_triangular, lower_triangular = matr.diagonal_split()
ed_matr = matrix()
ed_matr.generate_e(matr.cnt_row())
ed_matr.minus(lower_triangular)
rev = ed_matr.find_lower_square_reverse()
kth_step_matr = rev.multiply(upper_triangular)
vect = rev.multiply(vect)
cur_approx = vector(vect)
next_approx = vector(vect)
self.__eps = precision
statistics = [[],[]]
if kth_step_matr.determinant() > 1:
raise Exception('Matrix determinant > 1')
for iteration_num in range(iterations):
next_approx = kth_step_matr.multiply(cur_approx)
next_approx.add(vect)
is_accurate, error = self.__check_precision(cur_approx, next_approx, precision)
if is_accurate:
if interval != 0:
statistics[0].append(iteration_num + 1)
statistics[1].append(error)
if not silent:
print('Total number of iterations = ' + str(iteration_num))
break
if interval != 0 and iteration_num % interval == 0:
statistics[0].append(iteration_num + 1)
statistics[1].append(error)
cur_approx = vector(next_approx)
return statistics, next_approx
def update(self, measurement):
# predict
F = self.F(self.x)
self.x = self.f(self.x)
self.P = F * self.P * F.transpose()
z = matrix([[measurement[i]] for i in range(len(measurement))])
H = self.H(self.x)
y = z - self.h(self.x)
S = H * self.P * H.transpose() + self.R
try:
Si = S.inverse()
except ValueError as v:
print (S)
raise v
K = self.P * H.transpose() * Si
self.x = self.x + K * y
self.P = (self.I - K * H) * self.P
return self.x, self.P
def test_filter(self):
measurements = [[1.], [2.], [3.]] # locations
x = matrix([[0.], [0.]]) #initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # intial external motion
F = matrix([[1., 1.], [0., 1.]]) # new state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
ndkf = NDKalmanFilter()
x, P = ndkf.run(measurements=measurements,
x=x, P=P, u=u, F=F, H=H, R=R)
expected_x = [[4.0], [1.0]]
expected_P = [[2.3319, 0.9992],
[0.9992, 0.4995]]
self.assertAlmostEqual(expected_x, x.value, places=0)
self.assertAlmostEqual(expected_P, P.value, places=4)
def identity_matrix(n):
res = matrix([[]])
res.identity(n)
return res
print_help()
exit(0)
else:
matr_list = []
vect_list = []
try:
if 'input_file' in params:
matr_list, vect_list = io_lib.read(params['input_file'], params['input_type'],
params['separator'], matr_list, vect_list, True)
else:
matr_list, vect_list = io_lib.read('', params['input_type'],
params['separator'], matr_list, vect_list, False)
except Exception as error_msg:
print(error_msg)
exit(1)
matr = matrix(matr_list)
vect = vector(vect_list)
deltatime = 0
try:
if params['method'] == 'SEIDEL':
solver = seidel()
deltatime = datetime.now()
statistics, solution = solver.use_seidel(matr, vect, precision, iterations, silent, interval)
deltatime = datetime.now() - deltatime
else:
solver = implicit()
deltatime = datetime.now()
statistics, solution = solver.use_implicit(matr, vect, precision, iterations, silent, interval)
deltatime = datetime.now() - deltatime
except Exception as error_msg:
print(error_msg)
def doit(initial_pos, move1, move2, Z0, Z1, Z2):
# initial_position:
Omega = matrix([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]])
Xi = matrix([[initial_pos],
[0.0],
[0.0]])
# move1
Omega += matrix([[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[0.0, 0.0, 0.0]])
Xi += matrix([[-move1],
[move1],
[0.0]])
# move2
Omega += matrix([[0.0, 0.0, 0.0],
[0.0, 1.0, -1.0],
[0.0, -1.0, 1.0]])
Xi += matrix([[0.0],
[-move2],
[move2]])
# expand
Omega = Omega.expand(Omega.dimx + 1, Omega.dimy + 1, range(Omega.dimx))
Xi = Xi.expand(Xi.dimx + 1, Xi.dimy, range(Xi.dimx), [0])
# first measurement:
Omega += matrix([[1.0, 0.0, 0.0, -1.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 1.0]])
Xi += matrix([[-Z0],
[0.0],
[0.0],
[Z0]])
Omega += matrix([[0.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, -1.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 1.0]])
Xi += matrix([[0.0],
[-Z1],
[0.0],
[Z1]])
# third measurement:
Omega += matrix([[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 5.0, -5.0],
[0.0, 0.0, -5.0, 5.0]])
Xi += matrix([[0.0],
[0.0],
[-5*Z2],
[5*Z2]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Mu: ')
return mu
def matrix_fill_in(initial_pos, move_sigma, move1, move2, measure_sigma, Z0, Z1, Z2):
# initial position:
Omega = matrix([[1.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0]])
Xi = matrix([[initial_pos],
[0.0],
[0.0],
[0.0],
[0.0]])
# move 1:
Omega += matrix([[1.0/move_sigma, -1.0/move_sigma, 0.0, 0.0, 0.0],
[-1.0/move_sigma, 1.0/move_sigma, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0]])
Xi += matrix([[-move1/move_sigma],
[move1/move_sigma],
[0.0],
[0.0],
[0.0]])
# move 2:
Omega += matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1.0/move_sigma, -1.0/move_sigma, 0.0, 0.0],
[0.0, -1.0/move_sigma, 1.0/move_sigma, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0]])
Xi += matrix([[0.0],
[-move2/move_sigma],
[move2/move_sigma],
[0.0],
[0.0]])
# measure 0
Omega += matrix([[1.0/measure_sigma, 0.0, 0.0, -1.0/measure_sigma, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[-1.0/measure_sigma, 0.0, 0.0, 1.0/measure_sigma, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0]])
Xi += matrix([[-Z0/measure_sigma],
[0.0],
[0.0],
[Z0/measure_sigma],
[0.0]])
# measure 1
Omega += matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1.0/measure_sigma, 0.0, 0.0, -1.0/measure_sigma],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, -1.0/measure_sigma, 0.0, 0.0, 1.0/measure_sigma]])
Xi += matrix([[0.0],
[-Z1/measure_sigma],
[0.0],
[0.0],
[Z1/measure_sigma]])
# measure 2
Omega += matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0/measure_sigma, 0.0, -1.0/measure_sigma],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0/measure_sigma, 0.0, 1.0/measure_sigma]])
Xi += matrix([[0.0],
[0.0],
[-Z2/measure_sigma],
[0.0],
[Z2/measure_sigma]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Mu: ')
return mu
from matrix import matrix
measurements = [1,2,3]
x = matrix([[0.],[0.]]) #initial state (location and velocity)
P = matrix([[1000.,0.],[0.,1000.]]) #initial uncertainity
u = matrix([[0.],[0.]]) #external motion
F = matrix([[1.,1.],[0,1.]]) #next state function
H = matrix([[1.,0]]) #measurement function
R = matrix([[1.]]) #measurement uncertainty
I = matrix([[1.,0.],[0.,1.]])
def filter(x,P):
for n in range(len(measurements)):
#measurement update
Z = matrix([[measurements[n]]])
y = Z.transpose() - (H*x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K*y)
P = (I - (K*H)) * P
#prediction
## Test Case 1
answer_mu = matrix([
## Estimated Pose(s):
[49.998897453299165], [49.998505706587814],
[37.9714477943764 ], [33.64993445823509 ],
[26.183449863995392], [18.153338459791925],
[13.743443839776688], [2.1141193319706257],
[28.095060682659934], [16.78089653056425 ],
[42.382654337758865], [30.899617637854934],
[55.82918373374959 ], [44.494287384838586],
[70.85548928962663 ], [59.69712516287841 ],
[85.6953531635832 ], [75.54007207500423 ],
[74.00974406829611 ], [92.43147558585063 ],
[53.54264788322474 ], [96.45111370814985 ],
[34.52341231228876 ], [100.07762713840204],
[48.621309970082486], [83.95097871134821 ],
[60.19521941022714 ], [68.1050904555393 ],
[73.77592978594885 ], [52.932451315943574],
[87.12997140410576 ], [38.53576961176431 ],
[80.3007799395094 ], [20.505859780712917],
[72.79656231764604 ], [2.942675607797428 ],
[55.243590482706026], [13.253021809459227],
[37.41439688492312 ], [22.31502245240636 ],
## Estimated Landmarks:
[82.95425645256424 ], [13.536536707427121],
[70.49306062345174 ], [74.13913606764582 ],
[36.73812342335858 ], [61.2789905549488 ],
[18.696326102039485], [66.05733561281015 ],
[20.632945056999347], [16.87255837889543]
])
test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]]
