理论:(2)“RCTM” 和 “AIME” 故障检测与修复方法 (理论和C++代码)

//======================================================================//

GNSS/INS紧组合导航系统完好性监测（理论和c++代码）专栏，后续会开源全部代码

https://blog.csdn.net/hltt3838/category_12207970.html?spm=1001.2014.3001.5482

//======================================================================//

目录

一、故障检测算法

二、故障修复/隔离算法

2.1 基于检验统计量

2.2 基于新息协方差矩阵

三、代码部分

一、故障检测算法

 备注：

• 理论部分还是比较容易理解的，代码实现也比较简单，见后续 ！
• 该方法的优点是 对阶越故障相应快，渐变故障检测慢 ！
• 对于初学者需要注意的是 卡方检验的相关概念和求法，比较详细的介绍博客参考下面的连接 ！

状态分布函数 详细介绍_他人是一面镜子，保持谦虚的态度的博客-CSDN博客_虚警概率与卡方分布

 

上述的AIME并非 原论文中的AIME方法，更像是改进。源论文方法如下：

 备注：

• 理论部分理解相对复杂，代码实现稍微复杂，理论图如下

• 该方法的优点是 对渐变故障检测相对快 ！

二、故障修复/隔离算法

2.1 基于检验统计量

 

 参考论文：

INS/GNSS 松组合导航故障检测与修复算法（苗岳旺 1 , 孙付平 1 , 李海峰 1 , 李飞 2）

2.2 基于新息协方差矩阵

 参考论文：

GNSS/INS 紧组合导航接收机自主完好性监测技术研究 
 

三、代码部分

double TC_Filter::RCTM_test_statistic(Eigen::VectorXd &rk, Eigen::MatrixXd &Ak)
{
   double Ts = 0;
   Ts = rk.transpose() * Ak.inverse() * rk;
  // std::cout<<"--------------------------- Ts_RCTM ="<<Ts<<std::endl;
   
   return Ts;
}


double TC_Filter::AIME_test_statistic(Eigen::Matrix<double, NSTATE, 1> &mdx_, Eigen::Matrix<double, NSTATE, NSTATE> &m_Phi_, Eigen::MatrixXd &Hk_, Eigen::Matrix<double, NSTATE, NSTATE> &Pk_, Eigen::VectorXd &Zk_,Eigen::MatrixXd &Rk_)
{
   double Ts = 0;
   
   if( mdx_vec.size() < M_Td )
   {
      mdx_vec.push_back(mdx_);
      m_Phi_vec.push_back(m_Phi_);
      Hk_vec.push_back(Hk_);
      Pk_vec.push_back(Pk_);  
      Zk_vec.push_back(Zk_);
      Rk_vec.push_back(Rk_);
   }
   
   if( mdx_vec.size() >= M_Td )
   {
      Eigen::VectorXd Xks_, Zks_; 
      Eigen::VectorXd r_i; 
      Eigen::MatrixXd A_i,Aavg_inv;
      
      Eigen::VectorXd beta_i, r_avg; 
      
      Xks_ =  mdx_vec[0];
      
      for(int i = 0; i< mdx_vec.size(); i++)
      {
         A_i.noalias() = Hk_vec[i] * Pk_vec[i] * Hk_vec[i].transpose() + Rk_vec[i]; 
         
         Aavg_inv.setZero(A_i.rows(), A_i.cols());
         Aavg_inv.noalias() += A_i.inverse();
      }
    
     
      for(int i = 0; i < mdx_vec.size(); i++)
      {
         for(int j = 0; j <= i; j++)
            Xks_.noalias() = m_Phi_vec[j] * Xks_;
        
         Zks_.noalias() = Hk_vec[i] * Xks_;
         r_i = Zk_vec[i] - Zks_;  
         A_i.noalias() = Hk_vec[i] * Pk_vec[i] * Hk_vec[i].transpose() + Rk_vec[i]; 
         
         beta_i.setZero(A_i.rows());
         beta_i += A_i.inverse() * r_i;
      }
     
      r_avg.noalias() = Aavg_inv.inverse() * beta_i;  
      Ts = r_avg.transpose() * Aavg_inv * r_avg;
       
      // delete the first  
      mdx_vec.erase(mdx_vec.begin());
      m_Phi_vec.erase(m_Phi_vec.begin());
      Hk_vec.erase(Hk_vec.begin());
      Pk_vec.erase(Pk_vec.begin());
      Zk_vec.erase(Zk_vec.begin());
      Rk_vec.erase(Rk_vec.begin());
   }
   
   
   std::cout<<"--------------------------- mdx_vec.size() ="<<mdx_vec.size()<<std::endl; 
   
   return Ts;
}





