关于工具箱

kfplot是图像绘制的函数，用于绘制关于滤波（前/后）的误差图像。
本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

• PSINS初学指导：https://blog.csdn.net/callmeup/article/details/137087932

使用方法

函数形式为：
$$kfplot(xkpk,varargin)$$
其中xkpk为卡尔曼滤波的时候生成的滤波值、协方差对角线的数据等值。
剩余的变量为相关误差。

例程实践

运行代码

运行经典的例程test_SINS_GPS_153，最后两张图即为函数kfplot绘制的。
使用的代码为：
$$kfplot(xkpk,avperr,imuerr);$$
其中：

1. avperr为滤波后的avp误差
2. imuerr为imu的误差

运行结果

• 第一张，绘制滤波后的avp误差时序图：
  

• 第二张，绘制滤波后的avp对应的状态标准差：
  

源代码

function kfplot(xkpk, varargin)
global glv
global psinsdefif nargin>1  % kfplot(xk, pk, clm)if psinsdef.kfplot==0feval(psinsdef.typestr, psinsdef.kfplottag, [{xkpk},varargin]); return;endif size(xkpk,2)==size(varargin{1},2)if nargin==3, clm=varargin{2}; else, clm=1:psinsdef.kfplot; end;if length(clm)==1, clm=1:clm; endkfplot([xkpk(:,clm),varargin{1}(:,[clm,end])]);return;endendswitch psinsdef.kfplotcase {15,16,18,19} % psinsdef.kfplotxxinserrplot(xkpk(:,[1:psinsdef.kfplot,end]));if nargin>1if psinsdef.kfplot==15trueplot15(varargin);elseif psinsdef.kfplot==18trueplot18(varargin);elseif psinsdef.kfplot==19trueplot19(varargin);elseif psinsdef.kfplot==34endendinserrplot([sqrt(xkpk(:,psinsdef.kfplot+1:end-1)),xkpk(