KF精对准代码及个人理解注释

function [att0, attk, xkpk] = alignvn(imu, qnb, pos, phi0, imuerr, wvn, isfig)
%回来从这往下看，结合代码理解输入参数含义
%[att0v, attkv, xkpkv] = alignvn(imu, avp(1:3), avp(7:9), phi, imuerr1, wvn);
%         原型-实际输入-含义
%         imu - imu - IMU data 模拟的静基座imu数据
%         qnb - avp(1:3) -coarse attitude quaternion
%         粗略姿态四元数？加误差后的初始姿态，哪来的？是理解为初始化后能获得的粗略的qnb吗？
%         这里直接用的真值加初始误差？理解为粗对准后跟这个差不多？

%         pos - avp(7:9) -position
%         带误差的初始位置（实际应用中理解为初始化造成的误差，实际应用中可以通过GPS获得）
%         phi0 - phi -initial misalignment angles estimation
%         失准角（误差状态）初始值，怎么设的？书上好像有个初值选取？
%         imuerr - imuerr1 -IMU error setting 设置的imu数据误差
%         wvn - wvn -velocity measurement noise (3x1 vector)
%         量测的噪声，这个跟状态方程的imu噪声不应该相关吗？
%         isfig - figure flag

% SINS initial align uses Kalman filter with vn as measurement.
% Kalman filter states: 
%    [phiE,phiN,phiU, dvE,dvN,dvU, ebx,eby,ebz, dbx,dby,dbz]'.
%
% Prototype: [att0, attk, xkpk]    = alignvn(imu, qnb, pos, phi0, imuerr, wvn, isfig)
%            [att0v, attkv, xkpkv] = alignvn(imu, avp(1:3), avp(7:9), phi, imuerr1, wvn);


% Outputs: att0 - attitude align result
%         attk, xkpk - for debug
%
% Example:
%	avp0 = [[0;0;0], zeros(3,1), glv.pos0];
%	imuerr = imuerrset(0.03, 100, 0.001, 10);
%	imu = imustatic(avp0, 1, 300, imuerr);
%	phi = [.5; .5; 5]*glv.deg;
%	wvn = [0.01; 0.01; 0.01];
%	[att0, attk, xkpk] = alignvn(imu, avp0(1:3)', avp0(7:9)', phi, imuerr, wvn);
%
% See also  alignfn, aligngps, alignfn9, aligncmps, aligni0, alignWahba, alignsb, alignvndp, etm.

% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/06/2011
global glv
    if nargin<4,  phi0 = [1.5; 1.5; 3]*glv.deg;  end
    if nargin<5,  imuerr = imuerrset(0.01, 100, 0.001, 1);  end
    if nargin<6,  wvn = 0.01;  end;  if length(wvn)==1, wvn=repmat(wvn,3,1); end
    if nargin<7,  isfig = 1; end
    if length(qnb)==3, qnb=a2qua(qnb); end  %if input qnb is Eular angles.
    [nn, ts, nts] = nnts(2, diff(imu(1:2,end)));
    %设置子样数及采样时间间隔
    len = fix(length(imu)/nn)*nn;
    %舍去多余采样数据
    eth = earth(pos);
    %计算当前位置处的的地理参数
    vn = zeros(3,1); 
    %静基座下设置初始vn为0
    Cnn = rv2m(-eth.wnie*nts/2);%姿态更新算法中间项，见式4.1.50右边第一项对应的旋转矩阵
    %两时刻间n系的相对转动
    kf = avnkfinit(nts, pos, phi0, imuerr, wvn);
    %根据输入生成并初始化kf结构体
    [attk, xkpk] = prealloc(fix(len/nn), 7, 2*kf.n+1);
    %预分配内存
    ki = timebar(nn, len, 'Initial align using vn as meas.');
    for k=1:nn:len-nn+1
        %设置为len-nn+1，保证恰好读完最后一个多子样内的数据
        %% 利用增量数据实现姿态和速度更新
        wvm = imu(k:k+nn-1,1:6);
        %读取增量输出形式 多子样imu数据
        t = imu(k+nn-1,end);
        %多子样内最后一个子样的时间
        [phim, dvbm] = cnscl(wvm);
        %计算并补偿圆锥划桨误差等，输出补偿后的用原始增量数据表示的等效旋转矢量phim和b系下的比力速度增量dvbm
        Cnb = q2mat(qnb);
        %将前一步粗对准结果的四元数转换为姿态矩阵形式，这理解的不对
        dvn = Cnn*Cnb*dvbm;
        %修正后的比力速度增量项，就是式4.1.50（大框一模一样，只是每个项中有因为静基座忽略的项，或是没有外推）
        vn = vn + dvn + eth.gn*nts;
        %速度更新，由于是静基座，这里的有害加速度项好像只考虑了重力加速度
        %比力加速度项应该是f=a-g包含了重力影响，后面借助有害加速度项消去影响
        qnb = qupdt2(qnb, phim, eth.wnin*nts);
        %四元数形式的姿态更新吗，qnb初值是avp(1:3)，一个粗略的qnb
        %%
        Cnbts = Cnb*nts;
        %姿态阵*2倍采样时间，后面状态转移矩阵离散用
        
        %状态一步转移矩阵中也有部分需要不断更新：
        kf.Phikk_1(4:6,1:3) = askew(dvn);%直接对离散后一步状态转移矩阵进行赋值
            kf.Phikk_1(1:3,7:9) = -Cnbts;%离散和连续形式相比差一个nts，合理（没严谨推导，可以找一找严谨推导）
            kf.Phikk_1(4:6,10:12) = Cnbts;
            
        kf = kfupdate(kf, vn);
        %卡尔曼滤波更新，静基座下vn真值为0，所以这里输入vn相当于误差量deta vn
        %更新完成后xk是融合新一步观测后的状态估计
        %kf.xk(1:3)是更新后的失准角状态估计，至此出了最优误差（最优状态）
        qnb = qdelphi(qnb, 0.91*kf.xk(1:3)); 
        %由于上面是误差状态KF（间接），完成KF流程后估计出的是最优误差，还需要用惯导解算值减去最优误差
        %为什么用惯导解算值减？因为捷联惯导误差微分方程的误差就是指惯导解算值和真值之间的误差
        %还有个疑惑：为什么没直接减去最优误差，还加了个系数0.91
        kf.xk(1:3) = 0.09*kf.xk(1:3);
        %上一步减误差没减完，剩下0.09的误差，当做下一步误差KF的初值，应该是方便多次迭代至收敛？
        vn = vn-0.91*kf.xk(4:6);  kf.xk(4:6) = 0.09*kf.xk(4:6);
        %kf.xk(4:6)是估计出的最优速度误差，解算速度减最优速度误差得最优速度估计，
        %这里只减了部分最优误差，应该也是方便多次迭代至收敛？
        attk(ki,:) = [q2att(qnb); vn; t]';
        %存储每次更新后KF预测的qnb（四元数转欧拉角形式） vn
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t];
        %存储每次KF后的误差状态量及均方差
        ki = timebar;
        %每次执行ki都+1
    end
    attk(ki:end,:) = []; xkpk(ki:end,:) = [];
    %清空多余行
    att0 = attk(end,1:3)';
    %将迭代最终的结果作为此次精对准结果att0，并输出
    resdisp('Initial align attitudes (arcdeg)', att0/glv.deg);
    if isfig, avnplot(attk, xkpk); end
    %结果作图
    
