Test code to perform KF on ball in vacuum.
This commit is contained in:
Roger Labbe
parent
7d7d500106
commit
40debbd003
@ -41,8 +41,6 @@ def rk4(y, x, dx, f):
|
||||
dx is the difference in x (e.g. the time step)
|
||||
f is a callable function (y, x) that you supply to compute dy/dx for
|
||||
the specified values.
|
||||
|
||||
|
||||
"""
|
||||
|
||||
k1 = dx * f(y, x)
|
||||
@ -52,82 +50,73 @@ def rk4(y, x, dx, f):
|
||||
|
||||
return y + (k1 + 2*k2 + 2*k3 + k4) / 6
|
||||
|
||||
def fy(y, t):
|
||||
""" returns velocity of ball at time t. """
|
||||
return -9.8*t
|
||||
|
||||
|
||||
def fx(y, t):
|
||||
""" returns velocity of ball. Need to set vx.vel prior to first call"""
|
||||
|
||||
|
||||
|
||||
def fx(x,t):
|
||||
return fx.vel
|
||||
|
||||
|
||||
|
||||
def fx2(x,t):
|
||||
return fx2.vel
|
||||
|
||||
def fy2(y,t):
|
||||
return fy2.vel - 9.8*t
|
||||
|
||||
def fy(y,t):
|
||||
return fy.vel - 9.8*t
|
||||
|
||||
|
||||
class BallRungeKutta(object):
|
||||
def __init__(self, x=0, y=100., vel=10., omega = 0.0):
|
||||
|
||||
self.omega = math.radians(omega)
|
||||
self.vx = math.cos(self.omega) * vel
|
||||
self.vy = math.sin(self.omega) * vel
|
||||
def __init__(self, x=0, y=100., vel=10., omega = 0.0):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.t = 0
|
||||
|
||||
fx2.vel = self.vx
|
||||
fy2.vel = self.vy
|
||||
omega = math.radians(omega)
|
||||
|
||||
fx.vel = math.cos(omega) * vel
|
||||
fy.vel = math.sin(omega) * vel
|
||||
|
||||
def step (self, dt):
|
||||
self.x = rk4 (self.x, self.t, dt, fx2)
|
||||
self.y = rk4 (self.y, self.t, dt, fy2)
|
||||
self.t += dt
|
||||
|
||||
print self.x, self.y
|
||||
self.x = rk4 (self.x, self.t, dt, fx)
|
||||
self.y = rk4 (self.y, self.t, dt, fy)
|
||||
self.t += dt
|
||||
|
||||
|
||||
|
||||
def ball_scipy(y0, vel, omega, dt):
|
||||
|
||||
vel_y = math.sin(math.radians(omega)) * vel
|
||||
|
||||
def f(t,y):
|
||||
return vel_y-9.8*t
|
||||
|
||||
solver = ode(f).set_integrator('dopri5')
|
||||
solver.set_initial_value(y0)
|
||||
|
||||
ys = [y0]
|
||||
while brk.y >= 0:
|
||||
t += dt
|
||||
brk.step (dt)
|
||||
|
||||
ys.append(solver.integrate(t))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
dt = 1./30
|
||||
y0 = 15.
|
||||
vel = 100.
|
||||
omega = 10.
|
||||
omega = 0.
|
||||
vel_y = math.sin(math.radians(omega)) * vel
|
||||
print vel_y
|
||||
|
||||
def f(t,y):
|
||||
#print t,y
|
||||
return vel_y-9.8*t
|
||||
|
||||
be = BallEuler (y=y0, vel=vel)
|
||||
brk = BallRungeKutta (y=y0, vel=vel, omega=omega)
|
||||
|
||||
|
||||
solver = ode(f).set_integrator('dopri5')
|
||||
solver.set_initial_value(y0)
|
||||
t = 0
|
||||
y = y0
|
||||
ball_rk = BallRungeKutta (y=y0, vel=vel, omega=omega)
|
||||
|
||||
|
||||
while be.y >= 0:
|
||||
be.step (dt)
|
||||
#plt.scatter (be.x, be.y, color='red')
|
||||
ball_rk.step(dt)
|
||||
|
||||
plt.scatter (be.x, be.y, color='red')
|
||||
|
||||
|
||||
while brk.y >= 0:
|
||||
t += dt
|
||||
brk.step (dt)
|
||||
|
||||
y = solver.integrate(t)
|
||||
|
||||
|
||||
plt.scatter (brk.x, brk.y, color='blue', marker='v')
|
||||
plt.scatter (brk.x, y[0], color='green', marker='+')
|
||||
plt.axis('equal')
|
||||
plt.scatter (ball_rk.x, ball_rk.y, color='blue', marker='v')
|
||||
#plt.scatter (brk.x, y[0], color='green', marker='+')
|
||||
#plt.axis('equal')
|
||||
|
||||
95
exp/ball.py
Normal file
95
exp/ball.py
Normal file
@ -0,0 +1,95 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Sat Jul 5 16:07:29 2014
|
||||
|
||||
@author: rlabbe
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from KalmanFilter import KalmanFilter
|
||||
from math import radians, sin, cos
|
||||
import numpy.random as random
|
||||
import matplotlib.markers as markers
|
||||
|
||||
class BallPath(object):
|
||||
def __init__(self, x0, y0, omega_deg, velocity, g=9.8, noise=[1.0,1.0]):
|
||||
omega = radians(omega_deg)
|
||||
self.vx0 = velocity * cos(omega)
|
||||
self.vy0 = velocity * sin(omega)
|
||||
|
||||
self.x0 = x0
|
||||
self.y0 = y0
|
||||
|
||||
self.g = g
|
||||
self.noise = noise
|
||||
|
||||
def pos_at_t(self, t):
|
||||
""" returns (x,y) tuple of ball position at time t"""
|
||||
x = self.vx0*t + self.x0
|
||||
y = -0.5*self.g*t**2 + self.vy0*t + self.y0
|
||||
|
||||
return (x +random.randn()*self.noise[0], y +random.randn()*self.noise[1])
|
||||
|
||||
|
||||
|
||||
|
||||
y = 15
|
||||
x = 0
|
||||
omega = 0.
|
||||
noise = [1,1]
|
||||
v0 = 100.
|
||||
ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
|
||||
t = 0
|
||||
dt = 1
|
||||
g = 9.8
|
||||
|
||||
|
||||
f1 = KalmanFilter(dim=6)
|
||||
dt = 1/30. # time step
|
||||
|
||||
ay = -.5*dt**2
|
||||
|
||||
f1.F = np.mat ([[1, dt, 0, 0, 0, 0], # x=x0+dx*dt
|
||||
[0, 1, dt, 0, 0, 0], # dx = dx
|
||||
[0, 0, 0, 0, 0, 0], # ddx = 0
|
||||
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
|
||||
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
|
||||
[0, 0, 0, 0, 0, 1]]) # ddy = -g
|
||||
f1.B = 0.
|
||||
|
||||
f1.H = np.mat([
|
||||
[1, 0, 0, 0, 0, 0],
|
||||
[0, 0, 0, 1, 0, 0]])
|
||||
|
||||
f1.R = np.eye(2) * 5
|
||||
f1.Q = np.eye(6) * 0.
|
||||
|
||||
omega = radians(omega)
|
||||
vx = cos(omega) * v0
|
||||
vy = sin(omega) * v0
|
||||
|
||||
f1.x = np.mat([x,vx,0,y,vy,-9.8]).T
|
||||
|
||||
f1.P = np.eye(6) * 500.
|
||||
|
||||
|
||||
|
||||
|
||||
z = np.mat([[0,0]]).T
|
||||
count = 0
|
||||
markers.MarkerStyle(fillstyle='none')
|
||||
|
||||
np.set_printoptions(precision=4)
|
||||
while f1.x[3,0] > 0:
|
||||
count += 1
|
||||
#f1.update (z)
|
||||
f1.predict()
|
||||
print f1.x[0,0], f1.x[3,0]
|
||||
#markers.set_fillstyle('none')
|
||||
plt.scatter(f1.x[0,0],f1.x[3,0], color='green')
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user