Wrote some experimental code.

experiements/fusion.py

@@ -0,0 +1,66 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Fri Feb 13 17:47:56 2015
+
+@author: rlabbe
+"""
+import numpy as np
+from filterpy.kalman import  UnscentedKalmanFilter as UKF
+from math import atan2, radians,degrees
+from filterpy.common import stats
+import matplotlib.pyplot as plt
+
+p = (-10, -10)  
+
+def hx(x):
+    dx = x[0] - hx.p[0]
+    dy = x[1] - hx.p[1]    
+    return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5])
+    
+def fx(x,dt):
+    return x
+    
+
+
+kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.)
+
+kf.x = np.array([100, 100.])
+kf.P *= 40
+hx.p = kf.x - np.array([50,50])
+
+d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
+
+stats.plot_covariance_ellipse(
+       kf.x, cov=kf.P, axis_equal=True, 
+       facecolor='y', edgecolor=None, alpha=0.6)
+plt.scatter([100], [100], c='y', label='Initial')
+
+kf.R[0,0] = radians (1)**2
+kf.R[1,1] = 2.**2
+
+
+kf.predict()
+kf.update(np.array([radians(45), d]))
+
+print(kf.x)
+print(kf.P)
+
+stats.plot_covariance_ellipse(
+       kf.x, cov=kf.P, axis_equal=True, 
+       facecolor='g', edgecolor=None, alpha=0.6)
+plt.scatter([100], [100], c='g', label='45 degrees')
+       
+       
+p = (13, -11)
+hx.p = kf.x - np.array([-50,50])
+d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
+
+kf.predict()
+kf.update(np.array([radians(135), d]))
+
+stats.plot_covariance_ellipse(
+       kf.x, cov=kf.P, axis_equal=True, 
+       facecolor='b', edgecolor=None, alpha=0.6)
+plt.scatter([100], [100], c='b', label='135 degrees')
+
+plt.legend(scatterpoints=1, markerscale=3)

experiements/ukf_baseball.py

@@ -0,0 +1,214 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Feb  8 09:55:24 2015
+
+@author: rlabbe
+"""
+
+from math import radians, sin, cos, sqrt, exp, atan2, radians
+from numpy import array, asarray
+from numpy.random import randn
+import numpy as np
+import math
+import matplotlib.pyplot as plt
+from filterpy.kalman import UnscentedKalmanFilter as UKF
+from filterpy.common import runge_kutta4
+
+
+
+
+
+
+
+class BaseballPath(object):
+    def __init__(self, x0, y0, launch_angle_deg, velocity_ms,
+                 noise=(1.0,1.0)):
+        """ Create 2D baseball path object
+           (x = distance from start point in ground plane,
+            y=height above ground)
+
+        x0,y0            initial position
+        launch_angle_deg angle ball is travelling respective to
+                         ground plane
+        velocity_ms      speeed of ball in meters/second
+        noise            amount of noise to add to each position
+                         in (x,y)
+        """
+
+        omega = radians(launch_angle_deg)
+        self.v_x = velocity_ms * cos(omega)
+        self.v_y = velocity_ms * sin(omega)
+
+        self.x = x0
+        self.y = y0
+
+        self.noise = noise
+
+
+    def drag_force(self, velocity):
+        """ Returns the force on a baseball due to air drag at
+        the specified velocity. Units are SI
+        """
+        B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
+        return B_m * velocity
+
+
+    def update(self, dt, vel_wind=0.):
+        """ compute the ball position based on the specified time
+        step and wind velocity. Returns (x,y) position tuple
+        """
+
+        # Euler equations for x and y
+        self.x += self.v_x*dt
+        self.y += self.v_y*dt
+
+        # force due to air drag
+        v_x_wind = self.v_x - vel_wind
+        v = sqrt(v_x_wind**2 + self.v_y**2)
+        F = self.drag_force(v)
+
+        # Euler's equations for velocity
+        self.v_x = self.v_x - F*v_x_wind*dt
+        self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt
+
+        return (self.x, self.y)
+
+
+
+radar_pos = (100,0)
+omega = 45.
+
+
+def radar_sense(baseball, noise_rng, noise_brg):
+    x, y = baseball.x, baseball.y
+
+    rx, ry = radar_pos[0], radar_pos[1]
+
+    rng = ((x-rx)**2 + (y-ry)**2) ** .5
+    bearing = atan2(y-ry, x-rx)
+
+    rng += randn() * noise_rng
+    bearing += radians(randn() * noise_brg)
+
+    return (rng, bearing)
+
+
+ball = BaseballPath(x0=0, y0=1, launch_angle_deg=45,
+                    velocity_ms=60, noise=[0,0])
+
+
+'''
+xs = []
+ys = []
+dt = 0.05
+y = 1
+
+while y > 0:
+    x,y = ball.update(dt)
+    xs.append(x)
+    ys.append(y)
+
+plt.plot(xs, ys)
+plt.axis('equal')
+
+
+plt.show()
+
+'''
+
+
+dt = 1/30.
+
+
+def hx(x):
+    global radar_pos
+
+    dx = radar_pos[0] - x[0]
+    dy = radar_pos[1] - x[2]
+    rng = (dx*dx + dy*dy)**.5
+    bearing = atan2(-dy, -dx)
+
+    #print(x)
+    #print('hx:', rng, np.degrees(bearing))
+
+    return array([rng, bearing])
+
+
+
+
+def fx(x, dt):
+
+    fx.ball.x = x[0]
+    fx.ball.y = x[2]
+    fx.ball.vx = x[1]
+    fx.ball.vy = x[3]
+
+    N = 10
+    ball_dt = dt/float(N)
+
+    for i in range(N):
+        fx.ball.update(ball_dt)
+
+    #print('fx', fx.ball.x, fx.ball.v_x, fx.ball.y, fx.ball.v_y)
+
+    return array([fx.ball.x, fx.ball.v_x, fx.ball.y, fx.ball.v_y])
+
+
+fx.ball = BaseballPath(x0=0, y0=1, launch_angle_deg=45,
+                       velocity_ms=60, noise=[0,0])
+
+
+y = 1.
+x = 0.
+theta = 35. # launch angle
+v0 = 50.
+
+
+ball = BaseballPath(x0=x, y0=y, launch_angle_deg=theta,
+                    velocity_ms=v0, noise=[.3,.3])
+
+kf = UKF(dim_x=4, dim_z=2, dt=dt, hx=hx, fx=fx, kappa=0)
+
+#kf.R *= r
+
+kf.R[0,0] = 0.1
+kf.R[1,1] = radians(0.2)
+omega = radians(omega)
+vx = cos(omega) * v0
+vy = sin(omega) * v0
+
+kf.x = array([x, vx, y, vy])
+kf.R*= 0.01
+#kf.R[1,1] = 0.01
+kf.P *= 10
+
+f1 = kf
+
+
+t = 0
+xs = []
+ys = []
+
+while y > 0:
+    t += dt
+    x,y = ball.update(dt)
+    z = radar_sense(ball, 0, 0)
+    #print('z', z)
+    #print('ball', ball.x, ball.v_x, ball.y, ball.v_y)
+
+
+    f1.predict()
+    f1.update(z)
+    xs.append(f1.x[0])
+    ys.append(f1.x[2])
+    f1.predict()
+
+    p1 = plt.scatter(x, y, color='r', marker='o', s=75, alpha=0.5)
+
+p2, = plt.plot (xs, ys, lw=2, marker='o')
+#p3, = plt.plot (xs2, ys2, lw=4)
+#plt.legend([p1,p2, p3],
+#           ['Measurements', 'Kalman filter(R=0.5)', 'Kalman filter(R=10)'],
+#           loc='best', scatterpoints=1)
+plt.show()
+