function kf = avnkfinit(nts, pos, phi0, imuerr, wvn)
%速度量测误差作为状态的KF中，kf结构体初始化，对应理论见psins说明书式10.4.5
    eth = earth(pos);
    %又计算了一遍当地处的地理参数
    wnie = eth.wnie;
    %计算地球自转角速度在当地导航系的投影
    kf = []; 
    %初始化kf结构体为空
    kf.s = 1; 
    %这是啥？
    kf.nts = nts;
    %kf的结构体采样时间间隔
	kf.Qk = diag([imuerr.web; imuerr.wdb; zeros(6,1)])^2*nts;
    %系统状态 的噪声的协方差矩阵（均方误差阵），表示系统状态之间的相关性及每个状态的噪声情况，12维对角阵
    kf.Gammak = 1;
    %好像是噪声分配矩阵？
	kf.Rk = diag(wvn)^2/nts;
    %量测噪声协方差阵   
	kf.Pxk = diag([phi0; [1;1;1]; imuerr.eb; imuerr.db])^2;
    %设置系统状态 初始的状态估计均方误差阵，这里还是迷糊，得回第五章KF推导 式5.3.3那区分理解一下，这是咋用的
	%phi0应该是状态（失准角）的初始误差
    Ft = zeros(12); 
    %Ft是连续形式状态转移矩阵，这里初始化为12维0阵
    Ft(1:3,1:3) = askew(-wnie);
    %askew输出向量对应的反对称矩阵，这里把连续形式状态转移矩阵Ft的设置前3*3
    %其他位置在主函数里进行赋值
    kf.Phikk_1 = eye(12)+Ft*nts;
    %状态一步转移矩阵中不需要更新的常值部分，形式上像离散化的公式
	kf.Hk = [zeros(3),eye(3),zeros(3,6)];
    %量测矩阵，这个形式跟说明书一样，有个疑问：这个哪来的？不需要离散吗？迷糊
%     [kf.m, kf.n] = size(kf.Hk);
%     kf.I = eye(kf.n);
%     kf.xk = zeros(kf.n, 1);
%     kf.adaptive = 0;
%     kf.xconstrain = 0; kf.pconstrain = 0;
%     kf.fading = 1;
    kf = kfinit0(kf, nts);
    %将kf结构体中没有设置的参数初始化，设置为默认参数
    
function avnplot(attk, xkpk)
global glv
    if glv.isfig==0, return; end
    t = attk(:,end);
    myfigure;
	subplot(421); plot(t, attk(:,1:2)/glv.deg); xygo('pr'); title('Xi');
	subplot(423); plot(t, attk(:,3)/glv.deg); xygo('y');
    %画出每步精对准KF的对qnb（欧拉角形式，单位转换为角度）的估算结果
	subplot(425); plot(t, xkpk(:,7:9)/glv.dph); xygo('eb'); 
	subplot(427); plot(t, xkpk(:,10:12)/glv.ug); xygo('db');
    %kf.xk 的eb和db，显示误差状态陀螺和加计的 常值零偏随时间变化
    %对这两个图的结果不是很理解
	subplot(422); plot(t, sqrt(xkpk(:,13:15))/glv.min); xygo('phi'); title('\surdPii');
	subplot(424); plot(t, sqrt(xkpk(:,16:18))); xygo('dV');
	subplot(426); plot(t, sqrt(xkpk(:,19:21))/glv.dph); xygo('eb');
 	subplot(428); plot(t, sqrt(xkpk(:,22:24))/glv.ug); xygo('db');   
    %画出状态量均方误差阵对角线元素（即所有误差状态的标准差）随时间的变化
    %好像是不管哪种随机过程，某个量的标准差（或者方差）都代表了这个量的集中分布程度
    %也就是可信程度，这几个误差的均方差最后都近似收敛到0
    %对此理解是：每次KF更新都把最优估计结果传递给下次（可信度最高，均方误差最小的结果，最优估计结果）
    %最后多轮KF下来，最后输出的结果则是越来越可信的最优结果