How can I input more than one value in function
3 ビュー (過去 30 日間)
古いコメントを表示
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 件のコメント
回答 (1 件)
aymane souani
2019 年 5 月 16 日
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.
0 件のコメント
参考
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!