clc
clear
close all

addpath './lib';
addpath './lib/gnss';
addpath './lib/calbiration';
addpath './lib/plot';
addpath './lib/rotation';
addpath './lib/geo';
savepath

%% 说明
% UWB IMU 融合算法，采用误差卡尔曼15维经典模型，伪距组合
% PR(TOF) 伪距：        UWB硬件给出的原始测量距离值
% IMU:                 加速度(3) 3轴陀螺(3) 共6维,,
% noimal_state:        名义状态: 导航方程状态: 位置(3) 速度(3) 四元数(4) 共10维
% err_state:           KF误差状态: 位置误差(3) 速度误差(3) 失准角(3) 加速度计零偏(3) 陀螺零偏(3) 共15维
% du:                  零偏反馈: 加速度计零偏(3) 陀螺零偏(3)， 共6维

% 单位说明:
% 加速度,加速度零偏: m/s^(2)
% 角速度, 角速度(陀螺)零偏: rad/s
% 角度 rad
% 速度: m/s

%% 读取数据集
load datas2
dataset = datas;
% dataset的数据结构如下
% dataset
% ├── imu
% |   ├── acc    加速度计三维数据
% |   ├── gyr    陀螺仪三维数据
% |   └── time   时间戳数组
% ├── uwb
% |   ├── time   时间戳数组 1*17200
% |   ├── anchor 四个标签的坐标矩阵，x，y，z
% |   ├── tof    飞行时间，4*17200，表示4个基站在连续时间点上对同一移动标签的测距时间
% |   └── cnt    基站个数
% └── pos

N = length(dataset.imu.time);                               % 时间序列的长度
dt = mean(diff(dataset.imu.time));                          % dt：IMU数据的平均采样周期。通过diff方法计算相邻时间戳的差值，并求平均值得到dt

% 故意删除一些基站及数据，看看算法在基站数量很少的时候能否有什么奇迹。。
% dataset.uwb.anchor(:,1) = [];
% dataset.uwb.tof(1,:) = [];


% EKF融合使用的基站个数，融合算法最少2个基站就可以2D定位
%dataset.uwb.cnt = size(dataset.uwb.anchor, 2);


m_div_cntr = 0;                                              % 量测分频器：计数器，用于记录已累计的IMU更新次数。当计数器达到 m_div 时，触发UWB量测更新，并重置计数器。
m_div = 1;                                                   % 量测分频系数，每m_div次量测，才更新一次EKF量测(UWB更新),  可以节约计算量 或者 做实验看效果
                                                             % UWB的采样率通常远低于IMU（例如IMU 100Hz，UWB 10Hz），直接每次融合会导致冗余计算。通过分频系数可以匹配两者的实际更新频率，避免不必要的计算。
UWB_LS_MODE = 3;                                             % 2 纯UWB解算采用2D模式， 3：纯UWB解算采用3D模式
UWB_EKF_UPDATE_MODE = 3;                                     % EKF 融合采用2D模式，   3: EKF融合采用3D模式

%% 数据初始化
out_data.uwb = [];                                           % 初始化输出数据结构
out_data.uwb.time = dataset.uwb.time;                        % 存储UWB时间戳
out_data.imu.time = dataset.imu.time;                        % 存储IMU时间戳
out_data.uwb.anchor = dataset.uwb.anchor;                    % 存储基站坐标
% pr = 0;
% last_pr = 0;

%% 滤波参数初始化
settings = uwb_imu_example_settings();                       % 获取滤波器设置参数，使用uwb_imu_example_settings()函数获取滤波器的配置参数
                                                             % 返回值是一个结构体settings，包含滤波器的各种参数
R = diag(ones(dataset.uwb.cnt, 1)*settings.sigma_uwb^(2));   % 量测噪声协方差矩阵
noimal_state = init_navigation_state(settings) ;             % 初始化名义状态，10维：位置：（x,y,z）;速度（vx,vy,vz）;姿态四元数（q0,q1,q2,q3）
                                                             % 名义状态：系统在理想条件下的理论状态，忽略噪声、扰动和模型误差
err_state = zeros(15, 1);                                    % 初始化误差状态,15维：位置误差（3维）；速度误差（3维）；姿态误差（3维）；加速度计零偏（3维）；陀螺仪零偏（3维）
                                                             % 误差状态：实际状态与名义状态之间的偏差，由噪声、外部干扰或模型不确定性导致

