%%?參數(shù)設(shè)置
????clc;?
????clear?all;
????close?all;
????x_dem=6;
????z_dem=4;
????tf?=20000;?????????????????????????????????????%?模擬長度?
????d=200;
????
????%真實(shí)數(shù)據(jù)（實(shí)際）
????x=zeros（x_demtf+1）;
????z=zeros（z_demtf+1）;
????
????%真實(shí)初始位置（實(shí)際）
????x（:1）=[000333]‘;
????
????%基站位置
????P1=[d?-d?0]‘;
????P2=[-dd?0]‘;
????P3=[-d?-d0]‘;
????P4=[dd0]‘;
???????
????%ckf狀態(tài)估計(jì)
????x_ckf=zeros（x_demtf+1）;???????????
????x_ckf（:1）=x（:1）;
????
????%EKF的初始估計(jì)
????xhat=x_ckf（:1）;
????
????x_error=zeros（1tf+1）;
????y_error=zeros（1tf+1）;
????z_error=zeros（1tf+1）;

????
????Q?=?0.01;??????????????????????????????%?過程狀態(tài)協(xié)方差
????array_Q?=?Q?*?eye（6）;??????????????????%?過程狀態(tài)協(xié)方差矩陣
????R?=?0.016;?????????????????????????????%?測(cè)量噪聲協(xié)方差?
????array_R?=?R*eye（z_dem）;????????????????%?測(cè)量噪聲協(xié)方差矩陣
????
????%誤差協(xié)方差矩陣初始值?
????Pplus=diag（[0.00990.00990.00990.010.010.01]）;

????Ts=0.1;%時(shí)間間隔
????%狀態(tài)轉(zhuǎn)移矩陣（勻速運(yùn)動(dòng)）
????F=[1?0?0?Ts0?0;
???????0?1?0?0?Ts0;
???????0?0?1?0?0?Ts;
???????0?0?0?1?0?0;
???????0?0?0?0?1?0;
???????0?0?0?0?0?1];
????Gamma=[0.5;0.5;0.5;1;1;1];
???
????%CKF權(quán)值和sigma采樣點(diǎn)矩陣
????w=1/（2*x_dem）;??
????kesi=sqrt（x_dem）*[eye（x_dem）-eye（x_dem）];
??%%?觀測(cè)量模擬系統(tǒng)????
????for?k?=?1?:?tf?
???????
???????x（:k+1）?=?F?*?x（:k）;%?+?sqrt（Q）?*?[randnrandnrandnrandnrandnrandn]‘;??????%狀態(tài)值?
???????
???????temp?=?randn;
???????for?i=1:z_dem-1
???????????temp?=[temprandn];
???????end
???????z（:k+1）?=?uwb_obs（x（:k+1）P1P2P3P4）?+?sqrt（R）?*temp‘;??????%觀測(cè)值
????end;
??%%??CKF濾波
????for?k?=?1?:?tf?
???????%%?狀態(tài)預(yù)測(cè)
????????%求協(xié)方差矩陣平方根
????????S=chol（Pplus‘lower‘）;
????????%計(jì)算求容積點(diǎn)
????????for?i=1:2*x_dem
????????????Xpoint（:i）=S*kesi（:i）+xhat;
????????end
????????%求傳播容積點(diǎn)
????????for?i=1:2*x_dem
????????????Xminus（:i）=F*Xpoint（:i）;???????????????????????????
????????end
????????%狀態(tài)預(yù)測(cè)
????????xminus=w*sum（Xminus2）;
????????%狀態(tài)預(yù)測(cè)協(xié)方差陣
????????Pminus=w*sum_square（Xminus）-xminus*xminus‘+?array_Q;
?????????????
???????%%?觀測(cè)更新
????????%矩陣分解
????????Sminus=chol（Pminus‘lower‘）;
????????%計(jì)算求容積點(diǎn)
????????for?i=1:2*x_dem
????????????Xpoint1（:i）=Sminus*kesi（:i）+xminus;
????????end
????????%求傳播容積點(diǎn)
????????for?i=1:2*x_dem
????????????Z（:i）=uwb_obs（Xpoint1（:i）P1P2P3P4）;
????????end
????????%觀測(cè)預(yù)測(cè)
????????zhat=w*sum（Z2）;
????????%觀測(cè)預(yù)測(cè)協(xié)方差陣
????????Pzminus=w*sum_square（Z）-?zhat*zhat‘+array_R;
????????%互協(xié)方差陣
????????Pxzminus=zeros（x_demz_dem）;
????????for?i=1:2*x_dem
???????????????Pxzminus=?Pxzminus+w*Xpoint1（:i）*Z（:i）‘;
????????end
????????Pxzminus=?Pxzminus-xminus*zhat‘;
????????%計(jì)算卡爾曼增益
????????K=Pxzminus/Pzminus;
????????%狀態(tài)更新
????????xhat=xminus+K*（z（:k）-zhat）;
????????%狀態(tài)協(xié)方差矩陣更新
????????Pplus=Pminus-K*Pzminus*K‘;
????????
????????%只用預(yù)測(cè)
%?????????xhat=xminus;
???????%%?數(shù)據(jù)存儲(chǔ)
????????x_ckf（:k+1）=xhat;
%????????x_ckf（:k+1）=xminus;
????end
????
????t?=?0?:?tf;
????figure;
????plot（tx（1:）‘k.‘tx_ckf（1:）‘g‘）;
????legend（‘真實(shí)值

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件????????3679??2019-10-17?11:43??容積卡爾曼c(diǎn)kf\ckf.m
?????文件?????????186??2019-10-16?16:17??容積卡爾曼c(diǎn)kf\sum_square.m
?????文件?????????353??2019-10-17?10:55??容積卡爾曼c(diǎn)kf\uwb_obs.m
?????文件??????133325??2019-10-17?11:44??容積卡爾曼c(diǎn)kf\x_error.mat
?????文件??????133328??2019-10-17?11:44??容積卡爾曼c(diǎn)kf\y_error.mat
?????文件?????1578583??2019-10-17?11:39??容積卡爾曼c(diǎn)kf\z.mat
?????文件??????134365??2019-10-17?11:44??容積卡爾曼c(diǎn)kf\z_error.mat