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

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

????Ts=0.1;%時間間隔
????%狀態轉移矩陣（勻速運動）
????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權值和sigma采樣點矩陣
????w=1/（2*x_dem）;??
????kesi=sqrt（x_dem）*[eye（x_dem）-eye（x_dem）];
??%%?觀測量模擬系統????
????for?k?=?1?:?tf?
???????
???????x（:k+1）?=?F?*?x（:k）;%?+?sqrt（Q）?*?[randnrandnrandnrandnrandnrandn]‘;??????%狀態值?
???????
???????temp?=?randn;
???????for?i=1:z_dem-1
???????????temp?=[temprandn];
???????end
???????z（:k+1）?=?uwb_obs（x（:k+1）P1P2P3P4）?+?sqrt（R）?*temp‘;??????%觀測值
????end;
??%%??CKF濾波
????for?k?=?1?:?tf?
???????%%?狀態預測
????????%求協方差矩陣平方根
????????S=chol（Pplus‘lower‘）;
????????%計算求容積點
????????for?i=1:2*x_dem
????????????Xpoint（:i）=S*kesi（:i）+xhat;
????????end
????????%求傳播容積點
????????for?i=1:2*x_dem
????????????Xminus（:i）=F*Xpoint（:i）;???????????????????????????
????????end
????????%狀態預測
????????xminus=w*sum（Xminus2）;
????????%狀態預測協方差陣
????????Pminus=w*sum_square（Xminus）-xminus*xminus‘+?array_Q;
?????????????
???????%%?觀測更新
????????%矩陣分解
????????Sminus=chol（Pminus‘lower‘）;
????????%計算求容積點
????????for?i=1:2*x_dem
????????????Xpoint1（:i）=Sminus*kesi（:i）+xminus;
????????end
????????%求傳播容積點
????????for?i=1:2*x_dem
????????????Z（:i）=uwb_obs（Xpoint1（:i）P1P2P3P4）;
????????end
????????%觀測預測
????????zhat=w*sum（Z2）;
????????%觀測預測協方差陣
????????Pzminus=w*sum_square（Z）-?zhat*zhat‘+array_R;
????????%互協方差陣
????????Pxzminus=zeros（x_demz_dem）;
????????for?i=1:2*x_dem
???????????????Pxzminus=?Pxzminus+w*Xpoint1（:i）*Z（:i）‘;
????????end
????????Pxzminus=?Pxzminus-xminus*zhat‘;
????????%計算卡爾曼增益
????????K=Pxzminus/Pzminus;
????????%狀態更新
????????xhat=xminus+K*（z（:k）-zhat）;
????????%狀態協方差矩陣更新
????????Pplus=Pminus-K*Pzminus*K‘;
????????
????????%只用預測
%?????????xhat=xminus;
???????%%?數據存儲
????????x_ckf（:k+1）=xhat;
%????????x_ckf（:k+1）=xminus;
????end
????
????t?=?0?:?tf;
????figure;
????plot（tx（1:）‘k.‘tx_ckf（1:）‘g‘）;
????legend（‘真實值

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