阿尔法贝塔滤波器实例

阿尔法贝塔滤波器，是卡曼滤波器的简化，比卡曼滤波器速度快。

%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））;  %设置生成器在每次使用时的状态都不同（因为clock每次都不同）

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


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

for index=1:runs  % 滤波器已经运行几次
  index
  
  %滤波器初始化
  
  x=sqrtm（p_x_zero）*randn（41）+aver_x_zero ; %产生x0
  x_update_alp=aver_x_zero ;
  p_update_alp=p_x_zero;
  %//////////////////////////////
  
      for k=1:steps
          %/产生真值
          x=F*x+G*sqrtm（Q）*rand（21）;
          %量测
          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

%计算RMSE
RMS_alp=[（sqrt（mean（pos_err_square_alp）））;
        （sqrt（mean（vel_err_square_alp）））;];
       
        
   %开始画图     
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

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2571  2013-03-13 19:36  阿尔法贝塔滤波器\alpha.m
     文件       14336  2013-03-13 09:48  阿尔法贝塔滤波器\题目.doc
     目录           0  2013-03-13 19:37  阿尔法贝塔滤波器\