MATLAB Answers

how can we read matrix step by step in kalman filter

13 views (last 30 days)
swathi
swathi on 2 Aug 2020 at 17:41
Commented: swathi on 3 Aug 2020 at 6:42
my matrix size is 3*50 %% xt(:,k) = 3*50 having [r,phi,theta]
in this xt[3*50] matrix every time we can read single column by column r,phi,theta values
every time we read 3*1 matrix values ,,till 3*50
r = rand(1,50); % range
phi = rand(1,50); % azimuth
theta = rand(1,50); % elevation
xt = [r;phi;theta]; %% state matrix
% convert polar coordinates of (r,theta,phi) to cartessian coordinates of(x1,y1,z1)
x1 = r.*sin(theta).*cos(phi);
y1 = r.*sin(theta).*sin(theta);
z1 = r.*cos(theta);
v = [x1;y1;z1];
N = 50; % no of time steps
dt = 0.05; % time for one cycle
t = dt*(1:N); % time vector
A = [1 0 dt
0 1 0
0 0 1]; % state transition matrix
B = [0.5*dt^2
dt
1]; % control input matrix
H = [1 0 0]; % measured matrix
u = 9.8062; % gravitational force
Q = 0; %No noise assumed
I = eye(3);
w = 0;
% initialize the state vector
xt = zeros(3,N); % state vector
xt(:,1) = [x1;y1;z1]; % the first set of states are initial position(x,y,z)
%% set the true states are generated using prediction equations
for k = 2:N
xt(:,k) = A*xt(:,k-1)+B*u+w;
end
% generated noise measurements from the true states
R = 4; % error variance in measurement of position
V = sqrt(R)+v; % generated randm error the measurement of v
z = H*xt+V; % measured matrix
%% perform the kalman filter estimation
x = zeros(3,N);
x(:,1) = [0 0 0];
%%perform the kalman filter estimation
p = [0.01 0 0
0 0.01 0
0 0 0.01];
% perform the estimation of N steps
for k = 2:N
% predict the state vector
x(:,k) = A*x(:,k-1)+B*u;
% predict the covariance matrix
P = A*p*A'+Q;
% calculate the kalman gain matrix
K = P*H'/(H*P*H'+R);
% update / correct the state vector
x(:,k) = x(:,k)+K*(z(k)-H*x(:,k));
% update the covariance matrix
P1 = (I-K*H)*P;
end

  2 Comments

swathi
swathi on 3 Aug 2020 at 2:34
xt = rand(3,50);
how can we read xt in every column by column,one column
if it reads (3*1) in xt position ,then after read (3*2) till (3*50)
reads xt file in for loop
A = [1 0 0
0 1 0
0 0 1]; %% 3*3 matrix size
dt = 0.5;
N = 50;
B = [0.5dt^2
dt
1]; %% 3*1 matrix size
u = 9.8;
w =0;
for k = 1:N
xt(:,k) = A*xt(:,k)+B*u+w;
end

Sign in to comment.

Accepted Answer

KALYAN ACHARJYA
KALYAN ACHARJYA on 3 Aug 2020 at 5:59
See you have mat variable xt = rand(3,50); if you wish to read thia xt variable matrix column by column then
1 Column=xt(:,1);
2 Column=xt(:,2);
3 Column=xt(:,3);
....
so on
if you wish to do same using loop, to save the invididual column (vector data, not a sacalar), you have to use cell array
xt=rand(3,50);
col_data=cells(1,50)
for i=1:50
col_data{i}=xt(:,i)
end
Multiple ways you can do the same, like splitting matrix to individual column vector etc. I have answered the question based on you comments.
Hope it helps! 🙂

  1 Comment

swathi
swathi on 3 Aug 2020 at 6:42
how can we write that loop in given equation? can you help me
for k = 1:50
xt(:,k) = A*xt(:,k)+B*u+w;
end

Sign in to comment.

More Answers (0)