www.gusucode.com > 扩展卡尔曼滤波,粒子滤波,去偏卡尔曼滤波和循环增益尔曼滤波的源程序 > 去偏卡尔曼滤波/power_noise.m
function [real_xy,r,thita]=power_noise(ch,object_init_state,N,l,w,a) for i=1:N if i==1 real_xy(:,1)=ch*(object_init_state')+l*w(:,i); elseif i<30 real_xy(:,i)=ch*real_xy(:,i-1)+l*w(:,i); elseif i<80 real_xy(:,i)=ch*real_xy(:,i-1)+l*(w(:,i)+a(:,i)); else real_xy(:,i)=ch*real_xy(:,i-1)+l*w(:,i); end r(i)=sqrt(real_xy(1,i)^2+real_xy(3,i)^2); if real_xy(1,i)>0 thita(i)=atan(real_xy(3,i)/real_xy(1,i)); else thita(i)=atan(real_xy(3,i)/real_xy(1,i))+pi; end end