Filter löschen
Filter löschen

How can I input more than one value in function

1 Ansicht (letzte 30 Tage)
Tipu Sultan
Tipu Sultan am 16 Mai 2019
Kommentiert: Stephen23 am 16 Mai 2019
My function is as follows:
function [R,A] = MyEKFFun(theta1,theta2,theta3,r1,r2,r3,t1,t2,t3,T)
%% Function to implement Extended Kalman Filter
% Initialize S,Esimator Vector,Pre allocate memory to diferent partial derivatives
S = [100 0 0 0 0 0;
0 100 0 0 0 0;
0 0 100 0 0 0;
0 0 0 100 0 0;
0 0 0 0 100 0;
0 0 0 0 0 100];
prev_S = S;
Big_lambda = eye(3);
a=0;
b=0;
c=0;
p=0;
q=0;
s=0;
est_vec=[ a ; b; c; p; q; s];
%theta = [ 31 24 18] ;
%t= [ 1 1.1 1.2 ];
%r= [330 364 379];
%% Initialize angles,range,time
% Intialize angles,range and time from the input given to the function when
% called the function
theta = [ theta1 theta2 theta3]; % theta defines the angles measured from the 'BallDetection' function
t= [ t1 t2 t3 ]; % t defines the time of each image captured
r= [r1 r2 r3]; % r defines distance measured from the 'BallDetection' function
%% Input Measurement vector
% Create a matrix with the angles,range,time as input measurement vector
x = [ r; theta ; t]; % Input Measurement vector
%dif_x=zeros(2,3);
%% Preallocation various matrices
% Preallocate memory and create measurement vector
dif_a = arrayfun(@(t) [t^2 t 1 0 0 0; 0 0 0 t^2 t 1],t,'UniformOutput',false); % New Measurement vector
%time=2;
time=T;
dif_x = zeros(2, 3, 3); % Pre-allocation
w = zeros(2, 1, 3); % Pre-allocation
%pred_x = zeros(3, 3, 3);
W = zeros(2, 2); % Pre-allocation
y = zeros(2, 1, 3); % Pre-allocation
K = zeros(6, 2, 3); % Pre-allocation
%% Time update (Prediction)
% Repeat until the predefined condition is met and update estimator vector,Kalman gain,'S'.
for i=1:3
%% Calculte various matrices for a given iteration
% Calculate different partial dervatives,noise,kalman gain,measurement
% vector,new estimator vector
dif_x(:, :, i) = [-cos(theta(i)), r(i).*sin(theta(i)), 2*a.*t(i)+b;...
-sin(theta(i)), -r(i).*cos(theta(i)), 2*p.*t(i)+q];
W(:,:,i) = dif_x(:,:,i) * Big_lambda(i) * dif_x(:,:,i)';
pred_x = x(:,i) + [ 21.15; 0.06; 0.08] % Predicted Input Measurement Vector
w(:,:,i) = dif_x(:,:,i) * (x(:,i) - pred_x); % Measurement noise vector
y(:,:,i) = dif_a{i} * est_vec + w(:,:,i); % New Measurement vector
K(:,:,i) = prev_S*dif_a{i}'/((W(:,:,i) + dif_a{i}*prev_S*dif_a{i}')); % Kalman Gain
%% Update
% Update until a precondition is met
S_new = (eye(6) - (K(:,:,i) * dif_a{i})) * prev_S; % Upadte 'S'
prev_S = S_new;
est_vec_new = est_vec + K(:,:,i)*(y(:,:,i)-dif_a{i}*est_vec); % Measue new estimator vector
cond = abs(est_vec_new - est_vec);
if cond < 0.003 % Check the condition
break
end
est_vec = est_vec_new; % Update the estimator vector
a=est_vec(1,1); % Time rate of change of velocity
b=est_vec(2,1); % Velocity corresponding direction
c=est_vec(3,1); % Initial displacement with respect to S5
p=est_vec(4,1); % Time rate of change of velocity
q=est_vec(5,1); % Velocity corresponding direction
s=est_vec(6,1); % Initial displacement with respect to S3
%X(i) = a*time.^2+b*time+c; % To calculate x co-ordinate for t>=3
%Y(i) = p*time.^2+q*time+s; % To calculate y co-ordinate for t>=3
%figure,plot(t,meas_equa1)
%new_t=[t,time];
%figure,plot(r_cosTheta,r_sineTheta)
%% Co-ordinate determenation
% Calculate X,Y co-ordinates for t>=time
X = abs(a*time.^2+b*time+c); % To calculate x co-ordinate for t>=time
Y = abs(p*time.^2+q*time+s); % To calculate y co-ordinate for t>=time
R(i) = sqrt(X^2+Y^2); % To calculate next Range
A(i) = pred_x(2,1) + atan(Y/X) - 5; % To calculate new theta
%plot(pred_x,R)
%hold on
plot(theta,r,'--bo')
hold on
plot(A,R,'--ro')
end
end
In the function right now I am giving 3 inputs but I want to suppy 9 inputs each for theta,r & t so should I writen like:
function [R,A] = MyEKFFun(theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8.theta9,r1,r2,r3,r4,r5,r6,r7,r8,r9,t1,t2,t3,t4,t5,t6,t7,t8,t9,T)
So I want to know is there any other way to input the 9 values each for theta,r & t in a more robust and user friendly manner.
Thanks in advance.
  3 Kommentare
Tipu Sultan
Tipu Sultan am 16 Mai 2019
How do I do that if you give an example that will be very helpful.
Stephen23
Stephen23 am 16 Mai 2019
function [R,A] = MyEKFFun(theta,r,t,T)

Melden Sie sich an, um zu kommentieren.

Antworten (1)

aymane souani
aymane souani am 16 Mai 2019
You can use instead of theta1, ... etc an array of your values.
So the input will be only 3 arrays .
For example :
function [R, A] = MyEKFun (theta, r, t, T)
while :
theta = [theta1 theta2 theta3 theta4 theta5...]
The same for r and t.

Kategorien

Mehr zu Online Estimation finden Sie in Help Center und File Exchange

Produkte


Version

R2015b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by