Skip to content

Commit

Permalink
Nebw tests merged
Browse files Browse the repository at this point in the history
  • Loading branch information
@fatroom
fatroom committed Feb 29, 2012
1 parent bcdce91 commit 67cdb28
Show file tree
Hide file tree
Showing 4 changed files with 102 additions and 393 deletions.
72 changes: 27 additions & 45 deletions homework2.6/task.py
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,51 +137,33 @@ def __repr__(self):

########################################

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()
return x,P

########################################

print "### 4-dimensional example ###"
def calculate(measurements, initial_xy):
dt = 0.1

measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
initial_xy = [4., 12.]
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion

# measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]
# initial_xy = [-4., 8.]
### fill this in: ###
P = # initial uncertainty
F = # next state function
H = # measurement function
R = # measurement uncertainty
I = # identity matrix

# measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]
# initial_xy = [1., 19.]

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

### fill this in: ###
P = # initial uncertainty
F = # next state function
H = # measurement function
R = # measurement uncertainty
I = # identity matrix

def calculate():
return filter(x, P)
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
return x, P
return filter(x, P)
167 changes: 75 additions & 92 deletions homework2.6/test.py
100755 → 100644
View file
Original file line number Diff line number Diff line change
@@ -1,102 +1,85 @@

import unittest
import task
from task import matrix

class TestSequenceFunctions(unittest.TestCase):
def failUnlessArraysAlmostEqual(self, first, second, places=7, msg=None):
"""Fail if the two arrays are unequal as determined by their
difference rounded to the given number of decimal places
(default 7) and comparing to zero.
Note that decimal places (from zero) are usually not the same
as significant digits (measured from the most signficant digit).
"""
if (len(first) != len(second)):
raise self.failureException, \
def failUnlessArraysAlmostEqual(self, first, second, places=7, msg=None):
"""Fail if the two arrays are unequal as determined by their
difference rounded to the given number of decimal places
(default 7) and comparing to zero.
Note that decimal places (from zero) are usually not the same
as significant digits (measured from the most signficant digit).
"""
if (len(first) != len(second)):
raise self.failureException, \
(msg or '%r != %r because they have unequal lengths %d & %d', \
(first, second, len(first), len(second)))

for i in range(len(first)):
if isinstance(first[i], list):
self.failUnlessArraysAlmostEqual(first[i], second[i], places, msg)
elif round(abs(second[i]-first[i]), places) != 0:
raise self.failureException, \
for i in range(len(first)):
if isinstance(first[i], list):
self.failUnlessArraysAlmostEqual(first[i], second[i], places, msg)
elif round(abs(second[i]-first[i]), places) != 0:
raise self.failureException, \
(msg or '%r != %r within %r places' % (first, second, places))

# Synonym methods
assertArrayAlmostEqual = assertArrayAlmostEquals = failUnlessArraysAlmostEqual

def test_dataset1(self):
# ARRANGE
expected_x = [[9.99934], [0.00131], [9.99890], [-19.9978]]
expected_P = [[0.03955, 0.0, 0.06592, 0.0],
[0.0, 0.03955, 0.0, 0.06592],
[0.06592, 0.0, 0.10987, 0.0],
[0.0, 0.06592, 0.0, 0.10987]]


task.measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
task.initial_xy = [4., 12.]

# ACT
x, P = task.calculate()
# ASSERT
#self.assertArrayAlmostEquals(expected_x, actual_result)
#self.assertArrayAlmostEquals(expected_P, actual_result)

def test_dataset2(self):
# ARRANGE
expected_x = [[15.99333], [-7.99466], [49.98333], [-39.98667]]
# expected_P = [[0.03955, 0.0, 0.06592, 0.0],
# [0.0, 0.03955, 0.0, 0.06592],
# [0.06592, 0.0, 0.10987, 0.0],
# [0.0, 0.06592, 0.0, 0.10987]]


task.measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]
task.initial_xy = [-4., 8.]

# ACT
x, P = task.calculate()
# ASSERT
#self.assertArrayAlmostEquals(expected_x, actual_result)
#self.assertArrayAlmostEquals(expected_P, actual_result)

