shiyan.m

kalman滤波程序 关于信息融合方面的程序

% PURPOSE : To estimate the states of the following nonlinear, 
%           nonstationary state space model:

%           using the sequential importance-samping resampling
%           algorithm.                           
             
% AUTHOR  : Nando de Freitas - Thanks for the acknowledgement :-)
% DATE    : 08-09-98


clear;
echo off;

% INITIALISATION AND PARAMETERS:
% =============================

N = 80;                % Number of time steps.
t = 1:1:N;             % Time.  
%x0 = [0 0 0 0]';       % Initial state.
x0 = [50 -30 15000  -50]';%x0 = [0 230 15000 126]';
x = zeros(4,N);        % Hidden states.
y = zeros(2,N);        % Observations.
x(:,1) = x0;           % Initial state. 
%R = eye(2);            % Measurement noise variance.
R=[30 0;0 50];
%Q =eye(4);           % Process noise variance.
Q=[3 0 0 0;0 3 0 0;0 0 3 0;0 0 0 3];
%initVar =eye(4);     % Initial variance of the states.  
initVar =[30 0 0 0;0 30 0 0;0 0 30 0;0 0 0 30];
numSamples=500;         % Number of Monte Carlo samples per time step. 
M=80;       %time
xc=zeros(4,M);         
xoo=50000;    %圆心
yoo=50000;    %圆心
xcx0=50000;    %观测器起点位置
xcy0=35000;    %观测器起点位置
r0=pi/180;    
l=sqrt(15000^2+15000^2-2*15000*15000*cos(pi/180));   %一步弦长
R1=15000;          %半径
vo0=R1*M*r0/M;     %观测器线速度
F=[1 1 0 0
    0 1 0 0
    0 0 1 1
    0 0 0 1];  
n=M;           %定义扩展卡尔曼用到的变量
s=[x(1,:);x(2,:);x(3,:);x(4,:)];
H=zeros(2,4,n);              %雅可比矩阵H，维数2*4
ss=zeros(4,n);% 精确值
sn=zeros(4,n); % 估计修正值
hs=zeros(2,n);
r=zeros(2,n);
NN=zeros(4,4,n);
csz=x0;
p=initVar;
MM=zeros(4,4,n); % 一步预测均方误差
K=zeros(4,2,n); % 卡尔曼增益
for t=1:M                            %计算传感器运动
    if t==1
        xc(1,t)=cos(pi/360)*l;
        xc(3,t)=sin(pi/360)*l;
        xc(2,t)=cos(pi/360)*vo0;
        xc(4,t)=sin(pi/360)*vo0;
        xc(1,t)=xcx0+xc(1,t);
        xc(3,t)=xcy0+xc(3,t);
    else
        xc(1,t)=l*cos(pi/2-atan((yoo-xc(3,t-1))/(xc(1,t-1)-xoo))+pi/360);
        xc(3,t)=l*sin(pi/2-atan((yoo-xc(3,t-1))/(xc(1,t-1)-xoo))+pi/360);
        xc(2,t)=vo0*cos(pi/2-atan((yoo-xc(3,t-1))/(xc(1,t-1)-xoo))+pi/360);
        xc(4,t)=vo0*sin(pi/2-atan((yoo-xc(3,t-1))/(xc(1,t-1)-xoo))+pi/360);
        xc(1,t)=xc(1,t)+xc(1,t-1);
        xc(3,t)=xc(3,t)+xc(3,t-1);
    end
end;
for i=1:M-1
    U(:,i)=xc(:,i+1)-F*xc(:,i);
end



% GENERATE PROCESS AND MEASUREMENT NOISE:
% ======================================

v = 4*randn(2,N);
w = sqrt(Q)*randn(4,N);


% GENERATE STATE AND MEASUREMENTS:
% ===============================

y(1,1) =sqrt(x(1,1)^2+x(3,1)^2)+ v(1,1); 
y(2,1) =(x(1,1)*x(2,1)+x(3,1)*x(4,1))/y(1,1)+v(2,1);
for t=2:N,
  x(:,t) = F*x(:,t-1)+U(:,t-1)+w(:,t-1);
  %y(:,t) = sqrt(x(1,t)^2+x(3,t)^2,x(2,t)^2+x(4,t)^2)' + v(:,t); 
  y(1,t) =sqrt(x(1,t)^2+x(3,t)^2)+ v(1,t); 
  y(2,t) =(x(1,t)*x(2,t)+x(3,t)*x(4,t))/y(1,t)+ v(2,t);
end;
figure(1)
plot(1:80,x(1,:),'b:')
figure(2)
plot(1:80,x(2,:),'b:')
figure(3)
plot(1:80,x(3,:),'b:')
figure(4)
plot(1:80,x(4,:),'b:')