惯性传感器测量误差模型
参考教材： 捷联惯导算法与组合导航原理-严恭敏、翁浚
insupdate函数里关于补偿的部分：
[phim, dvbm] = cnscl(imu,0); % coning & sculling compensation
phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts; % calibration
ins.wib = phim/nts; ins.fb = dvbm/nts; % same as trjsimu
coning & sculling compensation为旋转运动和划桨运动误差补偿，现仅了解。
得到的phim和dvbm为角速度和速度增量，所以eb（gyro漂移）和db（acc零偏）这里乘了时间，在后面代码中角速度和比力又除了时间。
calibration为对准误差补偿，关于惯性器件的误差。
主要来自于四个部分：
- 失准角误差
μ，因为惯性器件的坐标轴为非直角坐标系{bg}，要使角速度和加速度在同一坐标系需要求坐标变换矩阵，详见教材附录D。其中中转的另一个坐标系{B}与载体坐标系{b}的等效旋转矢量即为失准角。 - 不正交误差
φ，非直角坐标系到中转的直角坐标系的偏差角，可以反映{bg}坐标系的不正交程度，越小正交性越好。 - 刻度系数误差
k，相当于直尺膨胀、紧缩带来的误差。与前三项包含在标定刻度误差矩阵Kg、Ka中。下图为陀螺仪误差模型。 - 常值漂移（零漂）
ε，上下标代表在某个坐标系中的投影。
整理得到，加速度计同理：
kf相关函数的学习
kf = kfinit(ins, davp0, imuerr, rk);
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2; kf.pconstrain=1;
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
kfinit初始化
模型建立：
function kf = kfinit(ins, varargin)
global glv psinsdef
[Re,deg,dph,ug,mg] = ... % just for short
setvals(glv.Re,glv.deg,glv.dph,glv.ug,glv.mg);
o33 = zeros(3); I33 = eye(3);
kf = [];
if isstruct(ins), nts = ins.nts;
else nts = ins;
end
定义全局变量glv、psinsdef，后者包含卡尔曼滤波的状态、测量维数类型。
switch(psinsdef.kfinit)
case psinsdef.kfinit153,
psinsdef.kffk = 15; psinsdef.kfhk = 153; psinsdef.kfplot = 15;
[davp, imuerr, rk] = setvals(varargin);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;
kf.Rk = diag(rk)^2;
kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;
kf.Hk = kfhk(0);
SINS_GPS_153.m中psinstypedef(153)；定义了psinsdef.kfinit的值为153，此时switch语句根据该值执行相应的语句。根据输入变量定义对应大小的davp，immerr，rk。
- 过程噪声矩阵
Qt与gyro和acc的随机游走白噪声web，wdb相关，且对准的是状态的前六项（失准角偏差和速度偏差）。 - 测量噪声矩阵
Rk与输入的gps位置误差rk有关。 - 协方差矩阵Pxk是【avp的误差】，【gyro和acc的漂移】的平方。
- 最后定义Hk使用函数kfhk。
function Hk = kfhk(ins, varargin)
global psinsdef
switch(psinsdef.kfhk)
case 153,
Hk = [zeros(3,6), eye(3), zeros(3,6)];
至此kf结构体如下：
kf = kfinit0(kf, nts);
定义好上面四个矩阵后，最后还有一个非常重要的kfinit0函数。
function kf = kfinit0(kf, nts)
% Always called by kfinit and initialize the remaining fields of kf.
% See also kfinit, kfupdate, kffeedback, psinstypedef.
kf.nts = nts;
[kf.m, kf.n] = size(kf.Hk);
kf.I = eye(kf.n);
kf.Kk = zeros(kf.n, kf.m);
kf.measmask = []; % measurement mask for no update 20/11/2022
kf.measstop = zeros(kf.m,1); % measurement stop time
kf.measlost = zeros(kf.m,1); % measurement lost time
kf.measlog = 0; % measurement log flag
上面代码段记录了Hk矩阵的大小，定义增益矩阵Kk以及一系列的测量相关的空矩阵和变量。下面代码段定义了kf一系列cell，现在用上的是xk，Qk以及pconstrain。
if ~isfield(kf, 'xk'), kf.xk = zeros(kf.n, 1); end
if ~isfield(kf, 'Qk'), kf.Qk = kf.Qt*kf.nts; end
if ~isfield(kf, 'Gammak'), kf.Gammak = 1; kf.l = kf.n; else, kf.l=size(kf.Gammak,2); end
if ~isfield(kf, 'fading'), kf.fading = 1; end
if ~isfield(kf, 'adaptive'), kf.adaptive = 0; end
% if kf.adaptive==1
if ~isfield(kf, 'b'), kf.b = 0.5; end
if ~isfield(kf, 'beta'), kf.beta = 1; end
if ~isfield(kf, 'Rmin'), kf.Rmin = 0.01*kf.Rk; end
if ~isfield(kf, 'Rmax'), kf.Rmax = 100*kf.Rk; end
if ~isfield(kf, 'Qmin'), kf.Qmin = 0.01*kf.Qk; end
if ~isfield(kf, 'Qmax'), kf.Qmax = 100*kf.Qk; end
% end
if ~isfield(kf, 'xtau'), kf.xtau = ones(size(kf.xk))*eps; end
if ~isfield(kf, 'T_fb'), kf.T_fb = 1; end
if ~isfield(kf, 'fbstr'), kf.fbstr = 'avped'; end
if ~isfield(kf, 'xconstrain'), kf.xconstrain = 0; end
if ~isfield(kf, 'pconstrain'), kf.pconstrain = 0; end
kf.Pmax = (diag(kf.Pxk)+1)*1.0e10;
kf.Pmin = kf.Pmax*0;
% kf.Pykk_1 = kf.Hk*kf.Pxk*kf.Hk'+kf.Rk;
kf.Pykk_1 = kf.Hk*kf.Pxk*kf.Hk'+0;
kf.xfb = zeros(kf.n, 1);
% kf.coef_fb = (1.0-exp(-kf.T_fb./kf.xtau));
% kf.coef_fb = ar1coefs(kf.T_fb, kf.xtau);
xtau = kf.xtau;
xtau(kf.xtau<kf.T_fb) = kf.T_fb; kf.coef_fb = kf.T_fb./xtau; %2015-2-22
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2; kf.pconstrain=1;
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
上面定义kf最小协方差矩阵，并将pconstrain设置为1，可以达到限制协方差矩阵发散的效果吗？
prealloc函数为滤波后的数据提前分配好空间。
kfupdate更新
进入滤波，先将ins的数据更新，得到更新后的ins。kffk函数创造转移矩阵。
转移矩阵
timebar(nn, len, '15-state SINS/GPS Simulation.');
ki = 1;
for k=1:nn:len-nn+1
k1 = k+nn-1;
wvm = imu(k:k1,1:6); t = imu(k1,end);
ins = insupdate(ins, wvm);
kf.Phikk_1 = kffk(ins);
下面为kffk函数，当kffk全局变量为15时，使用etm函数创造误差转移矩阵，可在严老师的教材和博士毕业论文学习。
得到Ft后再离散化，采样间隔大时指数运算离散化，采样间隔小时泰勒展开到一阶。
function [Fk, Ft] = kffk(ins, varargin)
%Create Kalman filter system transition matrix.
global psinsdef
%% get Ft
if isstruct(ins), nts = ins.nts;
else nts = ins;
end
switch(psinsdef.kffk)
case 15,
Ft = etm(ins);
%% discretization
Fk = Ft*nts;
if nts>0.1 % for large time interval, this may be more accurate.
Fk = expm(Fk);
else % Fk = I + Ft*nts + 1/2*(Ft*nts)^2 , 2nd order expension
Fk = eye(size(Ft)) + Fk;% + Fk*Fk*0.5;
end
这里涉及到avp更新方程和误差方程的推导。
function Ft = etm(ins)
%SINS Error Transition Matrix (see my PhD dissertation p39
%% fi dvn dpos eb db
Ft = [ Maa Mav Map -ins.Cnb O33
Mva Mvv Mvp O33 ins.Cnb
O33 Mpv Mpp O33 O33
zeros(6,9) diag(-1./[ins.tauG;ins.tauA]) ];
%[ins.tauG, ins.tauA] = setvals(inf(3,1)); % gyro & acc correlation time
更新函数
注释部分介绍很详细。
TimeMeasBoth - described as follows,
TimeMeasBoth='T' (or nargin==1) for time updating only,
TimeMeasBoth='M' for measurement updating only,
TimeMeasBoth='B' (or nargin==2) for both time and measurement updating.
- (1) If kf.adaptive=1, then use Sage-Husa adaptive method (but only
for measurement noise ‘Rk’)， where minimum constrain ‘Rmin’
and maximum constrain ‘Rmax’ are considered to avoid divergence. - (2) If kf.fading>1, then use fading memory filtering method.
- (3) Using Pmax&Pmin to constrain Pxk, such that Pmin<=diag(Pxk)<=Pmax.
根据输入变量数目调整TimeMeasBoth的值。
if nargin==1;
TimeMeasBoth = 'T';
elseif nargin==2
TimeMeasBoth = 'B';
end
见代码部分，如果仅更新时间（也就是状态向前传播，协方差也向前传播），此处的Gammk设置为1，作用还未知；并记录下测量停止的时间，测量丢失的时间（还未知，后面可能在丢失测量值时能用上或者两者频率不一样时能用上），其初始值都是zeros(3,1)，到此函数结束，步出kfupdate。
if TimeMeasBoth=='T' % Time Updating
kf.xk = kf.Phikk_1*kf.xk;
kf.Pxk = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
kf.measstop = kf.measstop - kf.nts; kf.measlost = kf.measlost + kf.nts;
else
仅测量更新和两者都更新是并列的，TimeMeasBoth=='M' 就是将上一个时刻传播的状态和协方差矩阵更新； TimeMeasBoth=='B'就是状态向前传播，协方差也向前传播，并记录下测量停止的时间，测量丢失的时间。
然后将测量值yk用于更新状态和协方差。并且可以实现自适应卡尔曼滤波器、衰减记忆滤波器；限制xk，Pk的功能防止其发散以达到好的滤波效果。
if TimeMeasBoth=='M' % Meas Updating
kf.xkk_1 = kf.xk;
kf.Pxkk_1 = kf.Pxk;
elseif TimeMeasBoth=='B' % Time & Meas Updating
kf.xkk_1 = kf.Phikk_1*kf.xk;
kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
kf.measstop = kf.measstop - kf.nts; kf.measlost = kf.measlost + kf.nts;
else
error('TimeMeasBoth input error!');
end
kf.Pxykk_1 = kf.Pxkk_1*kf.Hk';
kf.Py0 = kf.Hk*kf.Pxykk_1;
kf.ykk_1 = kf.Hk*kf.xkk_1;
kf.rk = yk-kf.ykk_1;
idxbad = []; % bad measurement index
if kf.adaptive==1 % for adaptive KF, make sure Rk is diag 24/04/2015
for k=1:kf.m
if yk(k)>1e10, idxbad=[idxbad;k]; continue; end % 16/12/2019
ry = kf.rk(k)^2-kf.Py0(k,k);
if ry<kf.Rmin(k,k), ry = kf.Rmin(k,k); end
if ry>kf.Rmax(k,k), kf.Rk(k,k) = kf.Rmax(k,k);
else kf.Rk(k,k) = (1-kf.beta)*kf.Rk(k,k) + kf.beta*ry;
end
end
kf.beta = kf.beta/(kf.beta+kf.b);
end
应用例子：
function kf = kfupdate(kf, yk, TimeMeasBoth)
kf = kfupdate(kf);相当于TimeMeasBoth='T'，仅向前传播xk、Pk，在没有接收到测量值yk之前一直传播。
kf = kfupdate(kf, ins.pos-posGPS, 'M');将GPS与ins的位置误差输入，同时将前面传播过来的xk、Pk赋值，后加入yk进行卡尔曼增益矩阵的计算以及xk、Pk的估计更新。
kf = kfupdate(kf, ins.pos-posGPS, 'B');将两个步骤合并，传播和更新合并在一起，在本例程中测量值和状态的频率不一致所以分开进行两个步骤。
kffeedback反馈
应用：
[kf, ins] = kffeedback(kf, ins, 1, 'avp');
function [kf, ins, xfb] = kffeedback(kf, ins, T_fb, fbstr)
T_fb为反馈时间间隔，fbstr为反馈字符串（反馈变量类型），输出反馈状态xfb。
下面部分代码kf中的反馈时间间隔与输入的不一致时，如何调整的反馈相关系数。
if nargin<4, fbstr=kf.fbstr; end
if nargin<3, T_fb=1; end
if kf.T_fb~=T_fb
kf.T_fb = T_fb;
% kf.coef_fb = (1.0-exp(-kf.T_fb./kf.xtau));
% kf.coef_fb = ar1coefs(kf.T_fb, kf.xtau);
% xtau = kf.xtau;
% xtau(kf.xtau<kf.T_fb) = kf.T_fb; kf.coef_fb = kf.T_fb./xtau; %2015-2-22
% kf.coef_fb(kf.xtau>kf.T_fb) = 1;
idx = kf.T_fb>kf.xtau; % scale<vector
kf.coef_fb(idx) = 1; kf.coef_fb(~idx) = kf.T_fb./kf.xtau(~idx); %2022-6-25
kf.coef_fb(kf.xtau<0.001 & kf.T_fb~=inf) = 1;
end
下面xfb_tmp为相关系数处理后的反馈缓存值，并创造等大小的零矩阵xfb。
根据不同的fbstr，使用卡尔曼滤波和相关系数处理后的xk对ins中的相应变量进行修正并进行反馈记录。
xfb_tmp = kf.coef_fb.*kf.xk; xfb = xfb_tmp*0;
for k=1:length(fbstr)
switch fbstr(k)
case 'a',
idx = 1:3; ins.qnb = qdelphi(ins.qnb, xfb_tmp(idx));
case 'v',
idx = 4:6; ins.vn = ins.vn - xfb_tmp(idx);
case 'p',
idx = 7:9; ins.pos = ins.pos - xfb_tmp(idx);
case 'e',
idx = 10:12; ins.eb = ins.eb + xfb_tmp(idx);
case 'd',
idx = 13:15; ins.db = ins.db + xfb_tmp(idx);
case 'A',
idx = 1:2; ins.qnb = qdelphi(ins.qnb, [xfb_tmp(idx);0]);
case 'V',
idx = 6; ins.vn(3) = ins.vn(3) - xfb_tmp(idx);
case 'P',
idx = 9; ins.pos(3) = ins.pos(3) - xfb_tmp(idx);
case 'E',
idx = 10:11; ins.eb(1:2) = ins.eb(1:2) + xfb_tmp(idx);
case 'D',
idx = 15; ins.db(3) = ins.db(3) + xfb_tmp(idx);
case 'L',
idx = 16:18; ins.lever = ins.lever + xfb_tmp(idx);
case 'T',
idx = 19; ins.tDelay = ins.tDelay + xfb_tmp(idx);
case 'G',
idx = 20:28; dKg = xfb_tmp(idx); dKg = [dKg(1:3),dKg(4:6),dKg(7:9)];
ins.Kg = (eye(3)-dKg)*ins.Kg;
case 'C',
idx = 29:34; dKa = xfb_tmp(idx); dKa = [dKa(1:3),[0;dKa(4:5)],[0;0;dKa(6)]];
ins.Ka = (eye(3)-dKa)*ins.Ka;
case 'H',
idx = [6,9,15]; ins.vn(3) = ins.vn(3) - xfb_tmp(6);
ins.pos(3) = ins.pos(3) - xfb_tmp(9); ins.db(3) = ins.db(3) + xfb_tmp(15);
otherwise,
error('feedback string mismatch in kf_feedback');
end
kf.xk(idx) = kf.xk(idx) - xfb_tmp(idx);
kf.xfb(idx) = kf.xfb(idx) + xfb_tmp(idx); % record total feedback
xfb(idx) = xfb_tmp(idx);
a、v、p、e、d分别使用估计出来的误差对ins中四元数姿态、速度、位置、陀螺漂移、加表零偏进行补偿。
A、V、P分别对姿态的pitch、roll角、z方向的速度、z方向的位置进行补偿。
E、D分别对陀螺漂移的x、y方向角速度、加表零偏的z方向比力
L、T、G、C使用更多维数的状态估计补偿。
H为进行高度（z方向）上的avp补偿。
误差方程的推导
以下均参考严老师教材第四章。
姿态
误差产生原因：
误差方程推导（在大多数情况下，比如在惯导系统标定比较准确的时候或者运载体机动不大时，可以忽略标度系数矩阵误差δKG、δKA的影响，过程噪声矩阵Q(t)对角线前三个元素为web陀螺角度随机游走/角速率白噪声：
列状态转移矩阵：
速度
误差方程来自于更新方程，原因和姿态误差处类似。
列状态转移矩阵，过程噪声矩阵Q(t)对角线第4-6个元素为wdb加计速度随机游走/比力白噪声：
位置
惯性器件模型误差：
状态转移矩阵Ft，在函数etm中，自己建模型时重新写一个函数即可：
%% fi dvn dpos eb db
Ft = [ Maa Mav Map -ins.Cnb O33
Mva Mvv Mvp O33 ins.Cnb
O33 Mpv Mpp O33 O33
zeros(6,9) diag(-1./[ins.tauG;ins.tauA]) ];
%[ins.tauG, ins.tauA] = setvals(inf(3,1)); % gyro & acc correlation time