imshow(matrix(:,:,1))
%identify axes
[x y] = ginput(2);
% preallocate matrices
cog = zeros(size(matrix,3),1);
%cog
% loop start
for i = 1:size(maytrix,3)
I = matrix(:,:,i);
%n = ceil(norm([diff(x), diff(y)])); % A rough estimation of number of points
test = interp2(I, 2, linspace(x(1), x(2),n), linspace(y(1), y(2),n));
%test = round(test);
cog(i) = sum((1:length(test)).*test')/sum(test);
% loop end
end
scog = (cog - min(cog)) / (max(cog) - min(cog));
Here's a toy example to get you started.
% Create a 100x100x100 3D matrix with a certain pattern
% Smooth pattern:
matrix = [1 : 100] .' * [1 : 100];
matrix = matrix(:) * [1 : 100];
matrix = reshape( matrix, 100, 100, 100 );
% Alternatively, try a random matrix:
%matrix = randn(100,100,100);
Endpoints = randi( [1, 100], [3, 2] ); % Randomly get 2 3D points within matrix
Numpoints = max( abs( diff( Endpoints, 1, 2 ) ) ); % Choose width of widest dimension
% Create a line in 3D space (containing N points) going from one Endpoint to the other.
Linepoints = [ linspace( Endpoints(1, 1), Endpoints(1, 2), Numpoints );
linspace( Endpoints(2, 1), Endpoints(2, 2), Numpoints );
linspace( Endpoints(3, 1), Endpoints(3, 2), Numpoints ); ];
InterpolatedIntensities = interp3( 1:100, 1:100, 1:100, matrix, Linepoints(1, :), Linepoints(2, :), Linepoints(3, :) );
plot( InterpolatedIntensities );
Related
How can I find the zeros, maxima, and minima of the polynomial x^3 - 10x + 20?
So far, I have done the following
fhandle1 = #(x) x.^3 - 10.*x + 20;
figure(1);
hold on;
fplot(fhandle1,[0, 10]);
[xMin, yMin] = fminbnd(fhandle1, 2.5, 4);
plot(xMin, yMin, 'or')
[xzero,yzero] = fzero(fhandle1,5);
plot(xzero,yzero,'*g')
How can I find the maxima of the polynomial?
fminbnd will return the max/min value of a function, which isn't necessarily the same as the maxima/minima.
The most complete way to approach this for any given polynomial leans on two facts
Extrema of a function f(x) occur when f'(x) = 0
You can classify the extrema as maxima/minima by evaluating f''(x)
You can use roots to find all roots of a polynomial from its coefficients, and you can use polyder to find the derivative coefficients for a polynomial.
Output
Code
coeffs = [1, 0, -10, 20]; % 1*x^3 + 0*x^2 - 10*x + 20
% Find zeros
r = realRoots( coeffs );
% Get the coeffs of the derivative
dcoeffs = polyder( coeffs );
% Minima/maxima are at roots of derivative
dr = realRoots( dcoeffs );
% Get coeffs of next derivative
ddcoeffs = polyder( dcoeffs );
% Minima: ddcoeffs > 0, Maxima: ddcoeffs < 0, Inflection pt: ddcoeffs = 0
minima = dr( polyval( ddcoeffs, dr ) > 0 );
maxima = dr( polyval( ddcoeffs, dr ) < 0 );
inflection = dr( polyval( ddcoeffs, dr ) == 0 );
% Plot (ranges would need to be based on extrema for a generic polynomial)
figure(1); clf; hold on; grid on;
x = -10:0.1:4;
plot( x, polyval( coeffs, x ), 'LineWidth', 1 ); % function line
plot( r, polyval( coeffs, r ), 'ok', 'LineWidth', 2 ); % roots
plot( minima, polyval( coeffs, minima ), 'vk', 'LineWidth', 2 ); % minima
plot( maxima, polyval( coeffs, maxima ), '^k', 'LineWidth', 2 ); % maxima
plot( inflection, polyval( coeffs, inflection ), 'xk', 'LineWidth', 2 ); % inflection
ylim( [-20, 50] );
% Helper function to return the real roots from polynomial coeffs
function r = realRoots( c )
r = roots( c ); % all roots
r = r( imag(r) == 0 ); % real roots
end
function plotData(X, y)
%PLOTDATA Plots the data points X and y into a new figure
% PLOTDATA(x,y) plots the data points with + for the positive examples
% and o for the negative examples. X is assumed to be a Mx2 matrix.
% Create New Figure
figure; hold on;
% Find Indices of Positive and Negative Examples
pos = find(y == 1);
neg = find(y == 0);
% Plot Examples
plot(X(pos, 1), X(pos, 2), 'k+','LineWidth', 2, ...
'MarkerSize', 7);
plot(X(neg, 1), X(neg, 2), 'ko', 'MarkerFaceColor', 'y', ...
'MarkerSize', 7);
hold off;
end
RESULT
octave:17> plotData ()
error: 'y' undefined near line 10 column 12
error: called from
plotData at line 10 column 5
I need to use the Kalman filter to fuse multi-sensors positions for gaussian measurement (for example 4 positions as the input of the filter and 1 position as output). It is possible to help me with some examples or tutorials because all the examples I found are related to the estimation of the positions?
OPTION 1
Weighted Avarage
In this case you don't need to implement a real Kalman Filter. You just can use the signal variances to calculate the weights and then calculate the weighted avarage of the inputs. The weights can be found as an inverse of the variances.
So if you have two signals S1 and S2 with variances V1 and V2, then the fused result would be
A fusion example can be seen on the next plot.
I simulated two signals. The variance of the second signal changes over the time. As long as it's smaller than the variance of the first signal the fused result is close to the second signal. It is not the case when the variance of the second signal is too high.
OPTION 2
Kalman Filter with Multiple Update Steps
The classical Kalman Filter uses prediction and update steps in a loop:
prediction
update
prediction
update
...
In your case you have 4 independent measurements, so you can use those readings after each other in separate update steps:
prediction
update 1
update 2
update 3
update 4
prediction
update 1
...
A very nice point is that the order of those updates does not matter! You can use updates 1,2,3,4 or 3,2,4,1. In both cases you should get the same fused output.
Compared to the first option you have following pros:
You have a variance propogation
You have the system noise matrix Q,
so you can control the smoothness of the fused output
Here is my matlab code:
function [] = main()
% time step
dt = 0.01;
t=(0:dt:2)';
n = numel(t);
%ground truth
signal = sin(t)+t;
% state matrix
X = zeros(2,1);
% covariance matrix
P = zeros(2,2);
% kalman filter output through the whole time
X_arr = zeros(n, 2);
% system noise
Q = [0.04 0;
0 1];
% transition matrix
F = [1 dt;
0 1];
% observation matrix
H = [1 0];
% variance of signal 1
s1_var = 0.08*ones(size(t));
s1 = generate_signal(signal, s1_var);
% variance of signal 2
s2_var = 0.01*(cos(8*t)+10*t);
s2 = generate_signal(signal, s2_var);
% variance of signal 3
s3_var = 0.02*(sin(2*t)+2);
s3 = generate_signal(signal, s3_var);
% variance of signal 4
s4_var = 0.06*ones(size(t));
s4 = generate_signal(signal, s4_var);
% fusion
for i = 1:n
if (i == 1)
[X, P] = init_kalman(X, s1(i, 1)); % initialize the state using the 1st sensor
else
[X, P] = prediction(X, P, Q, F);
[X, P] = update(X, P, s1(i, 1), s1(i, 2), H);
[X, P] = update(X, P, s2(i, 1), s2(i, 2), H);
[X, P] = update(X, P, s3(i, 1), s3(i, 2), H);
[X, P] = update(X, P, s4(i, 1), s4(i, 2), H);
end
X_arr(i, :) = X;
end
plot(t, signal, 'LineWidth', 4);
hold on;
plot(t, s1(:, 1), '--', 'LineWidth', 1);
plot(t, s2(:, 1), '--', 'LineWidth', 1);
plot(t, s3(:, 1), '--', 'LineWidth', 1);
plot(t, s4(:, 1), '--', 'LineWidth', 1);
plot(t, X_arr(:, 1), 'LineWidth', 4);
hold off;
grid on;
legend('Ground Truth', 'Sensor Input 1', 'Sensor Input 2', 'Sensor Input 3', 'Sensor Input 4', 'Fused Output');
end
function [s] = generate_signal(signal, var)
noise = randn(size(signal)).*sqrt(var);
s(:, 1) = signal + noise;
s(:, 2) = var;
end
function [X, P] = init_kalman(X, y)
X(1,1) = y;
X(2,1) = 0;
P = [100 0;
0 300];
end
function [X, P] = prediction(X, P, Q, F)
X = F*X;
P = F*P*F' + Q;
end
function [X, P] = update(X, P, y, R, H)
Inn = y - H*X;
S = H*P*H' + R;
K = P*H'/S;
X = X + K*Inn;
P = P - K*H*P;
end
And here is the result:
I have to generate a dataset of N data points, which are defined as t_n=f(x_n)+e, where e is drawn from normal distribution and f(x) is a nonlinear function.
For example, i have a function f(x)=x²+2x+10, how can i fill a vector v, such:
x = 1:1:100;
v = create(f(x)+normrnd(0,1),x);
Thank you
There are many methods to do this. Here I show you how to do it with anonymous functions http://www.gnu.org/software/octave/doc/v4.0.1/Anonymous-Functions.html#Anonymous-Functions
f = #(x) polyval ([1 2 10], x)
x = 1:100;
v = f(x) + normrnd (0, 1, size (x));
Or without a function:
x = 1:100;
v = x.^2 + 2.*x + 10 + normrnd (0, 1, size (x));
I've adjusted x here so that the noise is visible:
x = linspace (-3, 3);
v = f(x) + normrnd (0, 1, size (x));
plot (x, v)
grid on
Assuming a 3 dimensional irregular matrix where y = 1.5(x) and z = .5(y).
Further assuming an object starts at 0,0,0 and must move positively in at least two dimensions, and must move in all three dimensions (x+1, y+1, z-1 is okay, x+1, y+1, z=z is not). It may move any number of "spaces", but must move the same number in all directions.
The object is allowed to wraparound (x(max +1) = x(0)).
Move said object from its starting position to (0, max(y), .5(max(z))) For z, round up for fractions (end point in 4, 6, 3 matrix becomes 0, 6, 2)
Input is an Integer (X).
Output is the list of moves you would make (extra credit for showing the number of spaces you moved)
Sample Input/Output:
X = 4
Y = 6 //(4 * 1.5)
Z = 3 // 6 / 2
0, 0, 0 //Start
2, 5, 2 // (+2, -2, +2)
1, 2, 2 // (+4, +4, +4)
3, 4, 0 // (+2, +2, -2)
1, 6, 2 // (-2, +2, +2)
3, 3, 3 // (-3, -3, -3)
1, 5, 1 // (-2, +2, -2)
0, 6, 2 // (-1, +1, -1)
7 Moves.
Lua, 68 Characters
The long version below always solves the problem with one move by searching for the first all positive move that will solve problem.
x=...
y,z=x*3/2,x*3/4
a,b,c=0,y,math.ceil(z/2)
x,y,z=x+1,y+1,z+1
for i=1,math.huge do
if (x*i)%y==b and (x*i)%z==c then x=x*i break end
end
print("0,0,0\n0,"..b..","..c.."//+"..x..",+"..x..",+"..x.."\n1 move.")
Output for x = 12:
0,0,0
0,18,5//+455,+455,+455
1 move.
Output for x = 1000:
0,0,0
0,1500,375//+557424868,+557424868,+557424868
1 move.
Seems like the search could be replaced with some simple algebraic equation. But why stop there? Rules are easier to bend in golfing then doing the actual work.
So, assuming that there is always a single 1 move answer, and that I do not have to disclose the "number of spaces you moved", here is the 68 character golfed answer:
x=...print("0,0,0\n0,"..(x*3/2)..","..math.ceil(x*3/8).."\n1 move.")
Mathematica - Not Golfed
Just to see if we can get the ball rolling
... and trying to understand the problem ....
f[x_] := (
(* Init code *)
xmax = x;
ymax = 3 Round[xmax]/2;
zmax = Round[ymax]/2;
xobj = 0;
yobj = ymax;
zobj = Ceiling[zmax/2];
p = Join[Permutations[{1, 1, -1}], {{1, 1, 1}}];
Print["X = ", xmax, "\nY = ", ymax, "\nZ = ", zmax];
(* Loop *)
i = 0;
pos = {0, 0, 0};
k = "Start";
While[
(npos= {Mod[pos[[1]], xmax+1], Mod[pos[[2]], ymax+1], Mod[pos[[3]], zmax+1]})
!= {xobj, yobj, zobj},
i++;
Print[npos, " // ", k];
pos= npos+ (k= RandomInteger[{1,xmax}] p[[RandomInteger[{1, Length[p]}]]]);
];
Print[npos, " // ", k];
Print[i, " Moves"];
);
Invoke with
f[4]
Sample Output
X = 4
Y = 6
Z = 3
{0,0,0} // Start
{3,4,3} // {3,-3,3}
{0,0,2} // {-3,3,3}
{2,3,1} // {-3,3,3}
{0,6,2} // {3,3,-3}
4 Moves
Not sure if I'm following the rules ...