I implemented a 3d linear kalman filter to track the position of object. here are the details of the KF.
my state is 3d position only and my input(control vector) is 3d velocity (vx,vy,vz). and measurement is 3d position.
dt = time interval in state update
std_vx,std_vy,std_vz - standard deviation in control input e.g. 2 mtrs/sec
std_cooperative_pos_x,std_cooperative_pos_y,std_cooperative_pos_z - standard deviation of measurement error e.g. around 40 mtrs each axis
A = numpy.matrix([[1,0,0],[0,1,0],[0,0,1]]) # State transition matrix
B = numpy.matrix([[dt,0,0],[0,dt,0],[0,0,dt]]) # control input marix
H = numpy.matrix([[1,0,0],[0,1,0],[0,0,1]])
Q = numpy.matrix([[std_vxstd_vxdt, 0, 0],[0, std_vystd_vydt, 0],[0, 0 ,std_vzstd_vzdt]] ) # = Ex error in input , process error
R = numpy.matrix([[std_cooperative_pos_x2,0,0],[0,std_cooperative_pos_y2,0],[0,0,std_cooperative_pos_z**2]]) # measurement co- variance matrix
P = numpy.matrix([[1,0,0],[0,1,0],[0,0,1]])
here are my few questions :
how much the time interval that is dt value matters in the performance of filter. as i want dt such as 10 sec , or 20 sec .....or upto 200 secs. for dt values upto 10 sec i am able to get some improvement i my track but beyond that its not working . I am doing something wrong or its natural .
i implemented a linear filter is that right as to me combination of postion and velocity over time appears to be linear only .
thanks in advance for your time and help.
The process noise matrix 'Q' is a covariance matrix - it must contain variances rather than standard deviations. You have:
Q = numpy.matrix([[std_vxstd_vxdt, 0, 0],[0, std_vystd_vydt, 0],[0, 0 ,std_vzstd_vzdt]] ) # = Ex error in input , process error
You want:
Q = numpy.matrix([[std_vxstd_vx*std_vxstd_vx * dt *dt, 0, 0], ...
As you can see, the process noise matrix grows with dt^2. So the time interval is very important if it is the time between measurements. You need only one propagation step between measurements for this filter. There is no advantage to multiple steps (a smaller dt) unless you are approximating non-linear state propagation or there's more measurements available.
Related
I am going through a book on deep learning which initializes weights between two layers of neurons as:
w = np.random.randn(layers[i] + 1, layers[i + 1] + 1)
self.W.append(w / np.sqrt(layers[i]))
As per the book, divison by np.sqrt(layers[i]) in second line of code is done for following reason:
scale w by dividing by the square root of the number of nodes in the current layer, thereby
normalizing the variance of each neuron’s output
What does it exactly mean? And how would it impact if we don't do it?
Weights initialization is very important to tackle the vanishing/Explosion Gradients. In order for the output/gradients(reverse direction) to flow properly, the variance of the outputs of each layer to be equal to the variance of its input. Likewise of gradients in the reverse direction. the input and output flow of a layer is called fan-in and fan-out of the layer.
To better explain what I mean above, let me give you an example. Assume that we have a hundred consecutive layers and we apply a feed forward calculation with linear activation (After all it is just matrix multiplication), the data is 500 samples of 100 features:
neurons, features = 100, 100
n_layers = 100
X = np.random.normal(size=(500, features)) # your input
mean, var = 0, 0
for layer in range(n_layers):
W = np.random.normal(size=(features, neurons))
X = np.dot(X, W)
mean = mean + X.mean()
var = var + X.var()
mean/n_layers, np.sqrt(var/n_layers)
# output:
(-4.055498760574568e+95, 8.424477240271639e+98)
You will see that it will have a huge mean and standard deviations. Lets break this problem down; a property of a matrix multiplication of which the result will have a standard deviation very close to the square root of the number of fan in (input) connections. This property can be verified with this snippet of code:
fan_in = 1000 # change it to any number
X = np.random.normal(size=(100, fan_in))
W = np.random.normal(size=(fan_in, 1))
np.dot(X, W).std()
# result:
32.764359213560454
This happens because we sum fan_in (1000 in the above case) products of the element-wise multiplication of one element of inputs X by one column of W. Therefore, if we scaled every weights by the 1/sqrt(fan_in) to maintain the distribution of the flow as seen in the following snippet:
neurons, features = 100, 100
n_layers = 100
X = np.random.normal(size=(500, features)) # your input
mean, var = 0, 0
for layer in range(n_layers):
W = np.random.normal(size=(features, neurons), scale=np.sqrt(1 / neurons)) # scaled the weights with the fan-in
X = np.dot(X, W)
mean = mean + X.mean()
var = var + X.var()
mean/n_layers, np.sqrt(var/n_layers)
# output:
(0.0002608301398189543, 1.021452570914829)
You can read more about kernel initialization in the following blog
Pytorch provides a good example of using actor-critic to play Cartpole in the OpenAI gym environment.
I'm confused about several of their equations in the code snippet found at https://github.com/pytorch/examples/blob/master/reinforcement_learning/actor_critic.py#L67-L79:
saved_actions = model.saved_actions
value_loss = 0
rewards = []
for r in model.rewards[::-1]:
R = r + args.gamma * R
rewards.insert(0, R)
rewards = torch.Tensor(rewards)
rewards = (rewards - rewards.mean()) / (rewards.std() + np.finfo(np.float32).eps)
for (action, value), r in zip(saved_actions, rewards):
action.reinforce(r - value.data.squeeze())
value_loss += F.smooth_l1_loss(value, Variable(torch.Tensor([r])))
optimizer.zero_grad()
final_nodes = [value_loss] + list(map(lambda p: p.action, saved_actions))
gradients = [torch.ones(1)] + [None] * len(saved_actions)
autograd.backward(final_nodes, gradients)
optimizer.step()
What do r and value mean in this case? Why do they run REINFORCE on the action space with the reward equal to r - value? And why do they try to set the value so that it matches r?
Thanks for your help!
First the rewards a collected for a time, along with the state:action that resulted in the reward
Then r - value is the difference between the expected reward and actual
That difference is used to adjust the expected value of that action from that state
So if in state "middle", the expected reward for action "jump" was 10 and the actual reward was only 2, then the AI was off by -8 (2-10). Reinforce means "adjust expectations". So if we adjust them by half, we'll new expected reward is 10-(8 *.5), or 6. meaning the AI really thought it would get 10 for that, but now it's less confident and thinks 6 is a better guess. So if the AI is not off by much, 10 - ( 2 *.5) = 9, it will adjust by a smaller amount.
I have an acceleration data for X-axis and time vector for it. I determined the peaks more than threshold and now I should find the FFT for every peak.
As result I have this:
Peak Value 1 = 458, index 1988
Peak Value 2 = 456, index 1990
Peak Value 3 = 450, index 12081
....
Peak Value 9 = 432, index 12151
To find these peaks I used the peakfinder script.
The command [peakLoc, peakMag] = peakfinder(x0,...) gives me location and magnitude of peaks.
Also I have the Time (from time data vector) for each peak.
So what I suppose, that I should take every peak, find its width (or some data points around the peak) and make the FFT. Am I right? Could you help me in that?
I'm working in Octave and I'm new here :)
Code:
load ("C:\\..patch..\\peakfinder.m");
d =dlmread("C:\\..patch..\\acc2.csv", ";");
T=d(:,1);
Ax=d(:,2);
[peakInd peakVal]=peakfinder(Ax,10,430,1);
peakTime=T(peakInd);
[sortVal sortInd] = sort(peakVal, 'descend');
originInd = peakInd(sortInd);
for k = 1 : length(sortVal)
fprintf(1, 'Peak #%d = %d, index%d\n', k, sortVal(k), originInd (k));
end
plot(T,Ax,'b-',T(peakInd),Ax(peakInd),'rv');
and here you can download the data http://www.filedropper.com/acc2
FFT
d =dlmread("C:\\..path..\\acc2.csv", ";");
T=d(:,1);
Ax=d(:,2);
% sampling frequency
Fs_a=2000;
% length of FFT
Length_Ax=numel(Ax);
% number of lines of Fourier spectrum
fft_L= Fs_a*2;
% an array of time samples
T_Ax=0:1/Fs_a: Length_Ax;
fft_Ax=abs(fft(Ax,fft_L));
fft_Ax=2*fft_Ax./fft_L;
F=0:Fs_a/fft_L:Fs_a/2-1/fft_L;
subplot(3,1,1);
plot(T,Ax);
title('Ax axis');
xlabel('time (s)');
ylabel('amplitude)'); grid on;
subplot(3,1,2);
plot(F,fft_Ax(1:length(F)));
title('spectrum max Ax axis');
xlabel('frequency (Hz)');
ylabel('amplitude'); grid on;
It looks like you have two clusters of peaks, so I would plot the data over three plots: one of the whole timeseries, one zoomed in on the first cluster, and the last one zoomed in on the second cluster (note I have divided all your time values by 1e6 otherwise the tick labels get ugly):
figure
subplot(3,1,1)
plot(T/1e6,Ax,'b-',peakTime/1e6,peakVal,'rv');
subplot(3,1,2)
plot(T/1e6,Ax,'b-',peakTime(1:4)/1e6,peakVal(1:4),'rv');
axis([0.99*peakTime(1)/1e6 1.01*peakTime(4)/1e6 0.9*peakVal(1) 1.1*peakVal(4)])
subplot(3,1,3)
plot(T/1e6,Ax,'b-',peakTime(5:end)/1e6,peakVal(5:end),'rv');
axis([0.995*peakTime(5)/1e6 1.005*peakTime(end)/1e6 0.9*peakVal(5) 1.1*peakVal(end)])
I have set the axes around the extreme time and acceleration values, using some coefficients to have some "padding" around (the values of these coefficients were obtained through trial and error). This gives me the following plot, hopefully this is the sort of thing you are after. You can add x and y labels if you wish.
EDIT
Here's how I would do the FFT:
Fs = 2000;
L = length(Ax);
NFFT = 2^nextpow2(L); % Next power of 2 from length of Ax
Ax_FFT = fft(Ax,NFFT)/L;
f = Fs/2*linspace(0,1,NFFT/2+1);
% Plot single-sided amplitude spectrum.
figure
semilogx(f,2*abs(Ax_FFT(1:NFFT/2+1))) % using semilogx as huge DC component
title('Single-Sided Amplitude Spectrum of Ax')
xlabel('Frequency (Hz)')
ylabel('|Ax(f)|')
ylim([0 300])
giving the following result:
I'm trying to compute the Fourier coefficients for a waveform using MATLAB. The coefficients can be computed using the following formulas:
T is chosen to be 1 which gives omega = 2pi.
However I'm having issues performing the integrals. The functions are are triangle wave (Which can be generated using sawtooth(t,0.5) if I'm not mistaking) as well as a square wave.
I've tried with the following code (For the triangle wave):
function [ a0,am,bm ] = test( numTerms )
b_m = zeros(1,numTerms);
w=2*pi;
for i = 1:numTerms
f1 = #(t) sawtooth(t,0.5).*cos(i*w*t);
f2 = #(t) sawtooth(t,0.5).*sin(i*w*t);
am(i) = 2*quad(f1,0,1);
bm(i) = 2*quad(f2,0,1);
end
end
However it's not getting anywhere near the values I need. The b_m coefficients are given for a
triangle wave and are supposed to be 1/m^2 and -1/m^2 when m is odd alternating beginning with the positive term.
The major issue for me is that I don't quite understand how integrals work in MATLAB and I'm not sure whether or not the approach I've chosen works.
Edit:
To clairify, this is the form that I'm looking to write the function on when the coefficients have been determined:
Here's an attempt using fft:
function [ a0,am,bm ] = test( numTerms )
T=2*pi;
w=1;
t = [0:0.1:2];
f = fft(sawtooth(t,0.5));
am = real(f);
bm = imag(f);
func = num2str(f(1));
for i = 1:numTerms
func = strcat(func,'+',num2str(am(i)),'*cos(',num2str(i*w),'*t)','+',num2str(bm(i)),'*sin(',num2str(i*w),'*t)');
end
y = inline(func);
plot(t,y(t));
end
Looks to me that your problem is what sawtooth returns the mathworks documentation says that:
sawtooth(t,width) generates a modified triangle wave where width, a scalar parameter between 0 and 1, determines the point between 0 and 2π at which the maximum occurs. The function increases from -1 to 1 on the interval 0 to 2πwidth, then decreases linearly from 1 to -1 on the interval 2πwidth to 2π. Thus a parameter of 0.5 specifies a standard triangle wave, symmetric about time instant π with peak-to-peak amplitude of 1. sawtooth(t,1) is equivalent to sawtooth(t).
So I'm guessing that's part of your problem.
After you responded I looked into it some more. Looks to me like it's the quad function; not very accurate! I recast the problem like this:
function [ a0,am,bm ] = sotest( t, numTerms )
bm = zeros(1,numTerms);
am = zeros(1,numTerms);
% 2L = 1
L = 0.5;
for ii = 1:numTerms
am(ii) = (1/L)*quadl(#(x) aCos(x,ii,L),0,2*L);
bm(ii) = (1/L)*quadl(#(x) aSin(x,ii,L),0,2*L);
end
ii = 0;
a0 = (1/L)*trapz( t, t.*cos((ii*pi*t)/L) );
% now let's test it
y = ones(size(t))*(a0/2);
for ii=1:numTerms
y = y + am(ii)*cos(ii*2*pi*t);
y = y + bm(ii)*sin(ii*2*pi*t);
end
figure; plot( t, y);
end
function a = aCos(t,n,L)
a = t.*cos((n*pi*t)/L);
end
function b = aSin(t,n,L)
b = t.*sin((n*pi*t)/L);
end
And then I called it like:
[ a0,am,bm ] = sotest( t, 100 );
and I got:
Sweetness!!!
All I really changed was from quad to quadl. I figured that out by using trapz which worked great until the time vector I was using didn't have enough resolution, which led me to believe it was a numerical issue rather than something fundamental. Hope this helps!
To troubleshoot your code I would plot the functions you are using and investigate, how the quad function samples them. You might be undersampling them, so make sure your minimum step size is smaller than the period of the function by at least factor 10.
I would suggest using the FFTs that are built-in to Matlab. Not only is the FFT the most efficient method to compute a spectrum (it is n*log(n) dependent on the length n of the array, whereas the integral in n^2 dependent), it will also give you automatically the frequency points that are supported by your (equally spaced) time data. If you compute the integral yourself (might be needed if datapoints are not equally spaced), you might calculate frequency data that are not resolved (closer spacing than 1/over the spacing in time, i.e. beyond the 'Fourier limit').
I know this is pretty obvious but I'm having some trouble with a simple calculation.
I have an object that has X and Y components for its speed.
I am calculating its total speed simply by square root of squares of the X and Y components:
var totalSpeed:Number = Math.sqrt(b.currentSpeedY * b.currentSpeedY + b.currentSpeedX * b.currentSpeedX);
I also have a variable called divergence, which is guaranteed to be between -1 and 1. According to the divergence, I calculate my new X component of the speed after the collision, by multiplying divergence and total speed:
var sX:Number = -totalSpeed * divergence;
Now, as I have the new X speed, and the total speed, I simply obtain my new Y speed by subtracting the square of my new X value from total speed, and taking its square root:
var sY:Number = -Math.sqrt(totalSpeed - (sX * sX));
Here is my problem: The total speed before and after the calculations don't match. I can confirm this by both printing the total speed (root of sum of squares) before and after the collision, and by simply looking at the speed of the objects visually. After collision, the speed of the object always tends to be slower.
I am obviously missing something very simple somewhere, but unfortunatelly couldn't find it anywhere. Where is the error that prevents total sums from matching?
In the sY calculatin it should be
var sY:Number = -Math.sqrt(totalSpeed * totalSpeed - (sX * sX));