%aplha.m


%memo:term?project


%//////???initialization
clear?all;
close?all;
clc;
tic
%//////?define?some?paramater

runs=200;?%the?filiter?will?run?200?times

steps=50;?%?50?steps?a?time?

T=1;

G=[T*T/2????0;
????T???????0;
????0???T*T/2;
????0???????T;];

F=[1?T?0?0;
???0?1?0?0;
???0?0?1?T;
???0?0?0?1];??%Xk+1=F*Xk+G*Wk

H=[1?0?0?0;
???0?0?1?0;];%Zk=H*Xk+Vk

Q=diag（[0.04?0.09]）;?%variance?of?process?noise?
R=diag（[900?900]）;??%varicance?of?measurement?noise
aver_x_zero=[?5?20?6?18]‘;?
p_x_zero=diag（[400?64?625?49]）;
x_err_alp=zeros（4stepsruns）;

faiw_x=0.2;
faiw_y=0.3;
faiv_x=30;
faiv_y=30;

lm_x=faiw_x*T^2/faiv_x
lm_y=faiw_y*T^2/faiv_y

alp_x=-1/8*（lm_x^2+8*lm_x-（lm_x+4）*sqrt（lm_x^2+8*lm_x））;
beta_x=1/4*（lm_x^2+4*lm_x-lm_x*sqrt（lm_x^2+8*lm_x））;

alp_y=-1/8*（lm_y^2+8*lm_y-（lm_y+4）*sqrt（lm_y^2+8*lm_y））;
beta_y=1/4*（lm_y^2+4*lm_y-lm_y*sqrt（lm_y^2+8*lm_y））;

K_alp=[alp_x?0;
???????beta_x/T?0;
???????0??alp_y;
???????0?beta_y/T];



randn（‘state‘sum（100*clock））;??%設(shè)置生成器在每次使用時(shí)的狀態(tài)都不同（因?yàn)閏lock每次都不同）

%//////////////////////////////////


%/////////////////alp?filiter

for?index=1:runs??%?濾波器已經(jīng)運(yùn)行幾次
??index
??
??%濾波器初始化
??
??x=sqrtm（p_x_zero）*randn（41）+aver_x_zero?;?%產(chǎn)生x0
??x_update_alp=aver_x_zero?;
??p_update_alp=p_x_zero;
??%//////////////////////////////
??
??????for?k=1:steps
??????????%/產(chǎn)生真值
??????????x=F*x+G*sqrtm（Q）*rand（21）;
??????????%量測(cè)
??????????z=H*x+sqrtm（R）*rand（21）;


??????????%prediction
??????????x_pre_alp=F*x_update_alp;?%輸入為0
???????%???p_pre_alp=F*p_update_alp*F‘+G*Q*G‘;
??????????z_pre=H*x_pre_alp;


??????????%update
?????????%?S_alp=H*p_pre_alp*H‘+R‘;
???????%???K_alp=p_pre_alp*H‘*inv（S_alp）;
??????????x_update_alp=x_pre_alp+K_alp*（z-z_pre）;
????????%??p_update_alp=p_pre_alp-K_alp*S_alp*K_alp‘;
??????????
??????????x_err_alp（:kindex）=x-x_update_alp;
??????end?
end

for?index=1:runs
????index
????
????pos_err_square_alp（index:）=sum（x_err_alp（[1?3]:index）.^2）;
????vel_err_square_alp（index:）=sum（x_err_alp（[2?4]:index）.^2）;
end

%計(jì)算RMSE
RMS_alp=[（sqrt（mean（pos_err_square_alp）））;
????????（sqrt（mean（vel_err_square_alp）））;];
???????
????????
???%開(kāi)始畫(huà)圖?????
figure（1）;
time=1:50;

%plot（timeRMS_alp（1:）‘-k‘）
hndl=plot（timeRMS_alp（1:）‘o‘）;
set（hndl‘LineWidth‘1）;

figure（2）;
time=1:50;

%plot（timeRMS_alp（1:）‘-k‘）
hndl=plot（timeRMS_alp（2:）‘o‘）;
set（hndl‘LineWidth‘1）;

%ylim（[0?50]）;

??
????toc

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----
?????文件????????2571??2013-03-13?19:36??阿爾法貝塔濾波器\alpha.m
?????文件???????14336??2013-03-13?09:48??阿爾法貝塔濾波器\題目.doc
?????目錄???????????0??2013-03-13?19:37??阿爾法貝塔濾波器\