容积卡尔曼滤波（CKF）和嵌入式容积卡尔曼滤波（ECKF）

一个BOT的四维模型，内有容积卡尔曼滤波算法(CKF)和本人所提的嵌入式容积卡尔曼滤波(ECKF)算法的比较

%% ----------------------------------------------------------------
% Bearing-only tracking model for tesing the cubature Kalman filter （CKF） 
% and the embedded cubature Kalman filter （ECKF）
% 
% Version: 20140307
% 
% 
% @All rights reserved by Xin-Chun Zhang from UESTC
% You can use the code for learning teaching and education do not use it
% for commerical purpose!
% 
% 
% 
% 参考：
% [1] I.Arasaratnam S.Haykin. Cubature Kalman Filters[J]. IEEE Trans. Automat.
%     Control 2009 54（6）
% [2] 张鑫春等. 一种用于目标跟踪的嵌入式容积卡尔曼滤波器设计[C]. 综合电子技术教
%     育部重点实验室2012学术年会论文集，2012
% [3] 张鑫春 等. “均方根嵌入式容积卡尔曼滤波“ 控制理论与应用 2013 30（9）.         
% [4] Xin-Chun Zhang Yun-Long Teng. A new derivation of the cubature
%     Kalman filters [J]. Asian Journal of Control 2013 Accept
% [5] Xin-Chun Zhang A novel cubature Kalman filter for nonlinear state
%     estimation [C]. 52nd IEEE Conference on Decision and Control
%     Florence Italy 2013
% [6] Zhang Xin-Chun Guo Cheng-Jun Cubature Kalman filters: Derivation
%     and extension [J]. Chinese Physics B 2013 22（12）
% [7] Xin-Chun Zhang. Cubature information filter using embedded and
%     high-degree cubature rules [J]. Circuits Systems and Signal
%     processing Accept（2013）/Online（2014）
%
% 
% 
% 
%
%                      By Xin-Chun Zhang 
%                 E-mail：irving_zhang@163.com
%       University of Electronic Science and Technology of China
%% ----------------------------------------------------------------

clear all;
close all;
clc;

h = waitbar（0 ‘1‘ ‘Name‘ ‘Please wait ...‘ ‘Windowstyle‘ ‘modal‘ ...
    ‘CreateCancelBtn‘ ‘setappdata（gcbf‘‘canceling‘‘1）‘）;
setappdata（h ‘canceling‘ 0）;

% Global variables
global Q R fai gama kesi w n m w1 w2 kesi_im3 b ;

Q = 0.0005^2; % process noise
R = 0.001^2; % measurement noise
fai = [1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1];
gama = [0.5 0; 1 0; 0 0.5; 0 1];
n = 4; %dimension of the system
m = 2 * n; % Number of cubature points

% 3rd-degree CKF/SCKF
w = 1 / m; % Weight
kesi1 = eye（n）;
kesi2 = -eye（n）;
kesi = [kesi1 kesi2] * （sqrt（m / 2））; % Construction of the cubature points

% 3rd-degree ECKF
delta = 1;
w1 = 1 / （2^（n + 1） * delta）;
w2 = 1 - 1 / （2 * delta）;
yitao = zeros（n 1）;
yitak1 = [1 1 1 1; 1 1 1 -1; 1 1 -1 -1; 1 -1 -1 -1; 1 -1 1 1; 1 -1 -1 1; ...
    1 -1 1 -1; 1 1 -1 1]‘ * sqrt（2 * delta）;
kesi_im3 = [yitak1 -yitak1 yitao];
[a b] = size（kesi_im3）;

num = 30; % steps of the simulation

xarray_sum = zeros（n num + 1）;

xhatarray_sum = zeros（n num + 1）;
error_avg = zeros（n num + 1）;

xhatarray_S_sum = zeros（n num + 1）;
error_S_avg = zeros（n num + 1）;

xhatarray_5_sum = zeros（n num + 1）;
error_5_avg = zeros（n num + 1）;

xhatarray_I_sum = zeros（n num + 1）;
error_I_avg = zeros（n num + 1）;

len = 100;
for iter = 1 : len
    if getappdata（h ‘canceling‘）
        iter = iter - 1;
        break;
    end
    waitbar（iter / len h sprintf（‘%

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4846  2014-03-07 22:36  BOT\Main.m

     文件       1556  2014-03-07 22:23  BOT\Third_degree_CKF.m

     文件       2401  2014-03-07 22:26  BOT\Third_degree_ECKF.m

     目录          0  2014-03-07 22:37  BOT

----------- ---------  ---------- -----  ----

                 8803                    4