how to skip the measured object for certain time using kalman filter ?

2 views (last 30 days)
r = 11.01541; % range
phi = 103.6669; % azimuth
theta = 2.624099; % elevation
% 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);
%% implementation of kalman filter
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)+randn(3,N); % generated randm error the measurement of v
z = H*xt+v; % measured matrix
%% perform the kalman filter estimation
x = zeros(3,N);
x(:,1) = [-5.4493 0.0308 -11.0152];
%%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
%%plot the results
plot(t,z,'r-',t,xt(1,:),'g:',t,x(1,:),'b:','Linewidth',2)
xlabel('time');
ylabel('position');
title('position of x');
legend('measured object','predict the object','correct/update the object');
  1 Comment
Image Analyst
Image Analyst on 19 Jul 2020
I have no idea what you mean, unless you just want to put gaps (NaN's) in the curves where nothing plots, but I did format your code as code for you and attached the screenshot. You attached a .fig, and if I click that, it brings up a question about what program to use and if I say the latest MATLAB, it launches a whole new instance of MATLAB (so now I have two MATLABs running). Not convenient at all compared to just seeing the plot right here in Answers instantly.

Sign in to comment.

Accepted Answer

swathi
swathi on 2 Aug 2020
Edited: swathi on 2 Aug 2020
how can we read a matrix loop column by column xt(:,k) = 3*50
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

More Answers (0)

Tags

Community Treasure Hunt

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

Start Hunting!