%?author?:??Perry.Li?@USTC
%?function:?simulating?the?process?of?EKF
%?date:?????04/28/2015
%?
N?=?50;?????????%計算連續(xù)N個時刻?
n=3;????????????%狀態(tài)維度
Q=0.1^2*eye（n）;?%過程方差
R=0.04;?????????%測量值的方差?
f=@（x）[x（2）;x（3）;0.05*x（1）*（x（2）+x（3））];??%狀態(tài)方程
h=@（x）[x（1）;x（2）;x（3）];???????????????????%測量方程
s=[0;0;1];????????????????????????????????%初始狀態(tài)
%初始化狀態(tài)
x=s+q*randn（31）;?????????????????????????
P?=?eye（n）;???????????????????????????????
xV?=?zeros（nN）;??????????
sV?=?zeros（nN）;?????????
zV?=?zeros（nN）;
for?k=1:N
??z?=?h（s）?+?r*randn;?????????????????????
??sV（:k）=?s;?????????????????????????????%實際狀態(tài)
??zV（:k）??=?z;???????????????????????????%狀態(tài)測量值
??[x1A]=jaccsd（fx）;?????????????????????%計算f的雅可比矩陣，其中x1對應黃金公式line2
??P=A*P*A‘+Q;?????????????????????????????%過程方差預測，對應line3
??[z1H]=jaccsd（hx1）;????????????????????%計算h的雅可比矩陣
??K=P*H‘*inv（H*P*H‘+R）;???????????????????%卡爾曼增益，對應line4
??x=x1+K*（z-z1）;??????????????????????????%狀態(tài)EKF估計值，對應line5
??P=P-K*H*P;??????????????????????????????%EKF方差，對應line6
??xV（:k）?=?x;????????????????????????????%save
??s?=?f（s）?+?q*randn（31）;????????????????%update?process?
end
for?k=1:3
??FontSize=14;
??LineWidth=1;
??figure（）;
??plot（sV（k:）‘g-‘）;?%畫出真實值
??hold?on;
??plot（xV（k:）‘b-‘‘LineWidth‘LineWidth）?%畫出最優(yōu)估計值
??hold?on;
??plot（zV（k:）‘k+‘）;?%畫出狀態(tài)測量值
??hold?on;
??legend（‘真實狀態(tài)‘?‘EKF最優(yōu)估計估計值‘‘狀態(tài)測量值‘）;
??xl=xlabel（‘時間（分鐘）‘）;
??t=[‘狀態(tài)?‘num2str（k）]?;
??yl=ylabel（t）;
??set（xl‘fontsize‘FontSize）;
??set（yl‘fontsize‘FontSize）;
??hold?off;
??set（gca‘FontSize‘FontSize）;
end

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

?????文件????????275??2015-04-26?17:36??jaccsd.m

?????文件???????1716??2015-04-28?22:08??ekf.m

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

?????????????????1991????????????????????2