python 预测 位置_缺少测量值且仅位置已知时的卡尔曼滤波器预测

function[]=main()[t,accX,velX,posX,accY,velY,posY,t_sens,posX_sens,posY_sens,posX_var,posY_var]=generate_signals();n=numel(t_sens);%state matrix

X=zeros(6,1);%covariance matrix

P=diag([0.001,0.001,10,10,2,2]);%system noise

Q=diag([50,50,5,5,3,0.4]);dt=t_sens(2)-t_sens(1);%transition matrix

F=[1,0,dt,0,0.5*dt^2,0;0,1,0,dt,0,0.5*dt^2;0,0,1,0,dt,0;0,0,0,1,0,dt;0,0,0,0,1,0;0,0,0,0,0,1];%observation matrix

H=[100000;010000];%measurement noise

R=diag([posX_var,posY_var]);%kalman filter output through the whole time

X_arr=zeros(n,6);%fusionfori=1:n

y=[posX_sens(i);posY_sens(i)];if(i==1)[X]=init_kalman(X,y);%initialize the state using the1stsensorelseif(i>=40&&i<=58)%missing measurements between40ans58sec[X,P]=prediction(X,P,Q,F);else[X,P]=prediction(X,P,Q,F);[X,P]=update(X,P,y,R,H);end

end

X_arr(i,:)=X;end

figure;subplot(3,1,1);plot(t,posX,'LineWidth',2);hold on;plot(t_sens,posX_sens,'.','MarkerSize',18);plot(t_sens,X_arr(:,1),'k.','MarkerSize',14);hold off;grid on;title('PositionX');legend('Ground Truth','Sensor','Estimation');subplot(3,1,2);plot(t,velX,'LineWidth',2);hold on;plot(t_sens,X_arr(:,3),'k.','MarkerSize',14);hold off;grid on;title('VelocityX');legend('Ground Truth','Estimation');subplot(3,1,3);plot(t,accX,'LineWidth',2);hold on;plot(t_sens,X_arr(:,5),'k.','MarkerSize',14);hold off;grid on;title('AccX');legend('Ground Truth','Estimation');figure;subplot(3,1,1);plot(t,posY,'LineWidth',2);hold on;plot(t_sens,posY_sens,'.','MarkerSize',18);plot(t_sens,X_arr(:,2),'k.','MarkerSize',14);hold off;grid on;title('PositionY');legend('Ground Truth','Sensor','Estimation');subplot(3,1,2);plot(t,velY,'LineWidth',2);hold on;plot(t_sens,X_arr(:,4),'k.','MarkerSize',14);hold off;grid on;title('VelocityY');legend('Ground Truth','Estimation');subplot(3,1,3);plot(t,accY,'LineWidth',2);hold on;plot(t_sens,X_arr(:,6),'k.','MarkerSize',14);hold off;grid on;title('AccY');legend('Ground Truth','Estimation');figure;plot(posX,posY,'LineWidth',2);hold on;plot(posX_sens,posY_sens,'.','MarkerSize',18);plot(X_arr(:,1),X_arr(:,2),'k.','MarkerSize',18);hold off;grid on;title('Trajectory');legend('Ground Truth','Sensor','Estimation');axis equal;end

function[t,accX,velX,posX,accY,velY,posY,t_sens,posX_sens,posY_sens,posX_var,posY_var]=generate_signals()dt=0.01;t=(0:dt:70)';

posX_var = 8; % m^2

posY_var = 8; % m^2

posX_noise = randn(size(t))*sqrt(posX_var);

posY_noise = randn(size(t))*sqrt(posY_var);

accX = sin(0.3*t) + 0.5*sin(0.04*t);

velX = cumsum(accX)*dt;

posX = cumsum(velX)*dt;

accY = 0.1*sin(0.5*t)+0.03*t;

velY = cumsum(accY)*dt;

posY = cumsum(velY)*dt;

t_sens = t(1:100:end);

posX_sens = posX(1:100:end) + posX_noise(1:100:end);

posY_sens = posY(1:100:end) + posY_noise(1:100:end);

end

function [X] = init_kalman(X, y)

X(1) = y(1);

X(2) = y(2);

end

function [X, P] = prediction(X, P, Q, F)

X = F*X;

P = F*P*F'+Q;end

function[X,P]=update(X,P,y,R,H)Inn=y-H*X;S=H*P*H' + R;

K = P*H'/S;X=X+K*Inn;P=P-K*H*P;end