<think>我们遇到了一个错误：在调用kfinit函数时，提示无法识别的字段名称"typestr"。这通常是因为psinsdef结构体中缺少typestr字段。这可能是因为我们之前修改了状态定义（psinstypedef(187)）而psinsdef没有被正确更新。在PSINS工具箱中，psinsdef是一个全局结构体，包含各种类型的定义。当我们使用psinstypedef(187)定义新的状态类型时，需要确保psinsdef被正确更新。解决方案：1.在调用kfinit之前，确保已经正确设置了状态类型（187）并更新了psinsdef。2.检查PSINS工具箱版本，确保支持187类型。但是，由于PSINS工具箱的封闭性，我们可能需要自定义初始化函数。因此，我们可以采取以下步骤：-不使用kfinit函数，而是手动初始化卡尔曼滤波器。-或者，我们检查psinsdef是否已经更新，如果没有，则手动更新。由于错误提示在kfinit的第101行，我们查看PSINS源代码（kfinit.m）发现：kf=feval(psinsdef.typestr,psinsdef.kfinittag,[{ins},varargin]);这里psinsdef.typestr应该是一个字符串，表示当前状态类型的初始化函数。当我们调用psinstypedef(187)时，应该设置psinsdef.typestr为'kfinit187'之类的字符串，并且存在一个名为kfinit187.m的文件。所以，可能的原因是：我们没有为187类型提供相应的初始化函数。因此，我们需要：1.创建一个新的初始化函数kfinit187.m。2.或者，修改现有的初始化函数（如kfinit186）来支持24状态。这里我们选择第二种方法：基于186类型（18状态）的初始化函数修改为24状态。我们复制kfinit186.m为kfinit187.m，并修改以支持24状态。由于无法直接修改PSINS源代码，我们在当前工作目录下创建kfinit187.m，内容如下：```matlabfunctionkf=kfinit187(ins,davp,imuerr,lever,r0)%24-stateKFinitial%状态:姿态误差(3),速度误差(3),位置误差(3),陀螺零偏(3),加表零偏(3),杆臂补偿(3),位置漂移(3),速度漂移(3)%输入:ins-初始的导航参数结构体%davp-初始导航误差[att_err(1:3);dvn_err(4:6);dpos_err(7:9)](rad,m/s,m)%imuerr-IMU误差参数结构体%lever-杆臂向量(m)%r0-GPS观测噪声(m/s,m)[rv;rp]%输出:kf-卡尔曼滤波器结构体kf=[];kf.Qk=diag([imuerr.web;imuerr.wdb;zeros(3,1);imuerr.sqg;imuerr.sqa;zeros(6,1)])^2;%过程噪声%注意：需要为新增的6个状态设置过程噪声，这里初始设为0，后续再修改kf.Rk=diag([r0(1:3);r0(4:6)])^2;%量测噪声kf.Pxk=diag([davp(1:3);davp(4:6);davp(7:9);imuerr.eb;imuerr.db;lever*0;0.01*ones(3,1);0.001*ones(3,1)])^2;%初始协方差，注意杆臂初始误差设为0（因为杆臂是已知的固定值，但作为状态估计，初始误差可设较小），新增的位置漂移和速度漂移初始方差设置%状态向量和维数kf.x=zeros(24,1);%初始状态估计kf.n=24;%其他设置kf.Phikk_1=eye(kf.n);%状态转移矩阵初始为单位阵kf.Hk=zeros(6,kf.n);%量测矩阵kf=kfinit0(kf);%设置一些默认值```这样，当调用`kfinit`函数时，由于我们之前执行了`psinstypedef(187)`，它会查找`kfinit187`函数。但是，为了确保psinsdef被正确更新，我们可以手动设置：```matlabpsinsdef.typestr='kfinit187';```或者，按照PSINS的规范，使用`psinstypedef`应该会自动更新。因此，我们修改主程序：1.创建kfinit187.m文件（如上述内容）。2.在主程序中调用`psinstypedef(187)`后，确保工作路径包含该文件。另外，错误信息指出在kfinit中使用了psinsdef.typestr，所以我们需要在调用kfinit之前，确保psinsdef.typestr已经被正确设置为'kfinit187'。如果这样修改后仍然不行，我们可以尝试自己初始化kf结构而不使用kfinit函数。但是，为了遵循PSINS的结构，我们尽量使用工具箱的函数。所以，修改后的步骤：步骤一：在当前目录下创建kfinit187.m文件（内容如上）。步骤二：修改主程序，在调用kfinit之前，确保psinsdef.typestr已经更新。但psinstypedef(187)应该已经做了这个工作。若没有，我们可以手动设置：```matlabglobalpsinsdefpsinsdef.typestr='kfinit187';```步骤三：运行程序。如果还有错误，我们也可以尝试直接调用kfinit187函数：```matlab%替换原来的kfinit调用kf=kfinit187(ins,davp0,imuerr,lever,r0);```然后，在kfinit187函数内部，我们按照24状态进行初始化。这样，我们就避开了类型检查。下面给出修改后的主程序部分：```matlab%...前面的代码...%初始化24状态模型psinstypedef(187);%定义24状态模型%如果psinstypedef没有设置psinsdef.typestr，我们手动设置globalpsinsdefpsinsdef.typestr='kfinit187';%确保指定了正确的初始化函数%初始化卡尔曼滤波器%kf=kfinit(ins,davp0,imuerr,lever,r0);%如果kfinit内部调用错误，我们直接调用kfinit187kf=kfinit187(ins,davp0,imuerr,lever,r0);%然后继续后续的扩展（设置过程噪声等）...%因为在kfinit187中我们已经设置了kf.n=24,并初始化了状态和协方差，所以后面的扩展可以省略%但是，我们在kfinit187中新增状态的过程噪声初始为0，需要补充：%新增过程噪声：位置漂移和速度漂移kf.Qk=blkdiag(kf.Qk,diag([1e-5,1e-5,1e-5,1e-7,1e-7,1e-7]));%根据实际情况调整%注意：kfinit187中已经将新增状态的初始协方差设置，但过程噪声初始为0，这里我们重新设置整个Qk%其实在kfinit187中已经设置了Qk，但为了灵活性，我们可以在主程序中覆盖它%或者，我们在kfinit187中设置好Qk，这样主程序就可以直接使用```由于上述修改涉及工具箱内部函数，我们需要确保正确性。如果问题仍然存在，我们可以尝试完全手动初始化卡尔曼滤波器结构，但会复杂很多。因此，我们提供一个完整的kfinit187.m文件：```matlab:kfinit187.mfunctionkf=kfinit187(ins,davp,imuerr,lever,r0)%24-stateKFinitialfortype187%States:phi(3),dvn(3),dpos(3),eb(3),db(3),lever(3),dpos_drift(3),dvn_drift(3)%输入:%ins-初始导航结构%davp-初始导航误差(att,vel,pos)%imuerr-IMU误差参数%lever-杆臂向量%r0-测量噪声[速度测量噪声(3),位置测量噪声(3)]kf=[];kf.n=24;%状态数%状态初始化为零kf.x=zeros(kf.n,1);%过程噪声协方差矩阵%原有状态的过程噪声：姿态(3),速度(3),位置(3)没有过程噪声（由IMU噪声驱动）%IMU零偏的过程噪声：陀螺零偏、加表零偏%杆臂：通常建模为常值，过程噪声为0%新状态：位置漂移(3)和速度漂移(3)的过程噪声%设置Qk为对角阵nQ=zeros(24,1);nQ(10:12)=imuerr.sqg;%陀螺零偏的过程噪声标准差nQ(13:15)=imuerr.sqa;%加表零偏的过程噪声标准差nQ(16:18)=0;%杆臂的过程噪声设为0（常值）nQ(19:21)=0.0001;%位置漂移过程噪声，单位(m/s^2)^2nQ(22:24)=0.00001;%速度漂移过程噪声，单位(m/s^3)^2kf.Qk=diag(nQ.^2);%转化为方差%测量噪声kf.Rk=diag([r0(1:3);r0(4:6)]).^2;%初始协方差矩阵P=zeros(kf.n);P(1:3,1:3)=diag(davp(1:3)).^2;P(4:6,4:6)=diag(davp(4:6)).^2;P(7:9,7:9)=diag(davp(7:9)).^2;P(10:12,10:12)=diag(imuerr.eb).^2;P(13:15,13:15)=diag(imuerr.db).^2;P(16:18,16:18)=(0.001)^2*eye(3);%杆臂初始协方差设为较小值P(19:21,19:21)=(0.01)^2*eye(3);%位置漂移初始协方差P(22:24,22:24)=(0.001)^2*eye(3);%速度漂移初始协方差kf.Pxk=P;%状态转移矩阵初始为单位阵（每次会重新计算）kf.Phikk_1=eye(kf.n);%测量矩阵初始为0（每次更新时会重新计算）kf.Hk=zeros(6,kf.n);%设置kf的其他默认参数kf=kfinit0(kf);%这个函数在PSINS中用于设置一些默认字段end```同时，在主程序中，我们这样调用：```matlab%...其他初始化...%设置24状态模型psinstypedef(187);globalpsinsdef;psinsdef.typestr='kfinit187';%初始化卡尔曼滤波器kf=kfinit187(ins,davp0,imuerr,lever,r0);%不需要再扩展状态，因为已经在kfinit187中完成%主循环保持不变```注意：需要将kfinit187.m放在MATLAB路径下（例如当前工作目录）。这样修改后，应该可以解决错误。§§相关问题§§:1.如何验证新增的状态（位置漂移和速度漂移）在卡尔曼滤波器中是可观测的？2.在哪些实际应用中需要考虑位置和速度的系统漂移？3.当系统发生突变（例如IMU受到冲击）时，如何检测并重置漂移参数的估计？4.如何调整过程噪声参数以适应不同的运动条件？</think>
针对您在使用 PSINS 工具箱时遇到的"无法识别的字段名称 'typestr'"错误，我来提供完整的解决方案。这个问题通常是因为 PSINS 工具箱版本兼容性或状态类型定义问题导致的。
### 完整解决方案：修复 PSINS 错误并实现完整系统
```matlab
% 解决 'typestr' 错误的完整 SINS/GPS 实现 (24 状态模型)
% 确保加载了 PSINS 工具箱
addpath(genpath('psins')); % 替换为您的 PSINS 路径
% ====== 修改 PSINS 工具函数（避免 typestr 错误） ======
function kf = my_kfinit(ins, davp, imuerr, lever, r0, state_dim)
% 手动初始化卡尔曼滤波器 (避免 typestr 错误)
kf = [];
kf.n = state_dim; % 24 状态
kf.x = zeros(kf.n,1);
% 设置初始协方差
P_att = diag(davp(1:3)).^2;
P_vel = diag(davp(4:6)).^2;
P_pos = diag(davp(7:9)).^2;
P_eb = diag(imuerr.eb).^2;
P_db = diag(imuerr.db).^2;
P_lever = diag([0.01, 0.01, 0.01]); % 杆臂初始不确定度
P_dpos_drift = diag([0.01, 0.01, 0.01]); % 位置漂移初始不确定度
P_dvn_drift = diag([0.001, 0.001, 0.001]); % 速度漂移初始不确定度
kf.Pxk = blkdiag(P_att, P_vel, P_pos, P_eb, P_db, P_lever, P_dpos_drift, P_dvn_drift);
% 设置过程噪声
Q_gyro = diag(imuerr.web).^2;
Q_acc = diag(imuerr.wdb).^2;
Q_bias_g = diag(imuerr.sqg).^2;
Q_bias_a = diag(imuerr.sqa).^2;
Q_lever = zeros(3);
Q_dpos_drift = diag([1e-5, 1e-5, 1e-5]); % 位置漂移过程噪声
Q_dvn_drift = diag([1e-7, 1e-7, 1e-7]); % 速度漂移过程噪声
kf.Qk = blkdiag(Q_gyro, Q_acc, zeros(3), Q_bias_g, Q_bias_a, Q_lever, Q_dpos_drift, Q_dvn_drift);
% 设置测量噪声
kf.Rk = diag([r0(1:3); r0(4:6)]).^2;
% 初始化状态转移矩阵
kf.Phikk_1 = eye(kf.n);
end
% ====== 主程序 ======
% 初始化全局导航参数
glvs;
% 检查轨迹数据
if ~exist('trj10ms.mat', 'file')
error('请运行 test_SINS_trj.m 生成 trj10ms.mat');
end
% 加载轨迹数据
trj = load('trj10ms.mat');
trj = trj.trj;
[nn, ts, nts] = nnts(2, trj.ts); % 设置更新间隔
% IMU 误差参数
imuerr = struct(...
'gryb', zeros(1,3), ... % 陀螺零偏
'arw', [0.1,0.1,0.1], ... % 角度随机游走
'accb', zeros(1,3), ... % 加速度计零偏
'vrw', [0.01,0.01,0.01] % 速度随机游走
);
% 初始导航误差
davp0 = [0.5, -0.5, 30, 0.1, 0.1, 0.1, 1, 1, 3]; % [姿态(deg); 速度(m/s); 位置(m)]
% 杆臂补偿
lever = [1; 2; 3]; % GPS 天线到 IMU 的偏移
% 模拟 GPS 测量
gps = gpssimu(trj.avp, davp0(4:6), davp0(7:9), 1, lever);
% 时间同步
imugpssyn(imu(:,7), gps(:,7)); % 对齐时间戳
% 初始化 INS
ins = insinit(trj.avp0(1:9), ts, davp0);
% GPS 测量噪声
r0 = [0.1, 0.1, 0.1, 1, 1, 1]; % [速度(m/s), 位置(m)]
% ====== 使用自定义初始化函数 ======
kf = my_kfinit(ins, davp0, imuerr, lever, r0, 24);
% ====== 主仿真循环 ======
len = size(imu,1);
[avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
ki = 1;
% 进度条
hwait = waitbar(0, '24-状态 SINS/GPS 仿真中...');
for k = 1:nn:len-nn+1
k1 = k+nn-1;
wvm = imu(k:k1, 1:6);
t = imu(k1,7);
% 1. INS 机械编排
ins = insupdate(ins, wvm);
% 2. 卡尔曼滤波时间更新
% 状态转移矩阵 (24 状态)
kf.Phikk_1 = [
eye(3), -skewm(ins.web)*nts, zeros(3), ins.Cnb*nts, zeros(3), zeros(3), zeros(3), zeros(3);
zeros(3), eye(3), -skewm(ins.wnb)*nts, zeros(3), ins.Cnb*nts, zeros(3), zeros(3), zeros(3);
zeros(3), eye(3)*nts, eye(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3);
zeros(3), zeros(3), zeros(3), diag(exp(-imuerr.sqg*nts)), zeros(3), zeros(3), zeros(3), zeros(3);
zeros(3), zeros(3), zeros(3), zeros(3), diag(exp(-imuerr.sqa*nts)), zeros(3), zeros(3), zeros(3);
zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), eye(3), zeros(3), zeros(3);
zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), eye(3), zeros(3);
zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), eye(3);
];
% 协方差预测
kf.Pxk = kf.Phikk_1 * kf.Pxk * kf.Phikk_1' + kf.Qk;
% 3. 测量更新 (GPS 可用时)
kgps = find(gps(:,7) >= imu(k,7) & gps(:,7) <= imu(k1,7), 1);
if ~isempty(kgps)
vnGPS = gps(kgps, 1:3)';
posGPS = gps(kgps, 4:6)';
% 杆臂补偿
ins = inslever(ins);
% 测量矩阵 (24 状态)
kf.Hk = [
zeros(3,3), eye(3), zeros(3,3), zeros(3,3), zeros(3,3), -skewm(ins.wnb)*ins.Mpv, zeros(3,3), eye(3);
zeros(3,3), zeros(3,3), eye(3), zeros(3,3), zeros(3,3), -ins.Mpv, eye(3), zeros(3,3)
];
% 含漂移的残差计算
dt_gps = gps(kgps,7) - ins.t0;
pos_ins_drift = ins.posL + kf.x(19:21)*dt_gps;
vn_ins_drift = ins.vnL + kf.x(22:24)*dt_gps;
zk = [vn_ins_drift - vnGPS; pos_ins_drift - posGPS];
% 卡尔曼更新
K = kf.Pxk * kf.Hk' / (kf.Hk * kf.Pxk * kf.Hk' + kf.Rk);
kf.x = kf.x + K * zk;
kf.Pxk = (eye(kf.n) - K * kf.Hk) * kf.Pxk;
% 反馈校正
ins.qnb = qdelphi(ins.qnb, kf.x(1:3));
ins.vn = ins.vn - kf.x(4:6);
ins.pos = ins.pos - kf.x(7:9);
ins.eb = ins.eb - kf.x(10:12);
ins.db = ins.db - kf.x(13:15);
kf.x(1:15) = 0; % 重置校正状态
% 存储结果
avp(ki,:) = [q2att(ins.qnb)', ins.vn', ins.pos', t];
xkpk(ki,:) = [kf.x; diag(kf.Pxk); t]';
ki = ki + 1;
end
% 更新进度条
waitbar(k/len, hwait);
end
close(hwait);
avp(ki:end,:) = [];
xkpk(ki:end,:) = [];
% ====== 结果分析与绘图 ======
% 1. 导航误差计算
true_avp = interp1(trj.avp(:,end), trj.avp(:,1:9), avp(:,end), 'linear');
pos_err = avp(:,7:9) - true_avp(:,7:9);
% 2. 位置漂移估计
figure;
subplot(211);
plot(avp(:,end), xkpk(:,19:21));
title('位置漂移估计');
xlabel('时间 (s)');
ylabel('漂移率 (m/s)');
legend('东', '北', '天');
subplot(212);
plot(avp(:,end), sqrt([xkpk(:,kf.n+19).^2, xkpk(:,kf.n+20).^2, xkpk(:,kf.n+21).^2]));
title('位置漂移估计标准差');
xlabel('时间 (s)');
ylabel('标准差 (m/s)');
legend('东', '北', '天');
% 3. 位置误差分析
figure;
plot(avp(:,end), pos_err);
title('位置误差');
xlabel('时间 (s)');
ylabel('误差 (m)');
legend('东', '北', '天');
% 4. 性能指标
fprintf('\n最终位置误差: %.3f m E, %.3f m N, %.3f m U\n', ...
pos_err(end,1), pos_err(end,2), pos_err(end,3));
fprintf('位置漂移估计均值: %.5f m/s E, %.5f m/s N, %.5f m/s U\n', ...
mean(xkpk(:,19)), mean(xkpk(:,20)), mean(xkpk(:,21)));
fprintf('位置漂移估计标准差: %.5f m/s E, %.5f m/s N, %.5f m/s U\n', ...
std(xkpk(:,19)), std(xkpk(:,20)), std(xkpk(:,21)));
```
### 错误原因与解决方案解释
1. **错误原因分析**：
- `typestr` 错误通常发生在旧版本 PSINS 工具箱中
- `psinstypedef(187)` 未正确更新全局结构体 `psinsdef`
- 状态扩展时未正确处理初始化和状态转移矩阵
2. **关键解决方案**：
```matlab
% 自定义卡尔曼滤波器初始化函数 (避免 typestr 问题)
function kf = my_kfinit(ins, davp, imuerr, lever, r0, state_dim)
kf = [];
kf.n = state_dim; % 24 状态
...
```
- 完全避免了 `kfinit` 函数调用
- 手动初始化所有必需的滤波器参数
3. **漂移误差建模**：
```matlab
% 漂移状态的动态模型 (单位矩阵保持恒定)
kf.Phikk_1 = [
...
zeros(3,3), eye(3), zeros(3,3);
zeros(3,3), zeros(3,3), eye(3);
];
```
- 位置漂移(dpos_drift)和速度漂移(dvn_drift)保持恒定
- 这是简单的随机游走模型，适用于慢变化误差
4. **含漂移的残差计算**：
```matlab
% 包含漂移补偿的测量残差
dt_gps = gps(kgps,7) - ins.t0;
pos_ins_drift = ins.posL + kf.x(19:21)*dt_gps;
vn_ins_drift = ins.vnL + kf.x(22:24)*dt_gps;
zk = [vn_ins_drift - vnGPS; pos_ins_drift - posGPS];
```
- 通过 `漂移率 × 时间` 将漂移添加到测量值中
- 更精确地模拟了随时间线性增长的系统误差
5. **24状态定义**：
```
1-3: 姿态误差 (phi)
4-6: 速度误差 (dvn)
7-9: 位置误差 (dpos)
10-12: 陀螺零偏 (eb)
13-15: 加速度计零偏 (db)
16-18: 杆臂补偿 (lever)
19-21: 位置漂移 (dpos_drift) // 新增
22-24: 速度漂移 (dvn_drift) // 新增
```
### 修复后系统的优势
1. **无依赖关系**：完全避开了 `kfinit` 函数的 `typestr` 问题
2. **更好的可移植性**：不依赖特定 PSINS 版本
3. **明确的漂移建模**：将随时间增长的误差纳入状态估计
4. **完整的调试工具**：包含详细的性能分析和可视化
### 验证建议
1. **检查位置漂移估计**：在长时间静态测试中应趋近于零
2. **注入人工漂移**：验证滤波器是否能准确估计已知漂移
3. **蒙特卡洛仿真**：评估不同噪声水平下的鲁棒性
刚踏入北漂一族的作者，分享了初入北京的体验，包括天气的寒冷、皮肤的干燥以及高昂的生活成本。尽管住宿条件不错，但价格与家乡相似，仍让作者感到一丝寒意。暂时借住在朋友家中，正准备开始寻找工作。北京网络公司众多，作者将逐步展开求职之旅。
扫一扫
2268
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
