零速更新(ZUPT)辅助INS定位

在惯性导航系统中，通常会用gps+imu完成位置、速度、姿态估计。为了解决在一些无gps信号的地方，估算载体的位置、速度，通常会引入视觉、雷达或者地图等约束。而本文尝试想不通过增加外部传感器，完成位置，速度估计。这也就引入了今天主角，零速更新(ZUPT)。零速更新在我看来可以统称归纳为：基于载体运动信息引入速度约束，完成导航信息估计，如车载系统中，车体的侧向和竖向速度为0。

一、零速更新(Zero Velocity Update, ZUPT)

零速更新(Zero Velocity Update, ZUPT)即，当载体处于静止状态时，载体此时的速度为零，利用载体中的惯性系统的解算速度作为系统速度误差的观测量，对其他误差量进行修正，改善静止状态下的组合导航结果，不需要增加外部传感器。

二、零速更新(ZUPT)辅助INS定位步骤

1.零速检测

零速更新是利用载体静止时，速度为0，对系统误差误差进行修正。因此，需要通过一定手段检测到载体静止。常用的检测方法：
（1）加速度幅值检测：载体处于静止状态时，惯性测量器件中加速度计的三轴输出矢量和应稳定在当地重力加速度值附近，通过设定比力阈值检测零速状态。
（2）加速度滑动方差检测：载体处于静止状态时，惯性测量器件中加速度计三个轴的输出值的方差应该近似于0，，设置一定大小的滑动窗口，通过比力方差阈值对加速度计三个轴分别检测零速状态。
（3）角速度幅值检测: 同加速度幅值检测，载体处于静止状态时，惯性测量器件中陀螺仪的三轴输出矢量和应近似为零，通过设定角速率阈值检测零速状态。
（4）角速度滑动方差检测: 同加速度滑动方差检测，载体处于静止状态时，惯性测量器件中陀螺仪三个轴的输出值的方差应该近似于0，设置一定大小的滑动窗口，通过角速率方差阈值对陀螺仪三个轴分别检测零速状态。

2.零速更新(ZUPT)成立，进行误差修正

可以理解为，零速传感器有值时进行速度修正。这句话好像毫无营养，hhh。。

if zupt(k)==true
        
        % Calculate the Kalman filter gain
        K=(P*H')/(H*P*H'+R);
        
        % Calculate the prediction error. Since the detector hypothesis 
        % is that the platform has zero velocity, the prediction error is 
        % equal to zero minus the estimated velocity.    
        z=-x_h(4:6,k);   
        
        % Estimation of the perturbations in the estimated navigation
        % states
        dx=K*z;
        
        
        % Correct the navigation state using the estimated perturbations. 
        % (Subfunction located further down in the file.)
        [x_h(:,k), quat]=comp_internal_states(x_h(:,k),dx,quat);     % Subfunction located further down in the file.
    
        
        % Update the filter state covariance matrix P.
        P=(Id-K*H)*P;
        
        % Make sure the filter state covariance matrix is symmetric. 
        P=(P+P')/2;
    
        % Store the diagonal of the state covariance matrix P.
        cov_h(:,k)=diag(P);
end

有观测量时，进行kalman更新校正。是吧，很简单。

三、算法验证

代码参考来自：

@authors Isaac Skog, John-Olof Nilsson
@copyright Copyright (c) 2011 OpenShoe, ISC License (open source)

github URL: https://github.com/hcarlsso/ZUPT-aided-INS.git

算法由matlab编写，主要有三个部分：
1.算法文件设置部分 settings.m，将参数统一放入结构体simdada中。
包含：（1）通用参数，初始经纬度，并通过经纬度计算得到的g；采样周期；初始航向；初始位置。
（2）零速检测参数，检测方法，包括GLRT，MV，MAG，ARE；acc/gyro标准差，零速更新检测窗口，零速更新检测阈值。
（3）kalman滤波参数，状态估计默认pos/vel/attitude，bias/scalefactors为可选项；过程噪声Q acc/gyro noise、acc_bias/gyro_bias noise；零速更新测量噪声R；初始协方差P。
2.零速更新检测zero_velocity_detector.m
根据配置文件选择的方法GLRT，MV，MAG，ARE，进入相应的检测算法。
3.运行kalman滤波算法ZUPTaidedINS.m
（1）预测
根据状态方程，对状态参数进行预测更新；
（2）校正
当满足零速检测条件时，进行零速校正误差。
主要代码如下：

%% MAINFUNCTION

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  funtion [x_h cov_h]=ZUPTaidedINS(u,zupt)
%
%> @brief Function that runs the zero-velocity aided INS Kalman filter 
%> algorithm. 
%>
%> @details Function that runs the zero-velocity aided INS Kalman filter 
%> algorithm. All settings for the filter is done in setting.m. 
%>
%> @param[out]  x_h     Matrix with the estimated navigation states. Each row holds the [position, velocity, attitude, (biases, if turned on),(scale factors, if turned on)] for time instant k, k=1,...N.        
%> @param[out]  cov     Matrix with the diagonal elements of the state covariance matrices.
%> @param[in]   u       The IMU data vector.     
%> @param[in]   zupt    Vector with the decisions of the zero-velocity.
%>
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x_h, cov_h]=ZUPTaidedINS(u,zupt, posupt, simdata)



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%      Initialize the data fusion          %%       
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Get  the length of the IMU data vector
N=length(u);

