容积卡尔曼滤波（ckf）MATLAB代码(一个ckf滤波器的子程序)

一个ckf滤波器的子程序，自己可以进行拓展，可以根据自己的实例进行套用，适用与多维实例的系统，代码注释全面，参数设置参考的文献均已注明，值得学习

%% ---------------------------------------------------------------
% Version: 20121120, one of the functions for Nonlinear Filtering.All
% Rights Reserved. Anyone can use it for your own simulations. Do not modify
% the following comments.
% 
% Implementation of the third-degree cubature Kalman filters. This script
% could be applied to multi-dimensional models.
% 
% Caution: For validating the third-degree cubture Kalman fiters only,
% the model employed is known as the bearing-only tracking (four-dimensional
% model), the values of parameters can be referred to the following
% references:
% [1] I. Arasaratnam, S. Haykin, "Cubature Kalman filters," IEEE Trans.
% Automat. Control, vol. 54, no. 6, 2009, pp. 1254-1269.
% [2] X. C. Zhang, C. J. Guo, "Square-root imbedded cubature Kalman
% filtering," Control Theory & Applications, to appear. (In Chinese).
% [3] X. C. Zhang, C. J. Guo, "Cubature Kalman filters: Derivation and
% extension," Chinese Physics B, accepted.
%
% 
%                                              - By Irvingzhang(UESTC)
%                                                    2012-11-20
%                                      Email Address: irving_zhang@163.com
%% ---------------------------------------------------------------
function [x, P] = CKF(xhat, Pplus, z)
global Q R fai gama kesi w m;
%% -----------------------------Time Update-----------------------
    % Evaluate the Cholesky factor
    Shat = sqrtm(Pplus);
    for cpoint = 1 : m
        % Evaluate the cubature points
        rjpoint(:, cpoint) = Shat * kesi(:, cpoint) + xhat;
        % Evaluate the propagated cubature points
        Xminus(:, cpoint) = fai * rjpoint(:, cpoint); 
    end


    % Estimate the predicted state
    xminus = w * sum(Xminus, 2);
    
    % Estimate the predicted error covariance
    Pminus = w * (Xminus * Xminus') - xminus * xminus' + gama * Q * gama';
%% ---------------------------------------------------------------


%% -------------------------Measurement Update--------------------
    % Evaluate the Cholesky factor
    Sminus = sqrtm(Pminus);
    for cpoint = 1 : m
        % Evaluate the cubature points
        rjpoint1(:, cpoint) = Sminus * kesi(:, cpoint) + xminus;
        % Evaluate the propagated cubature points
        Z(:, cpoint)=atan(rjpoint1(3, cpoint) / rjpoint1(1, cpoint));
    end    
    % Estimate the predicted measurement
    zhat = w * sum(Z, 2);
    
    % Estimate the innovation covariance matrix
    Pzminus = w * Z * Z'-zhat * zhat' + R;
    
    % Estimate the cross-covariance matrix
    Pxzminus = w * rjpoint1 * Z'- xminus * zhat';
    
    % Estimate the Kalman gain
    W = Pxzminus * inv(Pzminus);
    
    % Estimate the updated state
    x = xminus + W * (z - zhat);
    
    % Estimate the correspondong error covariance
    P = Pminus - W * Pzminus * W';