CKF和五阶CKF的目标跟踪代码

收到一些国内外朋友的来信，咨询关于容积卡尔曼滤波的问题(CKF)，大家比较疑惑的应该就是generator或G-orbit的概念。考虑到工作以后，重心必然转移，不可能再像现在这样详细的回答所有人的问题，更不可能再帮大家改论文、写(或改)代码了，请各位谅解！在此，上传一个CKF和五阶CKF用于目标跟踪的示例代码，代码中包含详细的注释，希望对大家以后的学习和研究有所帮助！ 此代码利用C++对五阶CKF的第二G-轨迹进行了封装(Perms.exe)，能理解最好，如果无法理解，也无须深究其具体构造方法！可执行文件底层是用字符串+递归算法实现的，理论上可以应用于任意维模型。但考虑到递归算法可能存在的栈溢出，重复压栈出栈带来的时间消耗等问题，我们利用矩阵的稀疏性和群的完全对称性，并通过分次调用，来尽可能减少栈的深度，提高计算速度。 容积点一次生成后，可以一直使用，通过对50维G-轨迹的生成速度(Core T6600@2.2GHz)进行测试，包含数据读写在内的速度约为1.5秒，速度尚可。而目前为止，本人尚未遇到达到甚至超过50维的系统，因此，暂时不作算法层面的优化。 注意：Perms.exe可以用于任意维模型，将可执行文件复制至工作目录下，调用时选择N/n，并输入你的模型维数，即可生成所需的第二G-轨迹。如果无法理解相关的概念，请参考示例代码，并记住如何使用即可~~~ 相关理论基础及所用模型，请参考以下文献： References (you may cite one of the articles in your paper): [1] X. C. Zhang, C. J. Guo, "Cubature Kalman filters: Derivation and extension," Chinses Physics B, vol.22, no.12, 128401, DOI: 10.1088/1674-1056/22/12/128401 [2] X. C. Zhang, Y. L. Teng, "A new derivation of the cubature Kalman filters," Asian Journal of Control, DOI: 10.1002/asjc.926 [3] X. C. Zhang, "Cubature information filters using high-degree and embedded cubature rules," Circuits, Systems, and Signal Processing,vol.33, no.6,pp.1799-1818,DOI: 10.1007/s00034-013-9730-0

function [x P] = Fifth_degree_CKF（xhat Pplus z）
global Q R T m kesi_5 b_5 w2_5 w_5 w4_5;
%% --------------------------5th-degree CKF-----------------------

%% ---------------------------Time Update-------------------------
% Evaluate the Cholesky factor
Shat = chol（Pplus ‘lower‘）;
for cpoint = 1 : b_5
    % Evaluate thecubature points
    rjpoint_5（: cpoint） = Shat * kesi_5（: cpoint） + xhat;
    % Evaluate the propagated cubature points
    fai3 = [1 sin（rjpoint_5（5 cpoint） * T） / rjpoint_5（5 cpoint） 0 -（1 - ...
        cos（rjpoint_5（5 cpoint） * T）） / rjpoint_5（5 cpoint） 0;
        0 cos（rjpoint_5（5 cpoint） * T） 0 -sin（rjpoint_5（5 cpoint） * T） 0;
        0 （1 - cos（rjpoint_5（5 cpoint） * T）） / rjpoint_5（5 cpoint） 1 ...
        sin（rjpoint_5（5 cpoint） * T） / rjpoint_5（5 cpoint） 0;
        0 sin（rjpoint_5（5 cpoint） * T） 0 cos（rjpoint_5（5 cpoint） * T） 0;
        0 0 0 0 1];
    Xminus_5（: cpoint） = fai3 * rjpoint_5（: cpoint）;
end

% Estimate the predicted state
xminus_5 = w2_5 * Xminus_5（: 1） + w_5 * sum（Xminus_5（: 2 : m + 1） 2） + ...
    w4_5 * sum（Xminus_5（: m + 2 : b_5） 2）;

% Estimate the predicted error covariance
Pminus_5 = Q + w2_5 * （Xminus_5（: 1） - xminus_5） * （Xminus_5（: 1） - xminus_5）‘...
    + w_5 * （Xminus_5（: 2 : m + 1） - repmat（xminus_5 1 m）） * ...
    （Xminus_5（: 2 : m + 1） - repmat（xminus_5 1 m））‘ + ...
    w4_5 * （Xminus_5（: m + 2 : b_5） - repmat（xminus_5 1 b_5 - m - 1）） ...
    * （Xminus_5（: m + 2 : b_5） - repmat（xminus_5 1 b_5 - m - 1））‘;
%% ---------------------------------------------------------------

%% -------------------------Measurement Update--------------------
% Evaluate the Cholesky factor
Sminus_5 = chol（Pminus_5 ‘lower‘）;
for cpoint = 1 : b_5
    % Evaluate the cubature points
    rjpoint1_5（: cpoint） = Sminus_5 * kesi_5（: cpoint） + xminus_5;
    %Evaluate the propagated cubature points
    Z_5（: cpoint） = [sqrt（rjpoint1_5（1 cpoint） ^ 2 + rjpoint1_5（3 cpoint） ^ 2） ...
        atan（rjpoint1_5（3 cpoint） / rjpoint1_5（1 cpoint））]‘;
end
% Estimate the predicted measurement
zhat_5 = w2_5 * Z_5（: 1） + w_5 * sum（Z_5（: 2 : m + 1） 2） + w4_5 * ...
    sum（Z_5（: m + 2 : b_5） 2）;

% Estimate the innovation covariance matrix
Pzminus_5 = R + w2_5 * （Z_5（: 1） - zhat_5） * （Z_5（: 1） - zhat_5）‘ + ...
    w_5 * （Z_5（: 2 : m + 1） - repmat（zhat_5 1 m）） * （Z_5（: 2 : m + 1） ...
    - repmat（zhat_5 1 m））‘+ w4_5 * （Z_5（: m + 2 : b_5） - ...
    repmat（zhat_5 1 b_5 - m - 1）） * （Z_5（: m + 2 : b_5） - repmat（zhat_5 1 b_5 - m - 1））‘;

% Estimate the cross-covariance matrix
Pxzminus_5 = w2_5 * （rjpoint1_5（: 1） - xminus_5） * （Z_5（: 1） - zhat_5）‘ + ...
    w_5 * （rjpoint1_5（: 2 : m + 1） - repmat（xminus_5 1 m）） * ...
    （Z_5（: 2 : m + 1） - repmat（zhat_5 1 m））‘...
    + w4_5 * （rjpoint1_5（: m + 2 : b_5） - repmat（xminus_5 1 b_5 - m - 1）） ...
    * （Z_5（: m + 2 : b_5） - repmat（zhat_5 1 b_5 - m - 1））‘;

% Estimate th

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

     文件       3274  2014-06-21 14:47  Target tracking\Fifth_degree_CKF.m

     文件    1092608  2014-06-21 15:08  Target tracking\Perms.exe

     文件        925  2013-01-23 11:16  Target tracking\rms.m

     文件       2188  2014-06-21 14:46  Target tracking\Three_degree_CKF.m

     文件       7034  2014-06-21 15:00  Target tracking\TR_Main.m

     目录          0  2014-06-21 15:09  Target tracking

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

              1106029                    6