【PSINS工具箱】基于工具箱的157滤波（EKF），观测量为航向角、三轴速度、三轴位置

本程序通过采用多重观测下的EKF算法，结合航向角、三轴速度、三轴位置等数据，提供了一种高效的状态估计解决方案，适用于各种动态定位需求。通过实时数据处理和精确估计，能够显著提升目标跟踪和导航的性能。

概述

本程序以位置、速度和航向角为观测量，构造157（15维状态量、7维观测量）的滤波，采用扩展卡尔曼滤波（EKF）进行状态估计。

主要模块

1. 数据输入：

   • 接收位置、速度和航向角的观测数据。
   • 数据格式化，准备进行滤波处理。

2. 状态预测：

   • 根据运动模型预测目标的下一个状态。
   • 计算状态转移矩阵和过程噪声协方差。

3. 测量更新：

   • 将观测数据与预测状态进行融合，更新状态估计。
   • 计算卡尔曼增益，最小化估计误差。

4. 结果输出：

   • 输出估计的状态信息，包括位置、速度和航向角。
   • 可视化结果，便于分析和评估滤波性能。

改进点

• 本程序在传统速度观测的基础上，进一步引入位置和航向角的观测数据，通过优化EKF算法，提高了状态估计的准确性和鲁棒性。
• 针对动态环境和非线性特性，进行了算法优化，确保在各种条件下都能保持良好的性能。

运行结果

AVP及其误差曲线：

三维轨迹曲线：

位置误差曲线：

速度误差曲线：

部分代码

% 【PSINS】位置、速度、航向角为观测的157，EKF。从速度观测的EKF153改进而来
% 2025-01-24/Ver1：位置与速度为观测量

% 清空工作空间，清除命令窗口，关闭所有图形窗口
clear; clc; close all;

rng(0); % 设置随机数种子为0，以确保结果可重复
glvs % 调用全局变量设置
psinstypedef(153); % 设置PSINS类型

% 读取轨迹文件
trj = trjfile('trj10ms.mat');

% 初始设置
[nn, ts, nts] = nnts(2, trj.ts); % 获取时间序列的参数
% imuerr = imuerrset(0.03, 100, 0.001, 5); % （注释掉的）设置IMU误差参数
imuerr = imuerrset(8, 14, 0.18, 57); % 设置IMU误差参数
imu = imuadderr(trj.imu, imuerr);  % 对IMU数据添加误差
% imuplot(imu); % （注释掉的）绘制IMU数据

% 设置初始姿态误差
davp0 = avperrset([1; 1; 10]*60, 0, [1; 1; 3]*0);

%% 速度观测EKF
ins = insinit(avpadderr(trj.avp0, davp0), ts); % 初始化惯性导航系统
rk = [1;1; 1; 1; poserrset([1;1;3])]; % 观测噪声的权重
kf = kfinit(ins, davp0, imuerr, rk); % 初始化卡尔曼滤波器

% 设置卡尔曼滤波器的噪声协方差矩阵
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9, 1)])^2; % IMU过程噪声
kf.Rk = diag(rk)^2; % 观测噪声
kf.Pxk = diag([davp0; imuerr.eb; imuerr.db]*1.0)^2; % 状态协方差矩阵
kf.Hk = [1,zeros(1,14);
    zeros(3, 3), eye(3), zeros(3, 9);
    zeros(3, 6), eye(3) , zeros(3, 6)]; % 修改观测方程
kf.Pmin = [avperrset(0.01, 1e-4, 0.1); gabias(1e-3, [1, 10])].^2; % 最小协方差
kf.pconstrain = 1; % 约束条件

完整代码下载链接：https://download.youkuaiyun.com/download/callmeup/90311241

如有需求，可通过下方卡片联系作者