Kalman滤波在重力匹配中的应用

1. Kalma滤波学习笔记

卡尔曼滤波包括预测和量测更新两个部分。

以汽车运动为例分析，其运动模型为:

X_k=F_k-1X_k-1+G_k-1U_k-1+w_k-1

U表示输入的控制量，w表示噪声。

量测模型为：

y_k=H_kX_k+v_k

噪声特性为w_k~N(0,Q_k), v_k~N(0,R_k)。

预测部分：

量测更新部分：

2. 惯性导航系统

在惯性导航系统中，加速度计和陀螺仪分别测量载体的加速度和角速度，通过积分的方法解算得到载体的位置、速度和姿态信息。

考虑姿态误差角小、高度不变的情形，惯性导航系统的位置误差、速度误差和姿态误差微分方程分别为：

3. 扩展卡尔曼滤波

选择状态变量为：

 

系统滤波方程为：

 

其中连续时间的状态转移矩阵F由INS误差微分方程决定，W为系统过程噪声方差阵，考虑由加速度计和陀螺仪的随机误差组成。

选择系统的量测量为INS测量位置对应重力图上的重力异常值与实测重力异常值之差，

 

其中INS下标表示惯性导航系统测量的位置在重力图中对应的重力异常值，G下标表示实际重力仪测得的重力异常值，gg和m下标和分别表示重力图的误差和重力仪的测量误差，V代表系统观测噪声，由重力仪测量随机误差构成。

上式化简得到，

 

式中求偏导分别代表重力异常值在纬度和经度方向上的梯度，H代表系统量测矩阵。其中梯度可通过局部线性化方式求取，对重力图局部区域拟合为经度和纬度的二阶多项式曲面，并求梯度，得到量测矩阵。

将状态方程与量测方程的非线性部分进行离散化：

 

 其中T为卡尔曼滤波周期，Fk和Qk表示第k时刻的状态转移矩阵与系统过程噪声方差矩阵的取值。

（具体推导过程见：https://blog.csdn.net/l2014010671/article/details/91126676?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.nonecase）

得到卡尔曼滤波递推方程如第一节中所述。由于INS测量频率高于重力仪测量频率，因此当有重力仪的量测值时，卡尔曼滤波器进行量测更新；否则，仅进行时间更新。量测更新之后，对INS的位置、速度和姿态进行修正。

部分主程序MATLAB代码如下所示：