% Initialize the filter state covariance matrix P, the processes noise
% covariance matrix Q, the pseudo measurement noise covariance R, and the
% observation matrix H.
[P, Q, R, H]=init_filter(simdata);          % Subfunction located further down in the file.

% Position updates
H_pos = zeros(size(H));
H_pos(1:3,1:3) = eye(3);
R_pos = simdata.R_pos;
assert(all(size(R_pos) == [3,3]))

% Allocate vecors
[x_h, cov_h, Id]=init_vec(N,P,simdata);     % Subfunction located further down in the file.

% Initialize the navigation state vector x_h, and the quaternion vector
% quat.
[x_h(1:9,1), quat]=init_Nav_eq(u,simdata);  % Subfunction located further down in the file.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%        Run the filter algorithm          %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %           Time  Update         %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Compensate the IMU measurements with the current estimates of the 
    % sensor errors  
    u_h=comp_imu_errors(u(:,k),x_h(:,k-1),simdata); % Subfunction located further down in the file.

    
    % Update the navigation equations.   
    [x_h(:,k), quat]=Navigation_equations(x_h(:,k-1),u_h,quat,simdata); % Subfunction located further down in the file.

    
    % Update state transition matrix
    [F, G]=state_matrix(quat,u_h,simdata); % Subfunction located further down in the file.

    
    % Update the filter state covariance matrix P.
    P=F*P*F'+G*Q*G';
    
    % Make sure the filter state covariance matrix is symmetric. 
    P=(P+P')/2;
    
    % Store the diagonal of the state covariance matrix P.
    cov_h(:,k)=diag(P);
   
     
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %      Zero-velocity update      %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Check if a zero velocity update should be done. If so, do the
    % following
    if zupt(k)==true
        
        % Calculate the Kalman filter gain
        K=(P*H')/(H*P*H'+R);
        
        % Calculate the prediction error. Since the detector hypothesis 
        % is that the platform has zero velocity, the prediction error is 
        % equal to zero minus the estimated velocity.    
        z=-x_h(4:6,k);   
        
        % Estimation of the perturbations in the estimated navigation
        % states
        dx=K*z;
        
        
        % Correct the navigation state using the estimated perturbations. 
        % (Subfunction located further down in the file.)
        [x_h(:,k), quat]=comp_internal_states(x_h(:,k),dx,quat);     % Subfunction located further down in the file.
    
        
        % Update the filter state covariance matrix P.
        P=(Id-K*H)*P;
        
        % Make sure the filter state covariance matrix is symmetric. 
        P=(P+P')/2;
    
        % Store the diagonal of the state covariance matrix P.
        cov_h(:,k)=diag(P);
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %      Position update      %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Check if a zero velocity update should be done. If so, do the
    % following
    if all(~isnan(posupt(:,k)))
        
        % Calculate the Kalman filter gain
        K=(P*H_pos')/(H_pos*P*H_pos' + R_pos);
        
        % Calculate the prediction error. 
        z = posupt(:,k) - x_h(1:3,k);   
        
        % Estimation of the perturbations in the estimated navigation
        % states
        dx=K*z;
        
        
        % Correct the navigation state using the estimated perturbations. 
        % (Subfunction located further down in the file.)
        [x_h(:,k), quat]=comp_internal_states(x_h(:,k),dx,quat);     % Subfunction located further down in the file.
    
        
        % Update the filter state covariance matrix P.
        P=(Id-K*H_pos)*P;
        
        % Make sure the filter state covariance matrix is symmetric. 
        P=(P+P')/2;
    
        % Store the diagonal of the state covariance matrix P.
        cov_h(:,k)=diag(P);
    end
        
end

end

4.显示模块view_data.m
将算法计算相关结果绘制如下：
（1）acc/gyro 数据如下图所示：

（2）运动轨迹如下图所示：

（3）零速检测时间段如下图所示：

（4）姿态如下图所示：

（5）位置、速度、航向协方差图下图所示：

（6）acc/gyro bias 估计如下图所示：

参考

1.https://github.com/hcarlsso/ZUPT-aided-INS
2.https://zhuanlan.zhihu.com/p/115529319