def test_dataset3(self):
# ARRANGE
expected_x = [[1.0], [11.00266], [0.0], [-19.99333]]
expected_P = [[0.05331, 0.0, 0.13328, 0.0],
[0.0, 0.05331, 0.0, 0.13328],
[0.13328, 0.0, 0.33322, 0.0],
[0.0, 0.13328, 0.0, 0.33322]]


task.measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]
task.initial_xy = [1., 19.]

# ACT
x, P = task.calculate()
# ASSERT
#self.assertArrayAlmostEquals(expected_x, actual_result)
#self.assertArrayAlmostEquals(expected_P, actual_result)


def test_dataset4(self):
# ARRANGE
expected_x = [[0.73342], [11.00266], [-0.66644], [-19.99333]]
expected_P = [[0.05331, 0.0, 0.13328, 0.0],
[0.0, 0.05331, 0.0, 0.13328],
[0.13328, 0.0, 0.33322, 0.0],
[0.0, 0.13328, 0.0, 0.33322]]


task.measurements = [[2., 17.], [0., 15.], [2., 13.], [0., 11.]]
task.initial_xy = [1., 19.]

# ACT
x, P = task.calculate()
# ASSERT
#self.assertArrayAlmostEquals(expected_x, actual_result)
#self.assertArrayAlmostEquals(expected_P, actual_result)

# Synonym methods
assertArrayAlmostEqual = assertArrayAlmostEquals = failUnlessArraysAlmostEqual

def test_dataset1(self):
# ARRANGE
measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
initial_xy = [4., 12.]
expected_x = matrix([[9.9993407317877168],[0.001318536424568617],[9.9989012196461928],[-19.997802439292386]])
expected_P = matrix([[0.039556092737061982, 0.0, 0.06592682122843721, 0.0], [0.0, 0.039556092737061982, 0.0, 0.06592682122843721], [0.065926821228437182, 0.0, 0.10987803538073201, 0.0], [0.0, 0.065926821228437182, 0.0, 0.10987803538073201]])

# ACT
x, P = task.calculate(measurements, initial_xy)

# ASSERT
self.assertArrayAlmostEquals(expected_x.value, x.value)
self.assertArrayAlmostEquals(expected_P.value, P.value)

def test_dataset2(self):
# ARRANGE
measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]
initial_xy = [-4., 8.]
expected_x = matrix([[15.993335554815062], [-7.9946684438520501], [49.983338887037647], [-39.986671109630123]])

# ACT
x, P = task.calculate(measurements, initial_xy)

# ASSERT
self.assertArrayAlmostEquals(expected_x.value, x.value)

def test_dataset3(self):
# ARRANGE
measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]
initial_xy = [1., 19.]
expected_x = matrix([[1.0], [11.002665778073975], [0.0], [-19.993335554815054]])
expected_P = matrix([[0.053315561479506911, 0.0, 0.13328890369876803, 0.0], [0.0, 0.053315561479506911, 0.0, 0.13328890369876803], [0.13328890369876789, 0.0, 0.33322225924692717, 0.0], [0.0, 0.13328890369876789, 0.0, 0.333222259246027171]])

# ACT
x, P = task.calculate(measurements, initial_xy)

# ASSERT
self.assertArrayAlmostEquals(expected_x.value, x.value)
self.assertArrayAlmostEquals(expected_P.value, P.value)

def test_dataset4(self):
# ARRANGE
measurements = [[2., 17.], [0., 15.], [2., 13.], [0., 11.]]
initial_xy = [1., 19.]
expected_x = matrix([[0.73342219260246477], [11.002665778073975], [-0.66644451849384057], [-19.993335554815054]])
expected_P = matrix([[0.053315561479506911, 0.0, 0.13328890369876803, 0.0], [0.0, 0.053315561479506911, 0.0, 0.13328890369876803], [0.13328890369876789, 0.0, 0.33322225924692717, 0.0], [0.0, 0.13328890369876789, 0.0, 0.333222259246027171]])

# ACT
x, P = task.calculate(measurements, initial_xy)

# ASSERT
self.assertArrayAlmostEquals(expected_x.value, x.value)
self.assertArrayAlmostEquals(expected_P.value, P.value)

if __name__ == '__main__':
unittest.main()
unittest.main()
Loading

0 comments on commit 67cdb28

Please sign in to comment.