自定义聚合滤波
使用调子函数优化数个聚变滤波的噪声参数,包括hrsfilter对象.示例显示如何定制成本函数实现各种优化目标
加载传感器数据与地面真
传感器数据包含传感器记录UAV执行小动作创建hrsfilter对象融合传感器数据并估计这些操作
负载AHRSCustomTune.mat创建滤波处理数据过滤器=ahrsfilter采样区Fs公司'DecimationFactor',10);过滤传感器数据并显示估计错误oEstInit = filt(sensorData.Accelerometer, sensorData.Gyroscope,sensorData.Magnetometer)!plotPerformance(oEstInit,groundTruth.Orientation,初始非调整错误);
引导滤波改善估计
性能表现hrsfilter不调噪声参数不理想使用调子函数提高滤波性能
重置(过滤)Cfg1=contrconfig赫斯过滤器,maxLitation20"ObjectiveLimit"0.0001tune(filt,sensorData,groundTruth(1:10:end,:),cfg1);
Iteration Parameter Metric _________ _________ ______ 1 AccelerometerNoise 0.4382 1 GyroscopeNoise 0.4371 1 MagnetometerNoise 0.4370 1 GyroscopeDriftNoise 0.4370 1 LinearAccelerationNoise 0.4202 1 MagneticDisturbanceNoise 0.4188 1 LinearAccelerationDecayFactor 0.4087 1 MagneticDisturbanceDecayFactor 0.4087 2 AccelerometerNoise 0.4086 2 GyroscopeNoise 0.4066 2 MagnetometerNoise 0.4066 2 GyroscopeDriftNoise 0.4066 2 LinearAccelerationNoise 0.3937 2 MagneticDisturbanceNoise 0.3932 2 LinearAccelerationDecayFactor 0.3856 2 MagneticDisturbanceDecayFactor 0.3854 3 AccelerometerNoise 0.3853 3 GyroscopeNoise 0.3826 3 MagnetometerNoise 0.3825 3 GyroscopeDriftNoise 0.3825 3 LinearAccelerationNoise 0.3690 3 MagneticDisturbanceNoise 0.3676 3 LinearAccelerationDecayFactor 0.3613 3 MagneticDisturbanceDecayFactor 0.3611 4 AccelerometerNoise 0.3610 4 GyroscopeNoise 0.3577 4 MagnetometerNoise 0.3576 4 GyroscopeDriftNoise 0.3576 4 LinearAccelerationNoise 0.3431 4 MagneticDisturbanceNoise 0.3414 4 LinearAccelerationDecayFactor 0.3364 4 MagneticDisturbanceDecayFactor 0.3363 5 AccelerometerNoise 0.3362 5 GyroscopeNoise 0.3328 5 MagnetometerNoise 0.3326 5 GyroscopeDriftNoise 0.3326 5 LinearAccelerationNoise 0.3190 5 MagneticDisturbanceNoise 0.3183 5 LinearAccelerationDecayFactor 0.3152 5 MagneticDisturbanceDecayFactor 0.3150 6 AccelerometerNoise 0.3149 6 GyroscopeNoise 0.3121 6 MagnetometerNoise 0.3119 6 GyroscopeDriftNoise 0.3119 6 LinearAccelerationNoise 0.3040 6 MagneticDisturbanceNoise 0.3035 6 LinearAccelerationDecayFactor 0.3024 6 MagneticDisturbanceDecayFactor 0.3022 7 AccelerometerNoise 0.3022 7 GyroscopeNoise 0.2990 7 MagnetometerNoise 0.2989 7 GyroscopeDriftNoise 0.2989 7 LinearAccelerationNoise 0.2970 7 MagneticDisturbanceNoise 0.2955 7 LinearAccelerationDecayFactor 0.2952 7 MagneticDisturbanceDecayFactor 0.2948 8 AccelerometerNoise 0.2948 8 GyroscopeNoise 0.2903 8 MagnetometerNoise 0.2902 8 GyroscopeDriftNoise 0.2902 8 LinearAccelerationNoise 0.2883 8 MagneticDisturbanceNoise 0.2860 8 LinearAccelerationDecayFactor 0.2856 8 MagneticDisturbanceDecayFactor 0.2851 9 AccelerometerNoise 0.2851 9 GyroscopeNoise 0.2778 9 MagnetometerNoise 0.2777 9 GyroscopeDriftNoise 0.2777 9 LinearAccelerationNoise 0.2709 9 MagneticDisturbanceNoise 0.2698 9 LinearAccelerationDecayFactor 0.2690 9 MagneticDisturbanceDecayFactor 0.2689 10 AccelerometerNoise 0.2689 10 GyroscopeNoise 0.2593 10 MagnetometerNoise 0.2593 10 GyroscopeDriftNoise 0.2593 10 LinearAccelerationNoise 0.2492 10 MagneticDisturbanceNoise 0.2490 10 LinearAccelerationDecayFactor 0.2482 10 MagneticDisturbanceDecayFactor 0.2482 11 AccelerometerNoise 0.2481 11 GyroscopeNoise 0.2370 11 MagnetometerNoise 0.2369 11 GyroscopeDriftNoise 0.2369 11 LinearAccelerationNoise 0.2240 11 MagneticDisturbanceNoise 0.2237 11 LinearAccelerationDecayFactor 0.2230 11 MagneticDisturbanceDecayFactor 0.2228 12 AccelerometerNoise 0.2227 12 GyroscopeNoise 0.2117 12 MagnetometerNoise 0.2117 12 GyroscopeDriftNoise 0.2117 12 LinearAccelerationNoise 0.1984 12 MagneticDisturbanceNoise 0.1979 12 LinearAccelerationDecayFactor 0.1974 12 MagneticDisturbanceDecayFactor 0.1974 13 AccelerometerNoise 0.1973 13 GyroscopeNoise 0.1878 13 MagnetometerNoise 0.1878 13 GyroscopeDriftNoise 0.1878 13 LinearAccelerationNoise 0.1766 13 MagneticDisturbanceNoise 0.1763 13 LinearAccelerationDecayFactor 0.1761 13 MagneticDisturbanceDecayFactor 0.1761 14 AccelerometerNoise 0.1760 14 GyroscopeNoise 0.1686 14 MagnetometerNoise 0.1685 14 GyroscopeDriftNoise 0.1685 14 LinearAccelerationNoise 0.1601 14 MagneticDisturbanceNoise 0.1599 14 LinearAccelerationDecayFactor 0.1597 14 MagneticDisturbanceDecayFactor 0.1597 15 AccelerometerNoise 0.1596 15 GyroscopeNoise 0.1536 15 MagnetometerNoise 0.1536 15 GyroscopeDriftNoise 0.1536 15 LinearAccelerationNoise 0.1472 15 MagneticDisturbanceNoise 0.1469 15 LinearAccelerationDecayFactor 0.1469 15 MagneticDisturbanceDecayFactor 0.1469 16 AccelerometerNoise 0.1468 16 GyroscopeNoise 0.1422 16 MagnetometerNoise 0.1422 16 GyroscopeDriftNoise 0.1422 16 LinearAccelerationNoise 0.1380 16 MagneticDisturbanceNoise 0.1378 16 LinearAccelerationDecayFactor 0.1377 16 MagneticDisturbanceDecayFactor 0.1377 17 AccelerometerNoise 0.1377 17 GyroscopeNoise 0.1352 17 MagnetometerNoise 0.1351 17 GyroscopeDriftNoise 0.1351 17 LinearAccelerationNoise 0.1351 17 MagneticDisturbanceNoise 0.1351 17 LinearAccelerationDecayFactor 0.1351 17 MagneticDisturbanceDecayFactor 0.1351 18 AccelerometerNoise 0.1351 18 GyroscopeNoise 0.1351 18 MagnetometerNoise 0.1351 18 GyroscopeDriftNoise 0.1351 18 LinearAccelerationNoise 0.1351 18 MagneticDisturbanceNoise 0.1351 18 LinearAccelerationDecayFactor 0.1350 18 MagneticDisturbanceDecayFactor 0.1350 19 AccelerometerNoise 0.1350 19 GyroscopeNoise 0.1348 19 MagnetometerNoise 0.1344 19 GyroscopeDriftNoise 0.1344 19 LinearAccelerationNoise 0.1344 19 MagneticDisturbanceNoise 0.1344 19 LinearAccelerationDecayFactor 0.1344 19 MagneticDisturbanceDecayFactor 0.1344 20 AccelerometerNoise 0.1344 20 GyroscopeNoise 0.1344 20 MagnetometerNoise 0.1344 20 GyroscopeDriftNoise 0.1344 20 LinearAccelerationNoise 0.1344 20 MagneticDisturbanceNoise 0.1344 20 LinearAccelerationDecayFactor 0.1344 20 MagneticDisturbanceDecayFactor 0.1344
过滤传感器数据调试滤波并显示方向错误
oEstTuned = filt(sensorData.Accelerometer,sensorData.Gyroscope,sensorData.Magnetometer)!plotPerformance(oEstTuned,groundTruth.Orientation,转滤错误-默认配置);
自定义CostFcn和MATLAB编码器加速优化调试
滤波性能经调试后提高,但调试过程往往需要很长时间。上头调音器配置对象允许定制成本函数优化进程MATLAB编码器创建max函数加速调试速度自定义成本函数必须签名成本=fcn中位成本计算标量实数参数解析噪声参数阶梯优化传感器Data是一个传感器数据表,地面真象表格地面实情数据
从上段hrsfilter某些动作时没有很好估计方向自定义成本函数不使用默认root-均值误差,而使用更高顺序术语来更严厉地惩罚异常值
显示定制成本函数的细节函数绑定为m文件
类型自定义Fcn.m
函数C=自定义Fcn(参数、传感器Data、groundTruth)%h=ahrsfilter%参数化滤镜从%参数流出当前最优参数h.AccelerometerNoise = params.AccelerometerNoise!H.Gyrocle噪声=参数Gyrocle噪声h.MagnetometerNoise = params.MagnetometerNoise!h.GyroscopeDriftNoise = params.GyroscopeDriftNoise!h.LinearAccelerationNoise = params.LinearAccelerationNoise!h.MagneticDisturbanceNoise = params.MagneticDisturbanceNoise!h.LinearAccelerationDecayFactor = params.LinearAccelerationDecayFactor!h.MagneticDisturbanceDecayFactor = params.MagneticDisturbanceDecayFactor!h.ExpectedMagneticFieldStrength = params.ExpectedMagneticFieldStrength!引信传感器数据qest=h(传感器Data.加速计,传感器Data.GyrocsensorData.Magnetometer)!%计算方向错误d=det(qest,grootTruith.Orentation(1:decim:end))百分数使用六维电源重判外围值sqrt(平均数d
创建max文件需要示例输入参数创建参数实例并复制属性过滤器.
p={'AccelerometerNoise','DecimationFactor','ExpectedMagneticFieldStrength',.'GyroscopeDriftNoise','GyroscopeNoise','InitialProcessNoise','LinearAccelerationDecayFactor',.'LinearAccelerationNoise','MagneticDisturbanceDecayFactor','MagneticDisturbanceNoise',.'MagnetometerNoise','OrientationFormat',采样区}
p=1×13 cell加速计噪声}磁场扰动'}{磁场扰动>{{磁区扰动}{磁表噪声{{{磁表噪声}{
Foridx=1:Numel(p)parmEx.结束dep(参数Ex);
AccelerometerNoise: 5.4972e-07 DecimationFactor: 10 ExpectedMagneticFieldStrength: 50 GyroscopeDriftNoise: 4.0927e-11 GyroscopeNoise: 0.0041 InitialProcessNoise: [12×12 double] LinearAccelerationDecayFactor: 0.0050 LinearAccelerationNoise: 1.4370e-04 MagneticDisturbanceDecayFactor: 0.9872 MagneticDisturbanceNoise: 0.0360 MagnetometerNoise: 0.1278 OrientationFormat: 'quaternion' SampleRate: 200
生成代码
代码元自定义Fcn.m-args{paramEx传感器Data,rootTruth}
代码生成成功
使用max函数快速调音
scg=nockerconfig赫斯过滤器,费用计算,客户名,.CustomCostFcnserviceFn_mexmaxLitation20号"ObjectiveLimit"0.0001重置(过滤)调子(过滤器、传感器Data、地面图理、cfg);
Iteration Parameter Metric _________ _________ ______ 1 AccelerometerNoise 0.1581 1 GyroscopeNoise 0.1544 1 MagnetometerNoise 0.1544 1 GyroscopeDriftNoise 0.1544 1 LinearAccelerationNoise 0.1504 1 MagneticDisturbanceNoise 0.1498 1 LinearAccelerationDecayFactor 0.1497 1 MagneticDisturbanceDecayFactor 0.1474 2 AccelerometerNoise 0.1474 2 GyroscopeNoise 0.1437 2 MagnetometerNoise 0.1436 2 GyroscopeDriftNoise 0.1436 2 LinearAccelerationNoise 0.1395 2 MagneticDisturbanceNoise 0.1387 2 LinearAccelerationDecayFactor 0.1387 2 MagneticDisturbanceDecayFactor 0.1367 3 AccelerometerNoise 0.1367 3 GyroscopeNoise 0.1332 3 MagnetometerNoise 0.1332 3 GyroscopeDriftNoise 0.1332 3 LinearAccelerationNoise 0.1293 3 MagneticDisturbanceNoise 0.1284 3 LinearAccelerationDecayFactor 0.1284 3 MagneticDisturbanceDecayFactor 0.1271 4 AccelerometerNoise 0.1270 4 GyroscopeNoise 0.1243 4 MagnetometerNoise 0.1242 4 GyroscopeDriftNoise 0.1242 4 LinearAccelerationNoise 0.1209 4 MagneticDisturbanceNoise 0.1201 4 LinearAccelerationDecayFactor 0.1201 4 MagneticDisturbanceDecayFactor 0.1193 5 AccelerometerNoise 0.1193 5 GyroscopeNoise 0.1180 5 MagnetometerNoise 0.1178 5 GyroscopeDriftNoise 0.1178 5 LinearAccelerationNoise 0.1158 5 MagneticDisturbanceNoise 0.1152 5 LinearAccelerationDecayFactor 0.1152 5 MagneticDisturbanceDecayFactor 0.1147 6 AccelerometerNoise 0.1147 6 GyroscopeNoise 0.1147 6 MagnetometerNoise 0.1143 6 GyroscopeDriftNoise 0.1143 6 LinearAccelerationNoise 0.1132 6 MagneticDisturbanceNoise 0.1123 6 LinearAccelerationDecayFactor 0.1123 6 MagneticDisturbanceDecayFactor 0.1118 7 AccelerometerNoise 0.1118 7 GyroscopeNoise 0.1113 7 MagnetometerNoise 0.1108 7 GyroscopeDriftNoise 0.1108 7 LinearAccelerationNoise 0.1100 7 MagneticDisturbanceNoise 0.1093 7 LinearAccelerationDecayFactor 0.1093 7 MagneticDisturbanceDecayFactor 0.1093 8 AccelerometerNoise 0.1093 8 GyroscopeNoise 0.1088 8 MagnetometerNoise 0.1084 8 GyroscopeDriftNoise 0.1084 8 LinearAccelerationNoise 0.1084 8 MagneticDisturbanceNoise 0.1084 8 LinearAccelerationDecayFactor 0.1084 8 MagneticDisturbanceDecayFactor 0.1084 9 AccelerometerNoise 0.1084 9 GyroscopeNoise 0.1076 9 MagnetometerNoise 0.1072 9 GyroscopeDriftNoise 0.1072 9 LinearAccelerationNoise 0.1072 9 MagneticDisturbanceNoise 0.1072 9 LinearAccelerationDecayFactor 0.1072 9 MagneticDisturbanceDecayFactor 0.1071 10 AccelerometerNoise 0.1071 10 GyroscopeNoise 0.1068 10 MagnetometerNoise 0.1065 10 GyroscopeDriftNoise 0.1065 10 LinearAccelerationNoise 0.1062 10 MagneticDisturbanceNoise 0.1060 10 LinearAccelerationDecayFactor 0.1060 10 MagneticDisturbanceDecayFactor 0.1059 11 AccelerometerNoise 0.1059 11 GyroscopeNoise 0.1057 11 MagnetometerNoise 0.1055 11 GyroscopeDriftNoise 0.1055 11 LinearAccelerationNoise 0.1049 11 MagneticDisturbanceNoise 0.1048 11 LinearAccelerationDecayFactor 0.1048 11 MagneticDisturbanceDecayFactor 0.1048 12 AccelerometerNoise 0.1048 12 GyroscopeNoise 0.1047 12 MagnetometerNoise 0.1045 12 GyroscopeDriftNoise 0.1045 12 LinearAccelerationNoise 0.1038 12 MagneticDisturbanceNoise 0.1036 12 LinearAccelerationDecayFactor 0.1036 12 MagneticDisturbanceDecayFactor 0.1035 13 AccelerometerNoise 0.1035 13 GyroscopeNoise 0.1035 13 MagnetometerNoise 0.1033 13 GyroscopeDriftNoise 0.1033 13 LinearAccelerationNoise 0.1029 13 MagneticDisturbanceNoise 0.1027 13 LinearAccelerationDecayFactor 0.1027 13 MagneticDisturbanceDecayFactor 0.1027 14 AccelerometerNoise 0.1027 14 GyroscopeNoise 0.1024 14 MagnetometerNoise 0.1021 14 GyroscopeDriftNoise 0.1021 14 LinearAccelerationNoise 0.1019 14 MagneticDisturbanceNoise 0.1018 14 LinearAccelerationDecayFactor 0.1018 14 MagneticDisturbanceDecayFactor 0.1018 15 AccelerometerNoise 0.1018 15 GyroscopeNoise 0.1014 15 MagnetometerNoise 0.1012 15 GyroscopeDriftNoise 0.1012 15 LinearAccelerationNoise 0.1012 15 MagneticDisturbanceNoise 0.1012 15 LinearAccelerationDecayFactor 0.1011 15 MagneticDisturbanceDecayFactor 0.1011 16 AccelerometerNoise 0.1011 16 GyroscopeNoise 0.1008 16 MagnetometerNoise 0.1008 16 GyroscopeDriftNoise 0.1008 16 LinearAccelerationNoise 0.1006 16 MagneticDisturbanceNoise 0.1005 16 LinearAccelerationDecayFactor 0.1004 16 MagneticDisturbanceDecayFactor 0.1004 17 AccelerometerNoise 0.1004 17 GyroscopeNoise 0.1001 17 MagnetometerNoise 0.1001 17 GyroscopeDriftNoise 0.1001 17 LinearAccelerationNoise 0.0998 17 MagneticDisturbanceNoise 0.0998 17 LinearAccelerationDecayFactor 0.0996 17 MagneticDisturbanceDecayFactor 0.0995 18 AccelerometerNoise 0.0995 18 GyroscopeNoise 0.0992 18 MagnetometerNoise 0.0992 18 GyroscopeDriftNoise 0.0992 18 LinearAccelerationNoise 0.0990 18 MagneticDisturbanceNoise 0.0989 18 LinearAccelerationDecayFactor 0.0987 18 MagneticDisturbanceDecayFactor 0.0986 19 AccelerometerNoise 0.0986 19 GyroscopeNoise 0.0980 19 MagnetometerNoise 0.0980 19 GyroscopeDriftNoise 0.0980 19 LinearAccelerationNoise 0.0980 19 MagneticDisturbanceNoise 0.0980 19 LinearAccelerationDecayFactor 0.0978 19 MagneticDisturbanceDecayFactor 0.0975 20 AccelerometerNoise 0.0975 20 GyroscopeNoise 0.0965 20 MagnetometerNoise 0.0965 20 GyroscopeDriftNoise 0.0965 20 LinearAccelerationNoise 0.0964 20 MagneticDisturbanceNoise 0.0964 20 LinearAccelerationDecayFactor 0.0964 20 MagneticDisturbanceDecayFactor 0.0964
过滤数据并显示方向错误
oEst = filt(sensorData.Accelerometer, sensorData.Gyroscope, sensorData.Magnetometer)!plotPerformance(oEst, groundTruth.Orientation,推荐滤波错误-自定义成本函数);
金宝app支持函数
绘图性能绘制方向错误
函数显示properforceance(est,act,txt)%绘制新图窗口方向错误度图解plot(rad2deg(dist(est, act(1:10:end))))!ylabl词典标题(txt);结束
