Related
I'm trying to make a neural network that calculates the right input angles for a rotation matrix. I'm having the classic linear network structure and at the last step the output is put into my function for the rotation, which returns a point in space as a list.
Here's the code I wrote for it:
import torch
import torch.nn as nn
import torch.nn.functional as F
from torch.autograd import Variable as V
import torch.optim as opt
import numpy as np
import matplotlib.pyplot as plt
cam_pos = np.array([500, 160, 1140, 1]) # with respect to vehicle coordinates
img_res = (1280, 1080)
aspect_ratio = img_res[0] / img_res[1]
# in px
cx = 636 / aspect_ratio
cy = 548 / aspect_ratio
fx = 241 / aspect_ratio
fy = 238 / aspect_ratio
u = 872
v = 423
D = 1900 # mm
img_pt = np.array([u, v, 1, 1/D]).T
camera_matrix = np.array([[fx, 0, cx, 0],
[0, fy, cy, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
class Network(nn.Module):
def __init__(self):
super(Network, self).__init__()
self.lin1 = nn.Linear(3,10)
self.lin2 = nn.Linear(10,10)
self.lin3 = nn.Linear(10,3)
self.angle_list = []
def forward(self, x):
x = F.relu(self.lin1(x))
x = F.relu(self.lin2(x))
x = self.lin3(x)
self.angle_list.append(list(x.detach().numpy()))
return torch.tensor(self.cam_function(x), requires_grad=True)
def rot_x(self, alpha):
return np.array([ [1, 0, 0, 0],
[0, np.cos(alpha), -np.sin(alpha), 0],
[0, np.sin(alpha), np.cos(alpha), 0],
[0, 0, 0, 1]
])
def rot_y(self, beta):
return np.array([ [np.cos(beta), 0, np.sin(beta), 0],
[0, 1, 0, 0],
[-np.sin(beta), 0, np.cos(beta), 0],
[0, 0, 0, 1]
])
def rot_z(self, gamma):
return np.array([ [np.cos(gamma), -np.sin(gamma), 0, 0],
[np.sin(gamma), np.cos(gamma), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
def cam_function(self, net_output):
net_output = net_output.detach().numpy()
x = net_output[0]
y = net_output[1]
z = net_output[2]
rot_m = np.dot(self.rot_z(z), np.dot(self.rot_y(y), self.rot_x(x)))
extrinsic_matrix = np.array([ [rot_m[0][0], rot_m[0][1], rot_m[0][2], cam_pos[0]],
[rot_m[1][0], rot_m[1][1], rot_m[1][2], cam_pos[1]],
[rot_m[2][0], rot_m[2][1], rot_m[2][2], cam_pos[2]],
[0, 0, 0, 1 ]])
cam_output = img_pt * D * np.linalg.inv(camera_matrix) * extrinsic_matrix / 1000
cam_output = [cam_output[0][0], cam_output[1][1], cam_output[2][2]]
return cam_output
model = Network()
loss_function = nn.CrossEntropyLoss()
optimizer = opt.SGD(model.parameters(), lr=1e-3)
target = torch.tensor([1.636, 1.405, 0.262]).float()
dummy_data = torch.tensor([0, 0, 0]).float()
losses = []
for epoch in range(5000):
model.train()
prediction= model(dummy_data)
loss = loss_function(prediction, target)
losses.append(loss.item())
optimizer.zero_grad()
loss.backward()
optimizer.step()
And with that I'm getting a constant value of the loss and the output as well.
7.3858967314779305
tensor([7.9938, 3.9272, 1.8514], dtype=torch.float64, requires_grad=True)
7.3858967314779305
tensor([7.9938, 3.9272, 1.8514], dtype=torch.float64, requires_grad=True)
7.3858967314779305
tensor([7.9938, 3.9272, 1.8514], dtype=torch.float64, requires_grad=True)
Can someone help me please? If this works I would then extract the "angles" the NN used for the rotation matrix
Do not use numpy in cam_function, use torch.tensor. Using numpy, the gradient does not flow when using backward.
I have designed a single layer neural network:
x=AF
y=xw
A is matrix(nn) Graph Adjacency Matrix ,F is matrix(n*2) and w is Wight.
This is the code:
import numpy as np
import networkx as nx
def relu(X):
return np.maximum(0,X)
A1 = np.matrix([
[0, 0, 1, 0,0,0],
[0, 0, 1, 0,0,0],
[0, 1, 0, 0,1,1],
[1, 0, 1, 0,0,0],
[0,1,0,0,1,0],
[0,1,1,0,1,0]],
dtype=float
)
G = nx.from_numpy_matrix(A1, create_using=nx.DiGraph)
nodes=list(G.nodes())
print(nodes)
print('edges',len(G.edges()))
UN_G=G.to_undirected()
A=nx.adjacency_matrix(UN_G)
print('un_edges',len(UN_G.edges()))
F = np.matrix([
[G.in_degree(i), G.out_degree(i)]
for i in range(A.shape[0])
], dtype=float)
I = np.matrix(np.eye(A.shape[0]))
A_prem = A + I
D_hat= np.array(np.sum(A_prem, axis=0))[0]
D_hat = np.matrix(np.diag(D_hat))
A_hat=D_hat**-1 * A_prem * D_hat**-1
x=A_hat*F
x=A*F
(x)
In the unsupervised loss function: A is sparse matrix
def loss_fn(y,A):
coo = A.tocoo()
tmp=zip(coo.row, coo.col, coo.data)
sum = tf.Variable(0.0)
for i,j,k in tmp:
sum=sum+k*tf.linalg.norm(y[i]-y[j])
return (sum)
In the weight training phase:
epchs=50
w = tf.Variable(tf.random.normal((2, 2)), name='w')
eta=0.3
for ephoc in range(epchs):
with tf.GradientTape(persistent=True) as tape:
tape.watch(w)
y = tf.nn.softmax(x # w)
loss=loss_fn(y,A)
print(ephoc,' ',loss)
dl_dw = tape.gradient(loss, w)
w.assign_sub(eta*dl_dw)
value of [dl_dw] and loss are nan. What is the problem with my code? Thank you for guiding me
I'm trying to apply the Expectation Maximization Algorithm (EM) to a Gaussian Mixture Model (GMM) using Python and NumPy. The PDF document I am basing my implementation on can be found here.
Below are the equations:
When applying the algorithm I get the mean of the first and second cluster equal to:
array([[2.50832195],
[2.51546208]])
When the actual vector means for the first and second cluster are, respectively:
array([[0],
[0]])
and:
array([[5],
[5]])
The same thing happens when getting the values of the covariance matrices I get:
array([[7.05168736, 6.17098629],
[6.17098629, 7.23009494]])
When it should be:
array([[1, 0],
[0, 1]])
for both clusters.
Here is the code:
np.random.seed(1)
# first cluster
X_11 = np.random.normal(0, 1, 1000)
X_21 = np.random.normal(0, 1, 1000)
# second cluster
X_12 = np.random.normal(5, 1, 1000)
X_22 = np.random.normal(5, 1, 1000)
X_1 = np.concatenate((X_11,X_12), axis=None)
X_2 = np.concatenate((X_21,X_22), axis=None)
# data matrix of k x n dimensions (2 x 2000 dimensions)
X = np.concatenate((np.array([X_1]),np.array([X_2])), axis=0)
# multivariate normal distribution function gives n x 1 vector (2000 x 1 vector)
def normal_distribution(x, mu, sigma):
mvnd = []
for i in range(np.shape(x)[1]):
gd = (2*np.pi)**(-2/2) * np.linalg.det(sigma)**(-1/2) * np.exp((-1/2) * np.dot(np.dot((x[:,i:i+1]-mu).T, np.linalg.inv(sigma)), (x[:,i:i+1]-mu)))
mvnd.append(gd)
return np.reshape(np.array(mvnd), (np.shape(x)[1], 1))
# Initialized parameters
sigma_1 = np.array([[10, 0],
[0, 10]])
sigma_2 = np.array([[10, 0],
[0, 10]])
mu_1 = np.array([[10],
[10]])
mu_2 = np.array([[10],
[10]])
pi_1 = 0.5
pi_2 = 0.5
Sigma_1 = np.empty([2000, 2, 2])
Sigma_2 = np.empty([2000, 2, 2])
for i in range(10):
# E-step:
w_i1 = (pi_1*normal_distribution(X, mu_1, sigma_1))/(pi_1*normal_distribution(X, mu_1, sigma_1) + pi_2*normal_distribution(X, mu_2, sigma_2))
w_i2 = (pi_2*normal_distribution(X, mu_2, sigma_2))/(pi_1*normal_distribution(X, mu_1, sigma_1) + pi_2*normal_distribution(X, mu_2, sigma_2))
# M-step:
pi_1 = np.sum(w_i1)/2000
pi_2 = np.sum(w_i2)/2000
mu_1 = np.array([(1/(np.sum(w_i1)))*np.sum(w_i1.T*X, axis=1)]).T
mu_2 = np.array([(1/(np.sum(w_i2)))*np.sum(w_i2.T*X, axis=1)]).T
for i in range(2000):
Sigma_1[i:i+1, :, :] = w_i1[i:i+1,:]*np.dot((X[:,i:i+1]-mu_1), (X[:,i:i+1]-mu_1).T)
Sigma_2[i:i+1, :, :] = w_i2[i:i+1,:]*np.dot((X[:,i:i+1]-mu_2), (X[:,i:i+1]-mu_2).T)
sigma_1 = (1/(np.sum(w_i1)))*np.sum(Sigma_1, axis=0)
sigma_2 = (1/(np.sum(w_i2)))*np.sum(Sigma_2, axis=0)
Would really appreciate if someone could point out the mistake in my code or in my misunderstanding of the algorithm..
I'm implementing a Kalman filter for an 2D tracked object. I'm measuring the position and the velocity of the object. For the moment, I assume I have all the data from the sensors at the same time, so my observation matrix H is
H = eye(4,4), a 4x4 identity matrix. (See code below)
However, in my final implementation I will have the data from the sensors at different times. So in some update loops I will have the velocity, and in others I will have the position. How would I write the H matrix in those cases?
Is it okay to write
[position loop]
[1, 0, 0, 0 ]
[0, 1, 0, 0 ]
[0, 0, 0, 0 ]
[0, 0, 0, 0 ]
[velocity loop]
[0, 0, 0, 0 ]
[0, 0, 0, 0 ]
[0, 0, 1, 0 ]
[0, 0, 0, 1 ]
Note that my state space variables are [x, y, vx, vy]
I wonder if using those matrices does not imply that my observations are zero, or something like that.
Can I leave the covariances matrices untouched? I guess not.
#Implementation of 2D filter with FilterPy.
import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
import matplotlib.pyplot as plt
# --------- PARAM -----------
dt = 0.1
v_dev = 0.3
pos_dev = 0.8
duration = 50
acceleration_noise = 0.3
# --------- MODEL ------------
transition_matrix = [[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]]
transition_covariance = np.array([
[ 0.25*pow(dt, 4), 0, 0.5* pow(dt, 3), 0 ],
[ 0, 0.25*pow(dt, 4), 0, 0.5* pow(dt, 3)],
[ 0.5* pow(dt, 3), 0, dt*dt, 0],
[ 0, 0.5*dt*dt*dt, 0, dt*dt]]) * acceleration_noise *acceleration_noise # A large process noise favors the measurements. ()
#Transition matrix with acceleration componentn
observation_matrix = np.eye(4, 4)
initial_state = [0, 0, 0.5, 0.5]
initial_state_covariance = [[ pos_dev*pos_dev, 0, 0 ,0],[0, pos_dev*pos_dev, 0, 0],[0, 0, v_dev * v_dev, 0 ],[0, 0, 0, v_dev * v_dev ]]
observation_covariance = [[pos_dev * pos_dev , 0, 0 ,0],[0, pos_dev * pos_dev, 0, 0],[0, 0, v_dev * v_dev, 0 ],[0, 0, 0, v_dev * v_dev ]]
#-----------------------------
#---------- FAKE DATA ---------
ind = np.array( range( round(duration/dt) ) )
time = ind * dt
position = np.zeros( (2, len(ind)) )
position[0,:] = time
position[1,:] = 3 * np.sin(time)
noise = pos_dev * np.random.randn(2, len(ind))
noisy_pos = position + noise
vel = position[:,1:len(ind)] - position[:,0:len(ind)-1]
vel = vel / dt
vel_ind = np.zeros( (2, len(ind) -1 ) )
vel_ind[0,:] = position[0,0:len(ind)-1]
vel_ind[1,:] = position[1,0:len(ind)-1]
vel_noise = v_dev * np.random.randn(2, len(ind) - 1 )
noisy_vel = vel + vel_noise
observations = np.zeros((len(ind), 4))
observations[:,[0,1]] = np.transpose(noisy_pos)
observations[1:len(ind),[2,3]] = np.transpose(noisy_vel)
observations[0,[2,3]] = np.transpose(noisy_vel[[0,1],0] )
# KALMAN!
filtered_state_means = np.zeros((len(time), 4))
filtered_state_covariances = np.zeros( ( len(time), 4, 4) )
kf = KalmanFilter( dim_x = 4, dim_z = 4) # state space: x, y, vx, vy, measuring all
kf.x = np.array( initial_state )
kf.F = np.array( transition_matrix )
kf.H = np.array( observation_matrix )
kf.P = np.array( initial_state_covariance )
kf.Q = np.array( transition_covariance )
kf.R =np.array( observation_covariance ) #measurement covariance
for i in range(0, len(time) ):
# Ommitting some data points
if( i > no_gps_start and i < no_gps_end):
# No data from gps
kf.H = np.array( ([0, 0, 0, 0],[0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]) )
else:
kf.H = observation_matrix
kf.predict()
kf.update(observations[i])
filtered_state_means[i] = kf.x
filtered_state_covariances[i] = kf.P
# Plotting everything
xmin = -2
xmax = 22
ymin = -4.3
ymax = 4.3
axisLimits = [xmin, xmax, ymin, ymax]
plt.figure(1)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7')
plt.plot( noisy_pos[0,:], noisy_pos[1,:], '.')
plt.axis( axisLimits )
plt.figure(2)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7')
plt.quiver( vel_ind[0,:], vel_ind[1,:], noisy_vel[0,:], noisy_vel[1,:], angles='xy', scale_units='xy', scale=10)
plt.axis( axisLimits )
plt.figure(3)
plt.plot( position[0,:], position[1,:], linewidth=1 , color= '0.7', zorder= 1)
plt.plot( filtered_state_means[:,0], filtered_state_means[:,1], linewidth = 1, zorder= 2)
plt.plot( noisy_pos[0,:], noisy_pos[1,:], '.', color = "#fd92f8", zorder= 0)
plt.plot( no_gps_x, no_gps_y, 'ro')
plt.show()
You are right, you are not allowed to modify the observation matrix in this way.
In your case the best solution would be a sequential Kalman Filter, which was developed exactly for handling of missing measurements. The measurement vector is replaced through a sequence of separate scalar measurements. The filter can proceed them independently and is not corrupted if one or more measurements do not exist at some point in time.
Have a look at Dan Simon's "Optimal State Estimation" Chapter 6.1 (you can try to find the book online). He derives alternative equations for the Kalman Filter, which are pretty easy to implement. The prediction step stays the same, you need to modify the update step.
Pros:
you don't need to compute the inverse matrix at all (nice for embedded systems)
if your H matrix has a lot of zeros the equivalent sequential expressions are very short and computationally efficient
Contras:
the R matrix (measurement covariance) has to be diagonal
I have an array a of length N and need to implement the following operation:
With p in [0..1]. This equation is a lossy sum, where the first indexes in the sum are weighted by a greater loss (p^{n-i}) than the last ones. The last index (i=n) is always weigthed by 1. if p = 1, then the operation is a simple cumsum.
b = np.cumsum(a)
If if p != 1, I can implement this operation in a cpu-inefficient way:
b = np.empty(np.shape(a))
# I'm using the (-1,-1,-1) idiom for reversed ranges
p_vec = np.power(p, np.arange(N-1, 0-1, -1))
# p_vec[0] = p^{N-1}, p_vec[-1] = 1
for n in range(N):
b[n] = np.sum(a[:n+1]*p_vec[-(n+1):])
Or in a memory-inefficient but vectorized way (IMO is cpu inefficient too, since a lot of work is wasted):
a_idx = np.reshape(np.arange(N+1), (1, N+1)) - np.reshape(np.arange(N-1, 0-1, -1), (N, 1))
a_idx = np.maximum(0, a_idx)
# For N=4, a_idx looks like this:
# [[0, 0, 0, 0, 1],
# [0, 0, 0, 1, 2],
# [0, 0, 1, 2, 3],
# [0, 1, 2, 3, 4]]
a_ext = np.concatenate(([0], a,), axis=0) # len(a_ext) = N + 1
p_vec = np.power(p, np.arange(N, 0-1, -1)) # len(p_vec) = N + 1
b = np.dot(a_ext[a_idx], p_vec)
Is there a better way to achieve this 'lossy' cumsum?
What you want is a IIR filter, you can use scipy.signal.lfilter(), here is the code:
Your code:
import numpy as np
N = 10
p = 0.8
np.random.seed(0)
x = np.random.randn(N)
y = np.empty_like(x)
p_vec = np.power(p, np.arange(N-1, 0-1, -1))
for n in range(N):
y[n] = np.sum(x[:n+1]*p_vec[-(n+1):])
y
the output:
array([1.76405235, 1.81139909, 2.42785725, 4.183179 , 5.21410119,
3.19400307, 3.50529088, 2.65287549, 2.01908154, 2.02586374])
By using lfilter():
from scipy import signal
y = signal.lfilter([1], [1, -p], x)
print(y)
the output:
array([1.76405235, 1.81139909, 2.42785725, 4.183179 , 5.21410119,
3.19400307, 3.50529088, 2.65287549, 2.01908154, 2.02586374])