%% 惯性导航初始化
% 位置
delta_pos=gv.delta_pos/glv.nm/gv.weight;    % 初始位置误差（单位：弧度）
pos = location0 +delta_pos;%载体的初始位置【纬度；经度；高度】
H_ini = pos(3); %初始高度
pos_1 = pos; %前一时刻位置
pos_2 = pos; %前两个时刻位置
% 速度
vn = velocity0+gv.delta_velocity*ones(3,1);
vn_1 = vn;  %前一时刻速度
vn_2 = vn;  %前两个时刻速度
dRnm_1 = zeros(3,1); %前一时刻位置增量
dRnm_2 = zeros(3,1); %前两个时刻位置增量
% 姿态
att0=(angle0/glv.deg)';         % 初始姿态（单位：度）
delta_att=gv.delta_att;       % 初始姿态误差
att0=att0+delta_att';
qnb = AttToQuat(att0*pi/180);   %姿态矩阵
%% 滤波器初值    
% 状态变量为： 位置2个，速度2个，姿态3个，加速度计2个，陀螺3个,共12个
dpos0=delta_pos(1:2)';    % 位置初始误差
dvn0=gv.delta_velocity*[1;1];     % 速度误差初值，单位为m/s
datt0=gv.delta_att'*glv.deg;   % 姿态误差初值,单位为弧度
%IMU误差
db=gv.Fe_chang*ones(2,1)*glv.ug;    % 加速度计误差初值，单位为ug
eb=gv.We_chang*ones(3,1)*glv.dph;   % 陀螺误差初值
wdb=gv.Fe_randn*ones(2,1)*glv.ug;    % 加速度计随机误差，单位为ug
web=gv.We_randn*ones(3,1)*glv.dph;   % 陀螺随机误差
%滤波器初值
Pk=diag([dpos0';dvn0;datt0';db;eb])^2;    % 初始误差阵
Qt =diag([zeros(2,1);wdb;web;zeros(5,1)])^2;    % 系统状态误差方差阵
Xk =[dpos0,dvn0',datt0,db',eb']';   %状态变量初值，列
Rk=gv.g_noise^2;     % 系统观测误差方差阵（由重力仪测量随机误差构成）
Xkf=zeros(L/4,nn);
Hk=zeros(1,12);
%% -----------导航计算--------------------
hwait = waitbar(0,'Please wait...'); %进度条
kk=1;  % 解算标号
kkk=1;
km=1;
for k=1:n:L-n+1
    k1=k+n-1;
    fmm=fm(k:k1,:)';         %四子样数据,比力
    wmm=wm(k:k1,:)';       %角速度
    [qnb, vn, pos, dRnm] = sinsQuat(qnb, vn_1, vn_2, pos_1, pos_2, dRnm_1, dRnm_2, wmm, fmm, Ts); %导航程序
    Ft=get_F12(vn, pos, mean(fmm,2));   %获得INS状态转移矩阵
    [Phikk_1, Qk] =kfdis(Ft, Qt, T_Kalman, 12);      %   离散化得到一步转移矩阵和系统噪声矩阵
    if mod(kk,interval)==0   % 得到观测值
        % 观测模型
        [Hk(1:2),m,s]=get_H(3,pos(1),pos(2));                % 拟合范围,纬度,经度,得到量测矩阵H
        Hk(3:12)=zeros(1,10);    H_s(kkk)=s;   H_m(kkk)=m;   kkk=kkk+1;
        % 观测值
        pnum=find_num2(pos(1:2)*gv.weight);
        g_ins=z2(pnum(2),pnum(1));
        point=[Storedata.avp(k1,7) Storedata.avp(k1,8)]*gv.weight;
        pnum=find_num2(point);
        g_true=z2(pnum(2),pnum(1))+gv.g_noise*rand(1,1);
        Zk=g_ins-g_true;         %  背景图重力异常值减去测量值
        % Kalman
        [Xk, Pk] = kalman(Phikk_1, Qk, Xk, Pk, Hk, Rk, Zk);   %有测量，更新
        % -----------------------------闭环修正----------------------------
        if nn>1
            pos(1:2)=pos(1:2)-Xk(1:2);    %  位置反馈
        end
        if nn>3
            vn(1:2)=vn(1:2)-Xk(3:4);   %  速度反馈
        end
        if nn>4
            Cnb=(eye(3)+askew(Xk(5:7)))*QuatToAttMat(qnb); % 修正姿态矩阵
            Cnb=Cnb*(Cnb'*Cnb)^(-1/2);
            qnb = AttmatToQuat(Cnb);
        end
        Xk(1:nn)=zeros(nn,1);
    else
        [Xk, Pk] = kalman(Phikk_1, Qk, Xk, Pk);   %状态更新
    end
    %-----------------------   记录数据   ----------------------
    Xkf(kk,:)=Xk(1:nn);
    INS_data_k(kk,1)=pos(1);
    INS_data_k(kk,2)=pos(2);
    INS_data_k(kk,3)=vn(1);
    INS_data_k(kk,4)=vn(2);
    Att = QuatToAtt(qnb);     % 姿态输出
    INS_data_k(kk,5:7)= Att';
    kk=kk+1;
    vn(3) = 0;                     %  天向抑制
    pos(3) = H_ini;                %  抑制高度通道
    vn_2 = vn_1;
    vn_1 = vn;
    pos_2 = pos_1;
    pos_1 = pos;
    dRnm_2 = dRnm_1;
    dRnm_1 = dRnm;
    %-----------------------   记录数据   ---------------------------------
    waitbar(k/L);
end
close(hwait)

离散化卡尔曼滤波：

function [Fikk_1, Qk] = kfdis(Ft, Qt, Tkf, n)
Tkfi = Tkf;
facti = 1;
Fti = Ft;
Mi = Qt;
In = eye(size(Ft,1));
Fikk_1 = In + Tkf*Ft;
Qk = Qt*Tkf;
for i=2:1:n
    Tkfi = Tkfi*Tkf;
    facti = facti*i;
    Fti = Fti*Ft;
    Fikk_1 = Fikk_1 + Tkfi/facti*Fti;
    
    FtMi = Ft*Mi;
    Mi = FtMi + FtMi';
    Qk = Qk + Tkfi/facti*Mi;
end

卡尔曼滤波：

function [Xk, Pk] = kalman(Phikk_1, Qk, Xk_1, Pk_1, Hk, Rk, Zk)
if nargin<7     % 仅进行状态递推
    Xk = Phikk_1*Xk_1;
    Pk = Phikk_1*Pk_1*Phikk_1'+Qk;
else            % 有测量时滤波
    Xkk_1=Phikk_1*Xk_1;
    Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
    Pxz = Pkk_1*Hk';
    Pzz = Hk*Pxz + Rk;
    Kk = Pxz*Pzz^-1;
    Xk = Xkk_1 + Kk*(Zk-Hk*Xkk_1);
    Pk = Pkk_1-Kk*Hk*Pkk_1;
end
end

获得量测矩阵（重力在经纬度方向上的梯度）：

function [h,m,s]=get_H(n,fai,lamda)
h=zeros(1,2);
[h(1),h(2),m,s]=get_h1_h4(n,fai,lamda);
end

function [h1,h4,m,s]=get_h1_h4(n,fai,lamda)
load x2;
load y2;
load z2;
gvs;
map_gvs;
kk=1;         % 一维
k=1;          % 二维
x=zeros(2*n+1,2*n+1);
y=zeros(2*n+1,2*n+1);
xxxx=zeros(n*n,1);
yyyy=zeros(n*n,1);
zzzz=zeros(n*n,1);
point=[fai*gv.weight lamda*gv.weight];
num=find_num2(point);
for i=-n:n
    for j=-n:n
        if num(1)+i<1 || num(2)+j<1 || num(1)+i>length(x2) || num(2)+j>length(x2)
        else
            xxxx(kk)=x2(1,num(1)+i)/gv.weight;
            yyyy(kk)=y2(num(2)+j,1)/gv.weight;
            zzzz(kk)=z2(num(2)+j,num(1)+i);
            kk=kk+1;
        end
    end
end
xxx=xxxx(1:kk-1);    %拟合区域所有点的位置和重力值
yyy=yyyy(1:kk-1);
zzz=zzzz(1:kk-1);
f=fit([xxx,yyy],zzz,'poly22');     %曲面线性拟合
x=x2(num(1)-n:num(1)+n,num(1)-n:num(1)+n)/gv.weight;
y=y2(num(2)-n:num(2)+n,num(2)-n:num(2)+n)/gv.weight;
z=z2(num(2)-n:num(2)+n,num(1)-n:num(1)+n);
[h1,h4]=differentiate(f,fai,lamda);   %求梯度
D=z-(h1*x+h4*y);
D=D(:);
m=mean(D);    %拟合区域的误差
s=std(D);
end