%使用第一帧伪距作为初始状态
pr = dataset.uwb.tof(:, 1);                                  % 获取第一帧UWB测量数据
% 初始位置解算
% 测量值=真实值+零偏+噪声
% 零偏是传感器在无输入信号时输出的非零偏移量，反应其基准点的系统性偏差
% 伪距是UWB（超宽带）等无线定位系统中，通过信号传播时间计算出的等效距离测量值
% noimal_state前三项:位置坐标，使用最小二乘法解算
noimal_state(1:3) = multilateration(dataset.uwb.anchor, [ 1 1 0.99]',  pr', UWB_LS_MODE);

du = zeros(6, 1);                                            % 初始化零偏反馈
[P, Q1, Q2, ~, ~] = init_filter(settings);                   % 初始化滤波器参数，此处初始化时使用~占位符，因为这个地方这两个参数没有意义，这样占位可以不占用内存

% 输出提示
fprintf("共%d帧数据, IMU采样频率:%d Hz 共运行时间 %d s\n", N,  1 / dt, N * dt);
fprintf("UWB基站个数:%d\n", dataset.uwb.cnt);
fprintf("UWB量测更新频率为:%d Hz\n", (1 / dt) / m_div);
fprintf("UWB EKF量测更新模式: %dD模式\n", UWB_EKF_UPDATE_MODE);
fprintf("纯UWB最小二乘解算: %dD模式\n", UWB_LS_MODE);
fprintf("EKF 滤波参数:\n");
settings
fprintf("开始滤波...\n");


