容积卡尔曼ckf——基站测距定位解算

容积卡尔曼，六维状态量（3位置，3速度），matlab代码，模拟基站测距定位

%% 参数设置
    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