clear?all;close?all;clc;
ts=1;%%采樣時間
t=[0:ts:100];
T=length（t）;
Q=20;R=16;
vs=randn（1T）;
vs=vs/std（vs）;
vs=vs-mean（vs）;
a=0;
b=sqrt（R）;
vs=a+b*vs;%產方差為16的高斯白噪聲序列;
wa=randn（1T）;
wa=wa/std（wa）;
wa=wa-mean（wa）;
a=0;
b=sqrt（Q）;
wa=a+b*wa;%產方差為20的高斯白噪聲序列;
%%初始狀態
x=[1000?50?20]‘;%
x00=[990;0;0];%
p00=[10?0?0;0?100?0;0?0?100];
%%連續狀態空間模型
%x_dot（t）=Ax（t）+Bu（t）
%z（t）=Cx（t）+Du（t）;
A=[0?1?0;0?0?1;0?0?0];%
B=[0;0;1];%
C=[1?0?0];%
D=[1];%
%%離散狀態空間模型
%x（k+1）=Fx（k）+Gw（k）
%z（k）=Hx（k）+Iv（k）;
%連續形式的離散化
sysc=ss（ABCD）;
sysd=c2d（syscts‘zoh‘）;
[F?G?H?I?]=ssdata（sysd）;
%?F=eye（3）+A;
phi=F;
for?i=1:T-1
????x（:i+1）=F*x（:i）;
end
%x=x+w;
m=0;
x=x+G*wa;
z=H*x+I*vs;%計算觀測值

for?d=1:T-1
x10=phi*x00;
p10=phi*p00*phi‘+G*20*G‘;
K=p10*H‘*inv（H*p10*H‘+R）;
x11=x10+K*（z（:d）-H*x10）;
p11=p10-K*H*p10;
????p00=p11;
????x00=x11;
????jie（d）=x11（1）;%位置
????su（d）=x11（2）;%速度
????jia（d）=x11（3）;%加速度
???
end?????%卡爾曼濾波
for?d=1:T-1
??weicha（d）=jie（d）-x（1d）;?%位置誤差
???sucha（d）=su（d）-x（2d）;%速度誤差
???jiacha（d）=jia（d）-x（3d）;%加速度誤差
end
t=0:1:T-2;
d=0:1:T-1;
figure（1）;
plot（tjie‘g+‘）;
hold?on;
plot（dx（1:）‘r‘）;
figure（2）;
plot（tsu‘g+‘）;
hold?on;
plot（dx（2:）‘r‘）;
figure（3）;
plot（tjia‘g--‘）;
hold?on;
plot（dx（3:）‘r‘）;
figure（4）
plot（tweicha）;
figure（5）
plot（tsucha）;
figure（6）;
plot（tjiacha）;
mean1=mean（jia）%加速度均值
mean2=mean（weicha）%位置誤差均值
mean3=mean（sucha）%速度誤差均值
mean4=mean（jiacha）%加速度誤差均值
jiuncha1=std（jia）^2%加速度方差
jiuncha2=std（weicha）^2%位置誤差方差
jiuncha3=std（sucha）^2?%速度誤差方差
jiuncha4=std（jiacha）^2%加速度誤差方差

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件?????133632??2015-06-20?23:02??辛弄潮?201230050602021201.doc

?????文件???????1725??2015-06-20?22:58??test.m

-----------?---------??----------?-----??----

???????????????135357????????????????????2