How can I input more than one value in function

2 ビュー (過去 30 日間)
Tipu Sultan
Tipu Sultan 2019 年 5 月 16 日
コメント済み: Stephen23 2019 年 5 月 16 日
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 件のコメント
Tipu Sultan
Tipu Sultan 2019 年 5 月 16 日
How do I do that if you give an example that will be very helpful.
Stephen23
Stephen23 2019 年 5 月 16 日
function [R,A] = MyEKFFun(theta,r,t,T)

サインインしてコメントする。

回答 (1 件)

aymane souani
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.

カテゴリ

Help Center および File ExchangeOnline Estimation についてさらに検索

製品


リリース

R2015b

Community Treasure Hunt

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

Start Hunting!

Translated by