depth first search

#!/usr/bin/python
"""
Author: Jeremy M. Stober
Program: KALMAN.PY
Description: A simple KF for following a random walk with noisy observations.
"""
from numpy import *
import pylab
import numpy.random as npr
from matplotlib.path import Path
from numpy.linalg import inv
mean = zeros(2)
acov = eye(2) * 5.0
scov = eye(2) * 5.0
 
A = eye(2)
B = eye(2)
C = eye(2)
P = eye(2)
 
x = zeros(2)
u = zeros(2)
z = zeros(2)
e = zeros(2)
xm = zeros(2)
 
# change up the action every once and awhile
def choose_action(u,k):
    if k % 10 == 0:
        return npr.uniform(-1,1,2)
    else:
        return u
 
def noisy_update_state(x,u):
    noise = npr.multivariate_normal(mean,acov)
    return dot(A,x) + dot(B,u) + noise
 
def noisy_read_state(x):
    noise = npr.multivariate_normal(mean,scov)
    return dot(C,x) + noise
 
def simulate(x,u,k):
    u = choose_action(u,k)
    x = noisy_update_state(x,u)
    z = noisy_read_state(x)
    return (x,z,u)
 
xhist = [x]
zhist = [z]
uhist = [u]
ehist = [e]
Phist = [P]
xmhist = [xm]
 
for k in range(100):
 
    # run simulation
    (x,z,u) = simulate(x,u,k)
    xhist.append(x)
    zhist.append(z)
    uhist.append(u)
 
    # run filter
 
    # step one - pre-observation
    xm = dot(A,e) + dot(B,u)
    Pm = dot(A,dot(P,transpose(A))) + acov
    xmhist.append(xm)
 
    # step two - compute kalman gain
    T = dot(Pm, transpose(C))
    S = dot(C,dot(Pm,transpose(C))) + scov
    K = dot(T,inv(S))
 
    # step three - compute state estimate
    obs = z - dot(C,xm)
    e = xm + dot(K,obs)
    P = dot(eye(2) - dot(K,C),Pm)
    ehist.append(e)
 
 
xmmse = 0.0
emse = 0.0
zmse = 0.0
cnt = 0
 
for i in range(len(ehist)):
    xmmse += sum((xhist[i] - xmhist[i])**2)
    emse += sum((xhist[i] - ehist[i])**2)
    zmse += sum((xhist[i] - zhist[i])**2)
    cnt += 1
 
print xmmse,cnt
print "XM: %f" % (xmmse / cnt)
print "E: %f" % (emse / cnt)
print "Z: %f" % (zmse / cnt)
 
 
pylab.figure()
xhist = array(xhist)
pylab.plot(xhist[:,0],xhist[:,1], color='red', label='ground truth')
zhist = array(zhist)
pylab.scatter(zhist[:,0],zhist[:,1], color='blue',marker='+', label='observations')
ehist = array(ehist)
pylab.scatter(ehist[:,0],ehist[:,1],color='green',marker='x', label='state estimates')
xmhist = array(xmhist)
pylab.scatter(xmhist[:,0],xmhist[:,1],color='purple',marker='o', label='predictions')
pylab.scatter
pylab.legend()
pylab.show()