基于当前模型的UKF matlab程序

基于当前模型的matlab 程序由两部分组成 运行check_current 可以出图

clc;
clear;

TS=0.01;                               % simulation step     
alphax = 1/3;
alphay = 1/3;
F = [0 0 1 0 0 0;                      % current model 
    0 0 0 1 0 0; 
    0 0 0 0 1 0; 
    0 0 0 0 0 1; 
    0 0 0 0 -alphax 0; 
    0 0 0 0 0 -alphay];
F1 =[ 0 0 1 0 0 0;                       % real target model
      0 0 0 1 0 0; 
      0 0 0 0 1 0; 
      0 0 0 0 0 1; 
      0 0 0 0 0 0; 
      0 0 0 0 0 0];
G = [0 0; 0 0;0 0; 0 0; alphax 0; 0 alphay];
H = [1 0 0 0 0 0 ; 
    0 1 0 0 0 0 ];
[order_zorder]= size（H）;
syms s t;
PHAI = ilaplace（inv（eye（orderorder）*s-F））;
PHAI1 = ilaplace（inv（eye（orderorder）*s-F1））;
GK = int（PHAI*Gt0t）;
PHAI = subs（PHAItTS）;
PHAI1 = subs（PHAI1tTS）;
GK = subs（GKtTS）;
qx11=2*alphax*（1-exp（-2*alphax*TS）+2*alphax*TS+2*（（alphax*TS）^3）/3-2*（alphax^2）*（TS^2）-4*alphax*TS*exp（-alphax*TS））/（2*alphax^5）;
qx12=2*alphax*（1+exp（-2*alphax*TS）-2*exp（-alphax*TS）+2*alphax*TS*exp（-alphax*TS）-2*alphax*TS+alphax^2*TS^2）/（2*alphax^4）;
qx13=2*alphax*（1-exp（-2*alphax*TS）-2*alphax*TS*exp（-alphax*TS））/（2*alphax^3）;
qx22=2*alphax*（4*exp（-alphax*TS）-3-exp（-2*alphax*TS）+2*alphax*TS）/（2*alphax^3）;
qx23=2*alphax*（1+exp（-2*alphax*TS）-2*exp（-alphax*TS））/（2*alphax^2）;
qx33=2*alphax*（1-exp（-2*alphax*TS））/（2*alphax）;
qy11=2*alphay*（1-exp（-2*alphay*TS）+2*alphay*TS+2*（（alphay*TS）^3）/3-2*（alphay^2）*（TS^2）-4*alphay*TS*exp（-alphay*TS））/（2*alphay^5）;
qy12=2*alphay*（1+exp（-2*alphay*TS）-2*exp（-alphay*TS）+2*alphay*TS*exp（-alphay*TS）-2*alphay*TS+alphay^2*TS^2）/（2*alphay^4）;
qy13=2*alphay*（1-exp（-2*alphay*TS）-2*alphay*TS*exp（-alphay*TS））/（2*alphay^3）;
qy22=2*alphay*（4*exp（-alphay*TS）-3-exp（-2*alphay*TS）+2*alphay*TS）/（2*alphay^3）;
qy23=2*alphay*（1+exp（-2*alphay*TS）-2*exp（-alphay*TS））/（2*alphay^2）;
qy33=2*alphay*（1-exp（-2*alphay*TS））/（2*alphay）;
Q=[ qx11 0 qx12 0 qx13 0;
    0 qy11 0 qy12 0 qy13;
    qx12 0 qx22 0 qx23 0;
    0 qy12 0 qy22 0 qy23;
    qx13 0 qx23 0 qx33 0;
    0 qy13 0 qy23 0 qy33];
q0 = 5;
r = 2.5;
R = diag（[r^2r^2]）;

n = order;
m = order_z;
f=@（x）[PHAI*x（1:n1）+GK*x（n-n/3+1:n1）];        % nonlinear state equations
f1=@（x）[PHAI1*x（1:n1）];                         % target state equations
h=@（x）[H*x（1:n1）];                             % measurement equation
s=[15000;35000;0;-1300;2;0];                    % initial state
x=[15050;35050;1;-1280;1.5;0];                  % initial state with noise
P = diag（[100100404011]）;                    % initial state covraiance
N= 20000;                                      % total dynamic steps
xV = zeros（nN）;                                % estmate
sV = zeros（nN）;                                % allocate memory
zV = zeros（mN）;                                % actual

for k=1:N
    z = h（s） + R*randn（m1）;                    % measurments
    sV（:k）= s;                                 % save actual state
    zV（:k）  = z;                               % save measurment
    [x P] = ukf_Current（fxPhzQRq

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        3778  2014-11-20 13:47  ukf_Current.m
     文件        3627  2014-11-20 21:19  check_dangqian.m