experiements/ukf_range.py

@@ -0,0 +1,111 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Feb  8 09:34:36 2015
+
+@author: rlabbe
+"""
+
+from numpy import array, asarray
+from numpy.random import randn
+import math
+from filterpy.kalman import UnscentedKalmanFilter as UKF
+
+
+
+class RadarSim(object):
+    """ Simulates the radar signal returns from an object flying
+    at a constant altityude and velocity in 1D.
+    """
+
+    def __init__(self, dt, pos, vel, alt):
+        self.pos = pos
+        self.vel = vel
+        self.alt = alt
+        self.dt = dt
+
+    def get_range(self):
+        """ Returns slant range to the object. Call once for each
+        new measurement at dt time from last call.
+        """
+
+        # add some process noise to the system
+        self.vel = self.vel  + .1*randn()
+        self.alt = self.alt + .1*randn()
+        self.pos = self.pos + self.vel*self.dt
+
+        # add measurment noise
+        err = self.pos * 0.05*randn()
+        slant_dist = math.sqrt(self.pos**2 + self.alt**2)
+
+        return slant_dist + err
+
+
+
+dt = 0.05
+
+
+def hx(x):
+    return (x[0]**2 + x[2]**2)**.5
+    pass
+
+
+
+def fx(x, dt):
+    result = x.copy()
+    result[0] += x[1]*dt
+    return result
+
+
+
+
+f = UKF(3, 1, dt= dt, hx=hx, fx=fx, kappa=1)
+radar = RadarSim(dt, pos=-1000., vel=100., alt=1000.)
+
+f.x = array([0, 90, 1005])
+f.R = 0.1
+f.Q *= 0.002
+
+
+
+
+xs = []
+track = []
+
+for i in range(int(20/dt)):
+    z = radar.get_range()
+    track.append((radar.pos, radar.vel, radar.alt))
+
+    f.predict()
+    f.update(array([z]))
+
+    xs.append(f.x)
+
+
+xs = asarray(xs)
+track = asarray(track)
+time = np.arange(0,len(xs)*dt, dt)
+
+plt.figure()
+plt.subplot(311)
+plt.plot(time, track[:,0])
+plt.plot(time, xs[:,0])
+plt.legend(loc=4)
+plt.xlabel('time (sec)')
+plt.ylabel('position (m)')
+
+
+plt.subplot(312)
+plt.plot(time, track[:,1])
+plt.plot(time, xs[:,1])
+plt.legend(loc=4)
+plt.xlabel('time (sec)')
+plt.ylabel('velocity (m/s)')
+
+plt.subplot(313)
+plt.plot(time, track[:,2])
+plt.plot(time, xs[:,2])
+plt.ylabel('altitude (m)')
+plt.legend(loc=4)
+plt.xlabel('time (sec)')
+plt.ylim((900,1600))
+plt.show()