how to skip the measured object for certain time using kalman filter ?
1 次查看(过去 30 天)
显示 更早的评论
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');
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/334131/image.png)
1 个评论
Image Analyst
2020-7-19
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.
采纳的回答
更多回答(0 个)
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!