my_ekf_sample.m

来自「用matlab程序编写的一个三维雷达跟踪粒子滤波器」· M 代码 · 共 36 行

function[x,y] = my_ekf_sample(F, Q, R, init_state, T)
%[os ss]=size(H);
format long;
os=3;   
ss=5;
% T=50;
x=zeros(ss,T);
y=zeros(os,T);
h_x=zeros(os,T);
x(:,1)=init_state;  %column vector

syms F_beta  F_r  F_alpha  r alpha  beta  w v xc yc zc;
F_r=sqrt(xc.^2+yc.^2+zc.^2);
F_alpha=atan(yc./xc);
F_beta=atan(zc./sqrt(xc.^2+yc.^2));
w=[F_r;F_alpha;F_beta];
v=[xc,yc,zc];

state_noise_samples=sample_gaussian(zeros(length(Q),1),Q,T)';% Note ' here! state_noise_samples is: ss-T
observer_noise_samples=sample_gaussian(zeros(length(R),1),R,T)';% observer_noise_samples is os-T

h_x(:,1)=subs(w,{xc,yc,zc},{x(1,1) x(3,1) x(5,1)});
y(:,1)=h_x(:,1)+observer_noise_samples(:,1);

for t=2:T
    x(:,t)=F*x(:,t-1)+state_noise_samples(:,t);
    y(:,t)=subs(w,{xc,yc,zc},{x(1,t) x(3,t) x(5,t)})+observer_noise_samples(:,t);
end






%%%%%%%%%%
% h_x_pred=subs(w,{x,y,z},{x_pred(1) x_pred(2) x_pred(5)}); h_x_pred=h(X)