for k=1:N
    
    acc = dataset.imu.acc(:,k);
    gyr = dataset.imu.gyr(:,k);
    
    % 反馈 加速度bias, 陀螺bias
    % 测量值 = 真实值 + 零偏 + 噪声
    acc = acc - du(1:3);                                     % du是零偏，（1：3）和（4：6）分别代表加速度和陀螺仪的零偏
    gyr = gyr - du(4:6);                                     % 陀螺仪
    
    % 捷联惯导解算
    p = noimal_state(1:3);                                   % 位置坐标 [x; y; z]（单位：米）
    v = noimal_state(4:6);                                   % 速度 [vx; vy; vz]（单位：m/s）
    q =  noimal_state(7:10);                                 % 姿态四元数 [q0; q1; q2; q3]
    % 捷联惯导解算：输入当前状态和IMU测量值，输出更新后的位置、速度和姿态
    % pvq是当前的状态，acc、gyr是加速度和陀螺仪的测量值，dt为时间步长，后面为重力向量
    [p, v, q] = nav_equ_local_tan(p, v, q, acc, gyr, dt, [0, 0, -9.8]');  % 东北天坐标系，重力简化为-9.8
    
    %   小车假设：基本做平面运动，N系下Z轴速度基本为0，直接给0
    v(3) = 0;
    
    % 更新数值
    noimal_state(1:3) = p;
    noimal_state(4:6) = v;
    noimal_state(7:10) = q;
    out_data.eul(k,:) = q2eul(q);                            % 四元数转欧拉角并存入数据表 
    
    % 生成F阵   G阵
    % 看函数注释，根据当前的状态、加速度测量值 acc 和采样时间 dt，生成状态转移矩阵 F 和过程噪声耦合矩阵 G
    [F, G] = state_space_model(noimal_state, acc, dt);
    
    % 卡尔曼P阵预测公式
    P = F*P*F' + G*blkdiag(Q1, Q2)*G';
    
    % log数据
    out_data.x(k,:)  = noimal_state;
    out_data.delta_u(k,:) = du';                             % 位置方差  
    out_data.diag_P(k,:) = trace(P);                         % 姿态四元数方差 
    
    
    %% EKF UWB量测更新
    m_div_cntr = m_div_cntr+1;                               %量测分频器
    % 每m_div次量测，才更新一次EKF量测(UWB更新)，此时m_div设置为1
    if m_div_cntr == m_div
        m_div_cntr = 0;
        
        pr = dataset.uwb.tof(1:dataset.uwb.cnt, k);          % pr是当前时刻（第k帧）所有UWB基站的TOF
        
        %判断两次PR 差，如果差太大，则认为这个基站PR比较烂，不要了。相当于GNSS里的挑星
        %                         arr = find(abs(pr - last_pr) < 0.05);
        %                         last_pr = pr;
        %                         out_data.good_anchor_cnt(k,:) = length(arr); %记录挑出来的基站数
        %
        %                         if(isempty(arr))
        %                             continue;
        %                         end
        %
        %                         %构建 剔除不好的基站之后的基站位置矩阵和R矩阵
        %                         pr = pr(arr);
        %                         anch = dataset.uwb.anchor(:, arr);
        %                         R1 = R(arr, arr);
        
        % 算了不挑基站了，所有基站直接参与定位，其实也差不太多
        anch = dataset.uwb.anchor;
        R1 = R;                                               % 测量噪声协方差矩阵
        
        % 量测方程
        % Y是根据当前状态 noimal_state 预测出来的各个基站到目标的距离
        % H是观测矩阵，表示距离对状态的偏导
        [Y, H]  = uwb_hx(noimal_state, anch, UWB_EKF_UPDATE_MODE);
        
        % 卡尔曼公式，计算K
        S = H*P*H'+R1;                                       % system uncertainty 系统不确定度，用于衡量测量是否可信
        residual = pr - Y;                                   % residual 或者叫新息 测量与预测的差值
        
        %% 根据量测置信度给R一些增益   Self-Calibrating Multi-Sensor Fusion with Probabilistic
        %Measurement Validation for Seamless Sensor Switching on a UAV, 计算量测可靠性
        % 衡量量测可信度，L越大代表当前量测越不可信（Mahalanobis 距离L）
        L = (residual'*S^(-1)*residual);
        out_data.L(k,:) = L;
        
        %         if(L > 20 ) %如果量测置信度比较大，则更不相信量测
        %             S = H*P*H'+R1*5;
        %         end
        
        K = (P*H')/(S);                                      % 卡尔曼增益K
        err_state = [zeros(9,1); du] + K*(residual);         % 状态误差向量
        
        % 反馈速度位置
        noimal_state(1:6) = noimal_state(1:6) + err_state(1:6);
        
        % 反馈姿态
        q = noimal_state(7:10);                              % 提取姿态四元数
        q = qmul(rv2q(err_state(7:9)), q);
        noimal_state(7:10) = q;                              % 姿态反馈（使用小角度修正旋转）
        
        %存储加速度计零偏，陀螺零偏
        du = err_state(10:15);
        
        % P阵后验更新
        P = (eye(15)-K*H)*P;
    end
    
    
    %% 车载约束：Z轴速度约束： B系下 Z轴速度必须为0(不能钻地).. 可以有效防止Z轴位置跳动 参考https://kth.instructure.com/files/677996/download?download_frd=1 和 https://academic.csuohio.edu/simond/pubs/IETKalman.pdf
    %% 车辆在 B系Z轴上不应该有速度，如果估计有，就认为估计错了，通过观测去修正
    R2 = eye(1)*0.5;
    Cn2b = q2m(qconj(noimal_state(7:10)));                   % 计算四元数的共轭，得到从导航系到机体系的旋转;再转换成方向余弦矩阵
    
    H = [zeros(1,3), [0 0 1]* Cn2b, zeros(1,9)];             % 构造观测矩阵H，用于观测当前状态中的速度
    
    K = (P*H')/(H*P*H'+R2);
    z = Cn2b*noimal_state(4:6);                              % 当前估计的B系Z轴速度
    
    err_state = [zeros(9,1); du] + K*(0-z(3:3));             % 目标观测值是0（因为车不能在B系Z轴上移动），当前估计值是z(3)，差值为残差
    
    % 反馈速度位置 更新状态
    noimal_state(1:6) = noimal_state(1:6) + err_state(1:6);
    
    % 反馈姿态
    q = noimal_state(7:10);
    q = qmul(ch_rv2q(err_state(7:9)), q);                    % 更新原始四元数
    noimal_state(7:10) = q;
    
    %存储加速度计零偏，陀螺零偏
    % du = err_state(10:15);
    
    % P阵后验更新
    P = (eye(15)-K*H)*P;
    
    
end

fprintf("开始纯UWB最小二乘位置解算...\n");
%% 纯 UWB 位置解算
j = 1;
uwb_pos = [1 1 1]';
N = length(dataset.uwb.time);

for i=1:N
    pr = dataset.uwb.tof(:, i);
    % 去除NaN点
    %if all(~isnan(pr)) == true
    
    uwb_pos = multilateration(dataset.uwb.anchor, uwb_pos,  pr', UWB_LS_MODE);
    out_data.uwb.pos(:,j) = uwb_pos;
    j = j+1;
    %end
end
fprintf("计算完成...\n");

%% plot 数据
out_data.uwb.tof = dataset.uwb.tof;
out_data.uwb.fusion_pos = out_data.x(:,1:3)';
demo_plot(dataset, out_data);



%%  初始化nomial state
function x = init_navigation_state(~)

% 简单点， 全部给 0 。10维：位置：（x,y,z）;速度（vx,vy,vz）;姿态四元数（q0,q1,q2,q3）
q = eul2q(deg2rad([0 0 0]));                         % eul2q函数用于将欧拉角转换为四元数
x = [zeros(6,1); q];                                 % 拼接6维零向量与四元数
end


%% 初始化滤波器参数
function [P, Q1, Q2, R, H] = init_filter(settings)

% Kalman filter state matrix
P = zeros(15);
P(1:3,1:3) = settings.factp(1)^2*eye(3);
P(4:6,4:6) = settings.factp(2)^2*eye(3);
P(7:9,7:9) = diag(settings.factp(3:5)).^2;
P(10:12,10:12) = settings.factp(6)^2*eye(3);
P(13:15,13:15) = settings.factp(7)^2*eye(3);

% Process noise covariance
Q1 = zeros(6);
Q1(1:3,1:3) = diag(settings.sigma_acc).^2*eye(3);
Q1(4:6,4:6) = diag(settings.sigma_gyro).^2*eye(3);

Q2 = zeros(6);
Q2(1:3,1:3) = settings.sigma_acc_bias^2*eye(3);
Q2(4:6,4:6) = settings.sigma_gyro_bias^2*eye(3);

R =0;
H = 0;

end

%%  生成F阵，G阵
% 根据当前的状态 x、加速度测量值 acc 和采样时间 dt，生成状态转移矩阵 F 和过程噪声耦合矩阵 G
function [F,G] = state_space_model(x, acc, dt)
Cb2n = q2m(x(7:10));                                     % 把四元数转换成方向余弦矩阵，从机体坐标系（body）转到导航系（nav） 

sk = askew(Cb2n * acc);                                  % 对一个向量v构造出反对称矩阵，用于表示向量叉乘的线性化形式

O = zeros(3);
I = eye(3);
% P V theta 加计零偏  陀螺零偏
F = [
    O I   O O       O;
    O O -sk -Cb2n O;
    O O O O       -Cb2n;
    O O O O       O;
    O O O O       O];

% Approximation of the discret  time state transition matrix
% 将连续时间的状态转移矩阵做一次一阶近似离散化
F = eye(15) + dt*F;

% Noise gain matrix
G=dt*[
    O       O         O  O;
    Cb2n  O         O  O;
    O        -Cb2n O  O;
    O        O         I   O;
    O        O        O   I];
end

%% UWB量测过程
% Y 根据当前状态和UWB基站坐标预测出来的伪距
% H 量测矩阵
% anchor: 基站坐标  M x N: M:3(三维坐标)，  N:基站个数
% dim:  2: 二维  3：三维
function [Y, H] = uwb_hx(x, anchor, dim)
N = size(anchor,2);                                      % 基站个数

% 三维取xyz，二维取xy
position = x(1:3);
if(dim)== 2
    position = position(1:2);
    anchor = anchor(1:2, 1:N);
    %  uwb.anchor
end

Y = [];                                                  % Y是伪距预测，即目标位置到每个锚点的欧几里得距离
H = [];                                                  % 雅可比矩阵的每一行
% 计算预测的伪距s，所有预测的位置与各个锚点的向量差
perd_pr = repmat(position,1,N) - anchor(:,1:N);
for i=1:N
    
    if(dim)== 2
        H = [H ;perd_pr(:,i)'/norm(perd_pr(:,i)),zeros(1,13)];
    else
        H = [H ;perd_pr(:,i)'/norm(perd_pr(:,i)),zeros(1,12)];
    end
    Y = [Y ;norm(perd_pr(:,i))];
end



end
