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