CKF.m容积卡尔曼滤波在室内定位技术中的应用

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  状态方程：x（:k+1） = F * x（:k） +[sqrt（Q） * randn;0]; 
    %  观测方程：z（k+1） = atan（0.1 * x（1k+1）） + sqrt（R） * randn; 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    clc; 
    clear all;
    close all;
    n=501;
    tf = 500;                                     % 模拟长度 
    x=zeros（2n）;
    x（:1） =[1;0.1];                              % 初始状态 
    x_ckf=zeros（2n）;
     %x_estimate（:1） = [1;0.1];                  %状态的估计
    x_ckf（:1）=[1;0.1];
    % e_x_estimate = x_estimate（:1）;             %EKF的初始估计
    xhat=x_ckf（:1）;
    x_e_error=zeros（1n）;
    x_c_error=zeros（1n）;
    z_e_error=zeros（1n）;
    z_c_error=zeros（1n）;
    Q = 0.0001;                                    % 过程状态协方差 
    R = 0.16;                                      % 测量噪声协方差 
    P =[0.00990;00.0001];                        %初始估计方差
    Pplus=P;
    F=[11;
        01];
    Gamma=[0.5;1];
    w=0.25;  
    kesi=sqrt（2）*[10-10;
        010-1];
    for k = 1 : tf 
        % 模拟系统 
       x（:k+1） = F * x（:k）;      %状态值 
       %x（:k+1） = F * x（:k） +[sqrt（Q） * randn;0]; 
       if rand（）>0.4
       z（k+1） =  x（1k+1） +randn（）*2;     %观测值
       else
           z（k+1）=x（1k+1）+randn（）*3+4;
       end
    end
    for k = 1 : tf 
        %Cubature卡尔曼滤波器
        %%%%%（1）求协方差矩阵平方根
        S=chol（Pplus‘lower‘）;
        %%%%%（2）计算求容积点
        rjpoint（:1）=S*kesi（:1）+xhat;
        rjpoint（:2）=S*kesi（:2）+xhat;
        rjpoint（:3）=S*kesi（:3）+xhat;
        rjpoint（:4）=S*kesi（:4）+xhat;
        %%%%%（3）传播求容积点
        Xminus（:1）=F*rjpoint（:1）;                           %容积点经过非线性函数后的值
        Xminus（:2）=F*rjpoint（:2）;
        Xminus（:3）=F*rjpoint（:3）; 
        Xminus（:4）=F*rjpoint（:4）; 
        %%%%（4）状态预测
        xminus=w*Xminus（:1）+w*Xminus（:2）+w*Xminus（:3）+w*Xminus（:4）;
        %%%%（5）状态预测协方差阵
        Pminus=w*（Xminus（:1）*Xminus（:1）‘+Xminus（:2）*Xminus（:2）‘+Xminus（:3）*Xminus（:3）‘+Xminus（:4）*Xminus（:4）‘）-xminus*xminus‘+Gamma * Q* Gamma‘;
       %Pminus=w*（Xminus（:1）*Xminus（:1）‘+Xminus（:2）*Xminus（:2）‘+Xminus（:3）*Xminus（:3）‘+Xminus（:4）*Xminus（:4）‘）-xminus*xminus‘+[Q0;00]; 
       %%%%观测更新
        %%%%%（1）矩阵分解
        Sminus=chol（Pminus‘lower‘）;
        %%%%%（2）计算求容积点
        rjpoint1（:1）=Sminus*kesi（:1）+xminus;
        rjpoint1（:2）=Sminus*kesi（:2）+xminus;
        rjpoint1（:3）=Sminus*kesi（:3）+xminus;
        rjpoint1（:4）=Sminus*kesi（:4）+xminus;
        %%%%%（3）传播求容积点
        Z（1）=rjpoint1（11）;
        Z（2）=rjpoint1（12）;
        Z（3）=rjpoint1（13）;
        Z（4）=rjpoint1（14）;
       % Z（:4）=[atan（0.1*rjpoint1（14））;0];
        %%%%%%%（4）观测预测
        zhat=w*（Z（1）+Z（2）+Z（3）+Z（4））;
        %%%%（5）观测预测协方差阵
        %Pzminus=w*（Z（:1）*Z（:1）‘+Z（:2）*Z（:2）‘+Z（:3）*Z（:3）‘+Z（:4）*Z（:4）‘）-zhat*zhat‘+[R0;0Q];
        Pzminus=w*（Z（1）^2+Z（2）^2+Z（3）^2+Z（4）^2）-z