粒子滤波在基于误差状态量的INS/GNSS组合导航中的使用技巧-On Particle Filter based Error-State INS/GNSS Intergrated Navigation

ZhDM_bit

已于 2024-01-10 17:34:55 修改

阅读量1.4k

点赞数 23

分类专栏： SINS 文章标签： matlab 经验分享机器人

于 2024-01-09 16:30:33 首次发布

本文链接：https://blog.csdn.net/I33D33I/article/details/135478926

版权

SINS 专栏收录该内容

1 篇文章

订阅专栏

背景

摘要

1. 运行bug（pfinit.m）

2. PF通用框架（pfupdate.m）

3. **PF在运用于基于误差状态量的INS/GNSS组合导航时的处理技巧（test_SINS_GPS_PF_153.m）

总结

背景

月初，应审稿人要求，笔者要对比分析粒子滤波（PF）和扩展卡尔曼滤波（EKF）的效果表现。在投文章中的状态量选取为15维惯导（INS）误差状态量（包括9维导航误差量和6维传感器零偏）。应对此类问题时，EKF已被证实是非常成熟有效的滤波方法，而相同背景下PF缺少较为详细的论文介绍和网络资料。在苦于编程实现基于PF的惯导误差状态量组合导航方法时，检索到西工大严恭敏老师的开源工具箱中有PF相关库函数，但遗憾的是PF工具暂且运行不对。后从严老师的有关博客中获得灵感，笔者尝试修复PF工具并运行出与EKF相近的结果。仿真程序中状态量选取为15维惯导（INS）误差状态量，量测量为3维位置误差。现通过博客分享具体修复要点并发布相关源文件供感兴趣的读者检验和批评。

注：本文工作所基于的工具箱为严老师2023年9月23日发布MATLAB版本psins230923.rar，可能不适用于其他版本。在此向严老师表示感谢。

摘要

修复主要包括以下3点：

运行bug（pfinit.m）；
PF通用框架（pfupdate.m）；
PF在运用于基于误差状态量的INS/GNSS组合导航时的处理技巧（test_SINS_GPS_PF_153.m）。

1. 运行bug（pfinit.m）

函数pfinit()在输出时，误把方差阵赋值成其对角元，应在21行做以下修改。

pf.Pxk = diag(P0);

2. PF通用框架（pfupdate.m）

在原有pf结构中增加pf.weight属性。

pf.weight = ones(pf.Npts,1)/pf.Npts;

在原有函数pfupdate()中更改量测更新时似然率和权值计算，如下：

for k=1:pf.Npts
    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
       if isfield(pf,'hx')
           v = Zk - feval(pf.hx, pf.particles(:,k), pf.tpara)';
       else
           v = Zk - pf.Hk*pf.particles(:,k);
       end
           vRv(k) = sqrt(v'*invR*v);
    end
end
if TimeMeasBoth=='M' || TimeMeasBoth=='B'
    likelihood = exp(-vRv/2) + 1e-99; % Normal distribution, and avoid zero sum
    pf.weight = pf.weight.*likelihood;
    pf.weight = pf.weight/sum(pf.weight);
    % best state
    pf.xk = pf.particles*pf.weight;
end

在原有函数pfupdate()中更改协方差计算，如下：

weightSqr = sum(pf.weight.*pf.weight);
if abs(weightSqr - 1.0) < sqrt(eps)
    % from matlab, to avoid NaN values, use factor of 1.0 if the sum of
    % squared weights is close to 1.0.
    factor = 1.0;
else
    factor = 1.0;
    factor = factor / (factor - weightSqr);
end
xk = bsxfun(@minus, pf.particles, pf.xk);
% covariance
pf.Pxk = factor * (bsxfun(@times, xk, pf.weight') * xk.');

从MATLAB库中迁移基于残差的粒子重采样方法，为节约篇幅具体请参考主页资源文件中pfupdate.m中pfresample()函数。

3. **PF在运用于基于误差状态量的INS/GNSS组合导航时的处理技巧（test_SINS_GPS_PF_153.m）

当估计状态量选取的是惯导误差且使用EKF作为滤波方法时，量测更新所估计出的最佳误差状态量会被反馈补偿至惯导系统中。在执行反馈补偿后通常认为误差量恢复至小量，因此会将前一时刻误差状态量乘以非常小的值或者直接置零（具体参考工具箱中kffeedback.m）。因此，在采用PF作为滤波方法时同样需要对状态量进行重置，但由于PF依赖粒子采样的多样性，重置操作有别于EKF置零。具体实现是，针对经过反馈补偿后的状态量，以0均值和更新后的协方差pf.Pxk重新生成相应数量的粒子。如下：

% reset the particles, as done to EKF to reset error states
if strcmp(fb,'avp')
    num_reset=9;
end
randData = randn(pf.Npts, num_reset);
stdDev = chol(pf.Pxk(1:num_reset,1:num_reset));
zeroMeanSamples = randData * stdDev;
temp_samples = bsxfun(@plus, zeroMeanSamples', zeros(num_reset,1));
pf.particles(1:num_reset,:)=temp_samples;

注：粒子重置是运用于基于误差状态量组合导航这一特殊场景下的操作，与粒子滤波中的重采样无关。另外，由于粒子在每次量测更新后会进行重置，原本粒子滤波中的重采样仅起到恢复权重的作用，不对粒子进行筛选。
在进行以上操作后运行3600秒仿真。往往初期估计效果较好，经过数百秒至数千秒会随机发散。猜测其原因，应该是状态量维数较高且传递函数复杂，粒子数过少时难以采样到真实分布从而导致偶然的发散直至失效。以下图分别为100、500、2000和10000粒子数时轨迹估计效果。

经过验证，当粒子数为10000时，估计效果较好，接近与EKF，但运行耗时高达100分钟（i7-8700 CPU），在应用时应谨慎选择。

总结

尚存问题：即便粒子数为10000时，PF中陀螺协方差的收敛效果距离EKF差距较大，甚至始终不收敛，不排除是笔者代码本身有误。
笔者接触粒子滤波较少、经验尚浅，本文工作是以实现PF基本功能为导向而完成的，因此在分析和处理上难免出现纰漏之处，还请相关专家批评指正。
笔者更新的粒子滤波版本运行耗时严重，在给审稿人的回复中说明了粒子滤波在应对15维误差状态量组合导航这一特殊场景时的局限性。笔者不否认PF在其他场景下的优势，不排除通过引入自适应粒子数等改进算法优化耗时等，本文抛转引玉，希望PF在导航领域发挥更大作用。
感谢严老师贡献的各类开源资料对笔者在惯导和组合导航的学习中的帮助和启发。