// 原版 方法
void FUSION_KF::AIME_test_statistic( Eigen::VectorXd &Zk_p,Eigen::MatrixXd &Vk_p, double &Ts_AIME)
{
   if( Zk_vec.size() < M_Td )
   {
      Zk_vec.push_back(Zk_p);
      Vk_vec.push_back(Vk_p);
   }
   
    
   if( Zk_vec.size() >= M_Td )
   {
       Eigen::MatrixXd Vavg_inv, Vavg_inv_inv;
       Eigen::VectorXd beta, r_avg;  
       
       // 1、为了解决不同时刻矩阵维度不一致问题
       int max_rows,max_cols;
       max_rows = 0; max_cols = 0;
       for(int i = 0; i< Zk_vec.size(); i++)
       {
          if( Vk_vec[i].rows() > max_rows)
              max_rows = Vk_vec[i].rows();
              
          if( Vk_vec[i].cols() > max_cols)
              max_cols = Vk_vec[i].cols(); 
       }
       std::cout <<"--max_rows - max_cols 1= \n"<<max_rows<<","<<max_cols<<std::endl;
       
       
       // 2、求Vavg
       Vavg_inv.setZero(max_rows,max_cols);
       
       for(int i = 0; i< Zk_vec.size(); i++)
       {
         Eigen::MatrixXd Vi_inv,Vi_inv_mid;
         Vi_inv_mid.setZero(max_rows,max_cols);
         
         Vi_inv.setZero(Vk_vec[i].rows(), Vk_vec[i].cols());
         Vi_inv = Vk_vec[i].inverse();
         
         Vi_inv_mid.topLeftCorner(Vk_vec[i].rows(), Vk_vec[i].cols())  = Vi_inv;
         
         Vavg_inv += Vi_inv_mid;
       }
        
        
      // 3、
      beta.setZero(max_rows);
      for(int i = 0; i < Zk_vec.size(); i++) 
      {
         Eigen::VectorXd vkinv_rk,vkinv_rk_mid; 
         vkinv_rk_mid.setZero(max_rows);
         
         vkinv_rk.noalias() = Vk_vec[i].inverse() * Zk_vec[i];
         vkinv_rk_mid.head(vkinv_rk.rows()) = vkinv_rk;
         beta += vkinv_rk_mid;
      }
      
      // 4、
      r_avg.noalias() = Vavg_inv.inverse() * beta;
      Ts_AIME = r_avg.transpose() * Vavg_inv * r_avg ;
    
      Ts_AIME = sqrt(Ts_AIME);

      if(abs(Ts_AIME) > 500.0)
         Ts_AIME = 2.0;  
      
      std::cout <<"-----------------------Ts_AIME = \n"<<Ts_AIME<<std::endl;
      
    
      // 4、delete the first  
      Zk_vec.erase(Zk_vec.begin());
      Vk_vec.erase(Vk_vec.begin());
   }
}

备注：

• M_Td 是设置的外推周期 ！这个是可以根据不同的应用场景而改动 ！
• 函数放在 组合导航系统EKF更新函数中，后续会开源全部代码 ！
• 反是代码有问题，烦请指出，大家共同学习和进步，谢谢 ！

抗差 代码（上述理论对应）

       // =============== added by Jiang
       double Td = 10.83;
       Eigen::MatrixXd Ak,Ak_inv,MAX_mid;
       MAX_mid.setIdentity( Rmat.rows(),Rmat.cols() );
       Ak.noalias() = Hmat * covar_Th + Rmat;
       Ak_inv = Ak.inverse();
       
       for(int i = 0; i < Rmat.rows(); i++)
       {
          double Tsi = 0.0;
          Tsi = Zmat[i] * Ak_inv(i,i) * Zmat[i];
          
          if( Tsi > Td)
          {
            MAX_mid(i,i) = Td / Tsi;  
            std::cout<<"---------------Tsi= \n"<<Tsi<<std::endl;  
          }
          else
            MAX_mid(i,i) = 1.0;   
       }

三、题外话

我们在调试代码的时候可能会遇到一个奇怪的问题，组合导航结果在某一时间段 出现很大的定位误差，但是抗差对其无用。

       正常人的思维方式都司卫星观测值不好，其实还会有一种情况，那便是IMU的参数不对 ！很多人都会忽略IMU问题，但是确实存在相应参数不准确的情况，遇到改问题，分两步处理！

•  单点定位 看看GNSS的定位结果，若发现无异常，则是IMU问题；
• IMU参数 调大即可 （一般把零偏不确定性设为之前的五倍左右）