How to find the velocity used to find the maximum distance - python

Here is my code:
# Libraries
import numpy as np
from scipy.integrate import odeint
from scipy.integrate import quad
# Constant parameters
m_Rb = 1.443*10**-25 #mass of rubidium 87
k_b = 1.38*10**-23
hbar = 1.05*10**-34
L = 38.116*10**6 #spontaneous decay rate
epsilon_0 = 8.85418782*10**-12 #permittivity of free space
# Changable paramaters
lmbda = 700*10**-9 #wavelength of laser light
k = (2*np.pi)/lmbda #wavevector of laser light
V = 1.25*10**-4 #volume of MOT space
length = 5*10**-2 #length of MOT
Bohr = 9.274*10**-24 #Bohr magneton value
B = 5*10**-4 #magnetic field strength
# Scattering force
I_sat = 1 #intensity
n0 = 1 #refraction constant for medium
E_0 = ((2*I_sat)/(3*10**8*n0*epsilon_0))
Rabi = (E_0/hbar) * (4.2275*1.6*10**-19*0.529*10**-10)
n = 20
delta_omega = np.array([-20*10**6, -15*10**6, -10*10**6, -5*10**6])
def F(v):
i = 0
R_i = []
while i<len(delta_omega):
delta = delta_omega[i] + (k*v)
R_scat = L/2 * (Rabi**2/2)/(delta**2+(Rabi**2/2)+(L**2/4)) # scattering rate
R_i.append(R_scat)
i = i+1
R = np.sum(R_i)
R_total = (L*R)/(2*R + L)
def dXdt(t, X):
G, E = X
dGdt = E*(L+R_total) - G*R_total
dEdt = G*R_total - E*(L+R_total)
return [dGdt, dEdt]
t = np.linspace(0, 5, n)
solve = odeint(dXdt, [1, 0], t, tfirst=True)
G = solve.T[0]
E = solve.T[1]
F = hbar*k*(G-E)*R_total
return F
#stopping distance
ds = []
dt = 0.00001
x = np.zeros(n)
time = np.zeros(n)
#v = np.linspace(-700, 700, n)
v = np.zeros(n)
time[0] = 0
x[0] = 0
v[0] = 800
a = 0
print(n)
for a in np.arange(1, n):
time[a]=dt*a
solve = F(v=a)
F_int=solve[1]
v[a] = v[a-1] - dt*(F_int/m_Rb)*x[a-1]
x[a] = x[a-1] + dt*v[a-1]
for s in x:
if abs(s)<=length and abs(s)>0:
ds.append(abs(s))
# Capture velocity
dss = np.max(ds)
I want to find the value of velocity for which ds is at its maximum value (i.e. what's the maximum velocity that a particle can have and not exceed 0.05) but I don't know how to do this. I have calculated the distances travelled by a number of particles, and then made an array (ds) containing the particles which stop within the value of length. dss gives the maximum distance a particle has travelled, but it is the corresponding velocity for this value that I want to find.

Related

Adding Additional Rows To Tensors

I want to simulate a data generating process via tensor methods. In the end, the data will be exported to a csv file such that each row corresponds to a time period and each column corresponds to a unit. The following code
import numpy as np
import pandas as pd
import random
import tensorly as tl
from itertools import product
import matplotlib.pyplot as plt
import tensorly.decomposition
np.random.seed(1812)
# Data Generation
L = 0.05
H = 0.05
dx = 0.0025
dy = 0.0025
tmax = 60
dt = 0.01
epsilon = 0.0001
alpha = 0.5e-5+np.random.random()*1e-5
SimulateData = []
SimulateDataNoNoise = []
r_x = alpha*dt/dx**2
r_y = alpha*dt/dy**2
fo = r_x + r_y
if fo > 0.5:
msg = f'Current Fo = {fo}, which is numerically unstable (>0.5)'
raise ValueError(msg)
# x, y meshgrid based on dx, dy
nx = int(L/dx + 1)
ny = int(H/dy + 1)
X, Y = np.meshgrid(np.linspace(0, L, nx), np.linspace(0, H, ny))
# center point of the domain
ic = int((nx-1)/2)
jc = int((ny-1)/2)
# initial and boundary conditions
S = np.zeros((ny, nx))
def enforceBdy(S):
''' Enforces the boundary conditions on S, the temperature values on the domain's grid points'''
S[:, 0] = 1
S[:, -1] = 1
S[0, :] = 1
S[-1, :] = 1
return S
S = enforceBdy(S)
def Laplace(T):
'''Computes the Laplacian operator, del-squared on the data'''
tmp_x, tmp_y = np.gradient(T, dx, dy)
tmp_x, _ = np.gradient(tmp_x, dx)
_, tmp_y = np.gradient(tmp_y, dy)
return tmp_x+tmp_y
# iteration
nmax = int(tmax/dt)
for n in range(nmax):
dSdt = alpha*Laplace(S)
S = S + dSdt*dt
S = enforceBdy(S)
if n % 100 == 0:
noise = np.random.normal(size=S.shape)*.1
SimulateData.append(S.copy()+noise)
SimulateDataNoNoise.append(S.copy())
# check for convergence
err = np.abs(dSdt*dt).max()
if err <= epsilon:
break
#
# Creates Tensor
X = np.stack(SimulateData, 2)
nx,ny,nt = X.shape
# CP Decomposition
err = []
for i in range(1,11):
CP_Heat = tl.decomposition.parafac(X,i)
reconstructed = tl.kruskal_to_tensor(CP_Heat)
err.append(((X-reconstructed)**2).sum())
AIC1 = [2*e + 2*(i+1) for i,e in enumerate(err)]
AIC2 = [2*e + (i+1)*nx+(i+1)*ny+(i+1)*nt for i,e in enumerate(err)]
AIC = AIC2
idxmin = np.argmin(AIC)
R = idxmin+1
min_AIC = AIC[idxmin]
Y = np.zeros((21,40))
beta = np.random.randint(low=-0,high=15,size=21).reshape(-1,1)
for i in range(40):
RHS = 15 + X[:,:,i]#beta + np.random.normal(size=21).reshape(-1,1)
Y[:,i] = RHS.ravel()
Y
np.savetxt("Sim1.csv", Y, delimiter=",")
Returns a CSV file of 21 rows and 40 columns. Suppose, however, I wanted 40 or 70 rows in the final file with 40 columns. How would I do this? When I try with the number 22
import numpy as np
import pandas as pd
import random
import tensorly as tl
from itertools import product
import matplotlib.pyplot as plt
import tensorly.decomposition
np.random.seed(1812)
# Data Generation
L = 0.05
H = 0.05
dx = 0.0025
dy = 0.0025
tmax = 60
dt = 0.01
epsilon = 0.0001
alpha = 0.5e-5+np.random.random()*1e-5
SimulateData = []
SimulateDataNoNoise = []
r_x = alpha*dt/dx**2
r_y = alpha*dt/dy**2
fo = r_x + r_y
if fo > 0.5:
msg = f'Current Fo = {fo}, which is numerically unstable (>0.5)'
raise ValueError(msg)
# x, y meshgrid based on dx, dy
nx = int(L/dx + 1)
ny = int(H/dy + 1)
X, Y = np.meshgrid(np.linspace(0, L, nx), np.linspace(0, H, ny))
# center point of the domain
ic = int((nx-1)/2)
jc = int((ny-1)/2)
# initial and boundary conditions
S = np.zeros((ny, nx))
def enforceBdy(S):
''' Enforces the boundary conditions on S, the temperature values on the domain's grid points'''
S[:, 0] = 1
S[:, -1] = 1
S[0, :] = 1
S[-1, :] = 1
return S
S = enforceBdy(S)
def Laplace(T):
'''Computes the Laplacian operator, del-squared on the data'''
tmp_x, tmp_y = np.gradient(T, dx, dy)
tmp_x, _ = np.gradient(tmp_x, dx)
_, tmp_y = np.gradient(tmp_y, dy)
return tmp_x+tmp_y
# iteration
nmax = int(tmax/dt)
for n in range(nmax):
dSdt = alpha*Laplace(S)
S = S + dSdt*dt
S = enforceBdy(S)
if n % 100 == 0:
noise = np.random.normal(size=S.shape)*.1
SimulateData.append(S.copy()+noise)
SimulateDataNoNoise.append(S.copy())
# check for convergence
err = np.abs(dSdt*dt).max()
if err <= epsilon:
break
#
# Creates Tensor
X = np.stack(SimulateData, 2)
nx,ny,nt = X.shape
# CP Decomposition
err = []
for i in range(1,11):
CP_Heat = tl.decomposition.parafac(X,i)
reconstructed = tl.kruskal_to_tensor(CP_Heat)
err.append(((X-reconstructed)**2).sum())
AIC1 = [2*e + 2*(i+1) for i,e in enumerate(err)]
AIC2 = [2*e + (i+1)*nx+(i+1)*ny+(i+1)*nt for i,e in enumerate(err)]
AIC = AIC2
idxmin = np.argmin(AIC)
R = idxmin+1
min_AIC = AIC[idxmin]
Y = np.zeros((22,40))
beta = np.random.randint(low=-0,high=15,size=22).reshape(-1,1)
for i in range(40):
RHS = 15 + X[:,:,i]#beta + np.random.normal(size=22).reshape(-1,1)
Y[:,i] = RHS.ravel()
Y
np.savetxt("Sim1.csv", Y, delimiter=",")
Python throws an exception saying "(size 22 is different from 21)", but I'm unclear on where the 21 comes from when I do not specify the number 21 anywhere in my code.

How to use argmin() and find minimum value from array

I'm new to python so the code may not be the best. I'm trying to find the minimum Total Cost (TotalC) and the corresponding m,k and xM values that go with this minimum cost. I'm not sure how to do this. I have tried using min(TotalC) however this gives an error within the loop or outside the loop only returns the value of TotalC and not the corresponding m, k, and xM values. Any help would be appreciated. This section is at the end of the code, I have included my entire code.
I have tried using
minIndex = TotalC.argmin()
but I'm not sure how to use it and it only returns 0 each time.
import numpy as np
import matplotlib.pyplot as plt
def Load(x):
Fpeak = (1000 + (9*(x**2) - (183*x))) *1000 #Fpeak in N
td = (20 - ((0.12)*(x**2)) + (4.2*(x))) / 1000 #td in s
return Fpeak, td
#####################################################################################################
####################### Part 2 ########################
def displacement(m,k,x,dt): #Displacement function
Fpeak, td = Load(x) #Load Function from step 1
w = np.sqrt(k/m) # Natural circular frequency
T = 2 * np.pi /w #Natural period of blast (s)
time = np.arange(0,2*T,0.001) #Time array with range (0 - 2*T) with steps of 2*T/100
zt = [] #Create a lsit to store displacement values
for t in time:
if (t <= td):
zt.append((Fpeak/k) * (1 - np.cos(w*t)) + (Fpeak/(k*td)) * ((np.sin(w*t)/w) - t))
else:
zt.append((Fpeak/(k*w*td)) * (np.sin(w*t) - np.sin(w*(t-td))) - ((Fpeak/k) * np.cos(w*t)))
zmax=max(zt) #Find the max displacement from the list of zt values
return zmax #Return max displacement
k = 1E6
m = 200
dt = 0.0001
x = 0
z = displacement(m,k,x,dt)
###################################################################################
############### Part 3 #######################
# k = 1E6 , m = 200kg , Deflection = 0.1m
k_values = np.arange(1E6, 7E6, ((7E6-1E6)/10)) #List of k values between min and max (1E6 and 7E6).
m_values = np.arange(200,1200,((1200-200)/10)) #List of m values between min and max 200kg and 1200kg
xM = []
for k in k_values: # values of k
for m in m_values: # values of m within k for loop
def bisector(m,k,dpoint,dt): #dpoint = decimal point accuracy
xL = 0
xR = 10
xM = (xL + xR)/2
zmax = 99
while round(zmax, dpoint) !=0.1:
zmax = displacement(m,k,xM,dt)
if zmax > 0.1:
xL = xM
xM = (xL + xR)/2
else:
xR = xM
xM = (xL + xR)/2
return xM
xM = bisector(m, k, 4, 0.001)
print('xM value =',xM)
#####################################################
#######Step 4
def cost (m,k,xM):
Ck = 900 + 825*((k/1E6)**2) - (1725*(k/1E6))
Cm = 10*m - 2000
Cx = 2400*((xM**2)/4)
TotalC = Ck + Cm + Cx
minIndex = TotalC.argmin(0)
print(minIndex)
return TotalC
TotalC = cost(m, k, xM)
minIndex = TotalC.argmin()
print(minIndex)
print([xM, m, k, TotalC])
argmin() returns the index of a minimum value. If you are looking for the minimum itself, try using .min(). There is also a possibility that 0 is the lowest value in Your array so bear that in mind

How to animate this optimization model correctly

I have implemented a simple randomized, population-based optimization method - Grey Wolf optimizer. I am having some trouble with properly capturing the Matplotlib plots at each iteration using the camera package.
I am running GWO for the objective function f(x,y) = x^2 + y^2. I can only see the candidate solutions converging to the minima, but the contour plot doesn't show up.
Do you have any suggestions, how can I display the contour plot in the background?
GWO Algorithm implementation
%matplotlib notebook
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera
import ffmpeg
import pillow
# X : Position vector of the initial population
# n : Initial population size
def gwo(f,max_iterations,LB,UB):
fig = plt.figure()
camera = Camera(fig)
def random_population_uniform(m,a,b):
dims = len(a)
x = [list(a + np.multiply(np.random.rand(dims),b - a)) for i in range(m)]
return np.array(x)
def search_agent_fitness(fitness):
alpha = 0
if fitness[1] < fitness[alpha]:
alpha, beta = 1, alpha
else:
beta = 1
if fitness[2] > fitness[alpha] and fitness[2] < fitness[beta]:
beta, delta = 2, beta
elif fitness[2] < fitness[alpha]:
alpha,beta,delta = 2,alpha,beta
else:
delta = 2
for i in range(3,len(fitness)):
if fitness[i] <= fitness[alpha]:
alpha, beta,delta = i, alpha, beta
elif fitness[i] > fitness[alpha] and fitness[i]<= fitness[beta]:
beta,delta = i,beta
elif fitness[i] > fitness[beta] and fitness[i]<= fitness[delta]:
delta = i
return alpha, beta, delta
def plot_search_agent_positions(f,X,alpha,beta,delta,a,b):
# Plot the positions of search agents
x = X[:,0]
y = X[:,1]
s = plt.scatter(x,y,c='gray',zorder=1)
s = plt.scatter(x[alpha],y[alpha],c='red',zorder=1)
s = plt.scatter(x[beta],y[beta],c='blue',zorder=1)
s = plt.scatter(x[delta],y[delta],c='green',zorder=1)
camera.snap()
# Initialize the position of the search agents
X = random_population_uniform(50,np.array(LB),np.array(UB))
n = len(X)
l = 1
# Plot the first image on screen
x = np.linspace(LB[0],LB[1],1000)
y = np.linspace(LB[0],UB[1],1000)
X1,X2 = np.meshgrid(x,y)
Z = f(X1,X2)
cont = plt.contour(X1,X2,Z,20,linewidths=0.75)
while (l < max_iterations):
# Take the x,y coordinates of the initial population
x = X[:,0]
y = X[:,1]
# Calculate the objective function for each search agent
fitness = list(map(f,x,y))
# Update alpha, beta and delta
alpha,beta,delta = search_agent_fitness(fitness)
# Plot search agent positions
plot_search_agent_positions(f,X,alpha,beta,delta,LB,UB)
# a decreases linearly from 2 to 0
a = 2 - l *(2 / max_iterations)
# Update the position of search agents including the Omegas
for i in range(n):
x_prey = X[alpha]
r1 = np.random.rand(2) #r1 is a random vector in [0,1] x [0,1]
r2 = np.random.rand(2) #r2 is a random vector in [0,1] x [0,1]
A1 = 2*a*r1 - a
C1 = 2*r2
D_alpha = np.abs(C1 * x_prey - X[i])
X_1 = x_prey - A1*D_alpha
x_prey = X[beta]
r1 = np.random.rand(2)
r2 = np.random.rand(2)
A2 = 2*a*r1 - a
C2 = 2*r2
D_beta = np.abs(C2 * x_prey - X[i])
X_2 = x_prey - A2*D_beta
x_prey = X[delta]
r1 = np.random.rand(2)
r2 = np.random.rand(2)
A3 = 2*a*r1 - a
C3 = 2*r2
D_delta = np.abs(C3 * x_prey - X[i])
X_3 = x_prey - A3*D_delta
X[i] = (X_1 + X_2 + X_3)/3
l = l + 1
return X[alpha],camera
Function call
# define the objective function
def f(x,y):
return x**2 + y**2
minimizer,camera = gwo(f,7,[-10,-10],[10,10])
animation = camera.animate(interval = 1000, repeat = True,
repeat_delay = 500)
Is it possible that the line x = np.linspace(LB[0],LB[1],1000) should be x = np.linspace(LB[0],UB[1],1000) instead? With your current definition of x, x is an array only filled with the value -10 which means that you are unlikely to find a contour.
Another thing that you might want to do is to move the cont = plt.contour(X1,X2,Z,20,linewidths=0.75) line inside of your plot_search_agent_positions function to ensure that the contour is plotted at each iteration of the animation.
Once you make those changes, the code looks like that:
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera
import ffmpeg
import PIL
from matplotlib import animation, rc
from IPython.display import HTML, Image # For GIF
from scipy.interpolate import griddata
rc('animation', html='html5')
# X : Position vector of the initial population
# n : Initial population size
def gwo(f,max_iterations,LB,UB):
fig = plt.figure()
fig.gca(aspect='equal')
camera = Camera(fig)
def random_population_uniform(m,a,b):
dims = len(a)
x = [list(a + np.multiply(np.random.rand(dims),b - a)) for i in range(m)]
return np.array(x)
def search_agent_fitness(fitness):
alpha = 0
if fitness[1] < fitness[alpha]:
alpha, beta = 1, alpha
else:
beta = 1
if fitness[2] > fitness[alpha] and fitness[2] < fitness[beta]:
beta, delta = 2, beta
elif fitness[2] < fitness[alpha]:
alpha,beta,delta = 2,alpha,beta
else:
delta = 2
for i in range(3,len(fitness)):
if fitness[i] <= fitness[alpha]:
alpha, beta,delta = i, alpha, beta
elif fitness[i] > fitness[alpha] and fitness[i]<= fitness[beta]:
beta,delta = i,beta
elif fitness[i] > fitness[beta] and fitness[i]<= fitness[delta]:
delta = i
return alpha, beta, delta
def plot_search_agent_positions(f,X,alpha,beta,delta,a,b,X1,X2,Z):
# Plot the positions of search agents
x = X[:,0]
y = X[:,1]
s = plt.scatter(x,y,c='gray',zorder=1)
s = plt.scatter(x[alpha],y[alpha],c='red',zorder=1)
s = plt.scatter(x[beta],y[beta],c='blue',zorder=1)
s = plt.scatter(x[delta],y[delta],c='green',zorder=1)
Z=f(X1,X2)
cont=plt.contour(X1,X2,Z,levels=20,colors='k',norm=True)
plt.clabel(cont, cont.levels, inline=True, fontsize=10)
camera.snap()
# Initialize the position of the search agents
X = random_population_uniform(50,np.array(LB),np.array(UB))
n = len(X)
l = 1
# Plot the first image on screen
x = np.linspace(LB[0],UB[1],1000)
y = np.linspace(LB[0],UB[1],1000)
X1,X2 = np.meshgrid(x,y)
Z=f(X1,X2)
while (l < max_iterations):
# Take the x,y coordinates of the initial population
x = X[:,0]
y = X[:,1]
# Calculate the objective function for each search agent
fitness = list(map(f,x,y))
# Update alpha, beta and delta
alpha,beta,delta = search_agent_fitness(fitness)
# Plot search agent positions
plot_search_agent_positions(f,X,alpha,beta,delta,LB,UB,X1,X2,Z)
# a decreases linearly from 2 to 0
a = 2 - l *(2 / max_iterations)
# Update the position of search agents including the Omegas
for i in range(n):
x_prey = X[alpha]
r1 = np.random.rand(2) #r1 is a random vector in [0,1] x [0,1]
r2 = np.random.rand(2) #r2 is a random vector in [0,1] x [0,1]
A1 = 2*a*r1 - a
C1 = 2*r2
D_alpha = np.abs(C1 * x_prey - X[i])
X_1 = x_prey - A1*D_alpha
x_prey = X[beta]
r1 = np.random.rand(2)
r2 = np.random.rand(2)
A2 = 2*a*r1 - a
C2 = 2*r2
D_beta = np.abs(C2 * x_prey - X[i])
X_2 = x_prey - A2*D_beta
x_prey = X[delta]
r1 = np.random.rand(2)
r2 = np.random.rand(2)
A3 = 2*a*r1 - a
C3 = 2*r2
D_delta = np.abs(C3 * x_prey - X[i])
X_3 = x_prey - A3*D_delta
X[i] = (X_1 + X_2 + X_3)/3
l = l + 1
return X[alpha],camera
# define the objective function
def f(x,y):
return x**2 + y**2
minimizer,camera = gwo(f,7,[-10,-10],[10,10])
animation = camera.animate(interval = 1000, repeat = True,repeat_delay = 500)
And the output gives:

I'm trying visualize the solution to my Predator-Prey model (using Runge-kutta 3) by plotting a graph predator versus prey. But it won't plot

I'm trying to visualize the solution to my code by plotting - in python - a graph of the number of predators versus preys, and using a black circle to mark the current state. My code runs okay except I can't get the plotting portion of the graph to print the plots on the graph.
This is my current python code:
import matplotlib.pyplot as plt
import numpy as np
from trajectoryplotsupport import *
def ExactTrajectory(dt, ntimesteps):
tt = np.linspace(0, dt*ntimesteps, ntimesteps+1)
x = np.cos(2*np.pi*tt)
y = np.sin(2*np.pi*tt)
return x,y
def GetVelocity(pos, tn):
u =-2.0*np.pi * pos[1]
v = 2.0*np.pi * pos[0]
vel = np.array([u,v])
return vel
def RK3(p, t):
vel = GetVelocity(p, t) # stage 1 velocity
pt = p + 0.5*dt*vel # stage 1 provisional position
vel = GetVelocity(pt, t+dt) # stage 2 velocity
pt = 0.25 * p + 0.75*( pt + dt*vel ) # stage 2 provisional position
vel = GetVelocity(pt, t+0.5*dt) # stage 3 velocity
p = ( p + 2.0*(pt + dt*vel) )/3.0 # stage 3 provisional position
return p
def PreyPred(p, t):
b = 0.5 # prey birth rate
m = 0.2 # predator mortality
a = 0.04 # effect of predator abundance on prey
r = 0.01 # effect of prey abundance on predator
h, w = p[0], p[1] # initial densities of prey and predator are h(0) = 4.0 and w(0) = 8.0
dht = h*(b - a*w)
dwt = w*(-m + r*h)
return np.array([dht, dwt])
#################MAIN CODE#############
p = np.array([4.0, 8.0]) # initial position
ntimesteps = 120 # number of time steps
finalTime = 480 # final time
dt = 1 # time step
xe,ye = ExactTrajectory(dt, ntimesteps) # call exact trajectory
# Check on computations. Create 2 array to track the positions and initialize
pos = np.zeros(2) # create current position array
pos[0] = xe[0]; pos[1]=ye[0] # initialize current position
fig, lastPosition = InitialPlot(p, xe,ye)
for it in range(finalTime): # time loop
t = it*dt # set current time
p = RK3(p, t)
print(t, p)
UpdatePlot(fig,lastPosition,p,t) # update the plot
ttpoints = np.arange(0, 480, dt) #edited
hpoints, wpoints = [], []
p = np.array([0, 0], float)
for tt in ttpoints:
hpoints.append(p[0])
wpoints.append(p[1])
p += RK3(p, tt)
plt.plot(ttpoints, hpoints)
plt.plot(ttpoints, wpoints)
plt.xlabel("Prey")
plt.ylabel("Predator")
plt.plot(hpoints, wpoints)
plt.show()

Adding dataset for measuremnts to kalman filter

I am implementing the Kalman filter and I have a data set in mf4 format for the measurements. I have 6 measurements and each of them contains 1000 values, I am implementing a loop of 100 for the Kalman filter but as I am new to Kalman filter and python so I am not sure that the procedure I am following is correct or not. My question is that when I run the loop for the measurements in the update step for prediction of next state then all these 1000 values will be used and if it is so then the result might not be appropriate. Kindly suggest any solution, please.
My code is:
class KF:
# initialization of variables
def __init__(self, a,b,c,d,e,f,g,h,i):
# do initizatoin steps
#x = [a,b,c,d,e,f]
# a = yaw rate, b = float angle, c = offset between road and vehicle, d = curvature, e = angle
# b/w ego vehicles direction of motion and the road curvature tangent, f = lane width
def predict(self, dt, u):
# using equations do the predict step
# u = [g,h,i]
# g = front angle wheel, h = longitudinal velocity, i = acceleration
# dt = time step
def update(self, y):
# using equations of update step
# y1 = yaw rate, y2 = acceleration, y3 = curvature, y4 = angle b/w vehicle axis and lane,
# y5 = lane width, y6 = offset b/w road and vehicle
# Main code:
kf = KF(a = random.random(), b = random.random(), c = random.random(), d = random.random(), e= random.random(), f = random.random(), g= random.random(), h= random.random(), I = random.random())
variance = 0.1 ** 2
DT = 0.1
number = 100
for step in range(number):
kf.predict(dt=DT, tmp=np.array([kf.g,kf.h,kf.i]))
cc = np.empty(6, dtype=object)
meas_y1 = efficient.get('Car.Yaw') + np.random.randn() * np.sqrt(variance)
meas_y2 = efficient.get('Car.ay_1') + np.random.randn() * np.sqrt(variance)
meas_y3 = efficient.get('Car.Curv') + np.random.randn() * np.sqrt(variance)
meas_y4 = efficient.get('Car.LongSlope') + np.random.randn() * np.sqrt(variance)
meas_y5 = efficient.get('Car.Width') + np.random.randn() * np.sqrt(variance)
meas_y6 = efficient.get('Car.tMidLane') + np.random.randn() * np.sqrt(variance)
cc[:]= [meas_y1, meas_y2, meas_y3, meas_y4, meas_y5, meas_y6]
kf.update(y= cc)

Categories