乘法扩展卡尔曼滤波器（MEKF）

乘法扩展卡尔曼滤波器（MEKF）

• 主要参考这部分内容https://www.zhihu.com/people/wang-wei-xin-72-20/posts
• https://zhuanlan.zhihu.com/p/84010704

使用欧拉角设计了能够实现 IMU 姿态估计的卡尔曼滤波器。并且我们发现在俯仰角等于pi/2时，因为欧拉角的万向节锁死现象，滤波器的协方差矩阵的递推公式变得很不准确。在这篇文章中，我们介绍乘法扩展卡尔曼滤波器（Multiplicative Extended Kalman Filter, MEKF），来解决这一问题。

在国内论坛讨论 IMU姿态估计的大量文章中，算法集中在对卡尔曼滤波器的直接应用，及Robert Mahony等人提出的互补滤波器。然而目前在导航领域最通用的算法并不是这两种，而是1982年提出的一种名为“Multiplicative Extended Kalman Filter”，直译是“乘法扩展卡尔曼滤波器”的方法。

旋转矢量和欧拉角都具有奇异性，也就是说在某种姿态下，它们的导数会趋向无穷大；四元数没有奇异性，但它和姿态是2比1的对应关系；只有旋转矩阵既没有奇异性，而且和姿态是一一对应的。

在介绍互补滤波器的时候我们提到，互补滤波器是将两个信号通过 G(s)与1-G(s)两个滤波器，再进行相加，这实际上也是加权平均的过程。对比“互补滤波器”那篇文章中的公式 (6) 和这篇文章中的公式 (6) 可以看到，两者的区别在于卡尔曼滤波器的权重是随时间变化的，而互补滤波器不变。从公式 (2) 和 (4) 可以看到，每进行一次迭代，预测值的准确性是随噪声的积累和测量的到来不断变化的，而卡尔曼滤波器会自动调整权重使估计的结果最优。

在很多关于 IMU 姿态估计的文献中，作者将不使用欧拉角的原因归结于欧拉角在俯仰角计算时会有“万向节锁死”的特性，这种说法是正确的。

综上所述，MEKF 经过 50 年的发展，不仅具有完善的理论，而且经过了最为严格的实验验证，我认为是所有使用 IMU 进行姿态估计算法中的不二之选。但 MEKF 在民用领域推广的过程中名字发生了一些变化，例如在机器人领域，它被称为 error state Kalman filter [5]，和 indirect Kalman filter [6]。这些变化在我们查阅资料时会带来不少额外的困难，可能也是它没有 Mahony 的互补滤波器知名度高的原因。我写这个专栏的主要目的就是介绍 MEKF ，希望这个在航天领域的标准算法能被更多使用 IMU 的朋友们用在自己的工作科研中。

MEKF:

误差状态量扩展卡尔曼滤波

程序中的做法如下：

现在有代码如下：

需要提前导入数据

% % 前提是有角度测量的方式，那么就比较有效
clear all;
close all;


N = 10;
sf = 200;

[qTrue,gyro] = genTrig(N,sf);
qMea = genMea(qTrue);
q0 = mulQua1(qTrue(1,:),expQua([pi,0,0]));

%%
dt = 1/sf;
Nt = length(gyro);

% rotation or vector measurement
if isempty(qMea)
    meaType = 'V';
else
    meaType = 'A';
end

% default parameters
U.P0 = 100;
U.GyroAngleRW = 0.1*pi/180;
U.rvstd = 0.1;
U.vMeaStd = [0.1,0.1];

% U = parseVar(varargin,U);

% Q and R
Q = eye(3)*U.GyroAngleRW^2*dt;
R = eye(3)*U.rvstd^2;%%重要


% pre-allocate memory
qEst = zeros(Nt,4);
P = zeros(3,3,Nt);
if strcmp(meaType,'V')
    qMea = zeros(Nt,4);
end

% initialize
qEst(1,:) = q0;
P(:,:,1) = eye(3)*U.P0^2;

% filter iteration
for nt = 2:Nt
    % integration
    av = 0.5*(gyro(nt-1,:) + gyro(nt,:));%%求中位数
    qEst(nt,:) = mulQua1(qEst(nt-1,:),expQua(av*dt));%%为何不用旋转矩阵的？
    
    % uncertainty propagation
%     第一种
%     F = expRot(av*dt)';
%     第二种
%     S = omegaMatrix(av*dt);
%     normV  = sqrt(S(1,2)^2+S(1,3)^2+S(1,3)^2);
%     F = eye(3)+sin(normV)/normV*S(:,:)+...
%             (1-cos(normV))/normV^2*S(:,:)^2;
%     第三种    
%%和罗德里公式一样？
    rotation_part = av*dt;
    rotation_part = rotation_part';
    %%李代数指数映射的大小
    theta = sqrt(rotation_part'*rotation_part);         % 求的是向量的模
    %%实现的时候先计算出1/theta，然后使用乘法替换下面所有的除法。
    a = rotation_part/theta;            % 这个是什么？
    c = cos(theta);
    s = sin(theta);
    F = c*eye(3)+(1-c)*a*a'+s*omegaMatrix(a);
    
    %%
    P(:,:,nt) = F*P(:,:,nt-1)*F'+Q;
    
    % update
    K = P(:,:,nt)*(P(:,:,nt)+R)^-1;     %%K始终是3维的，这个和协方差的性质相关的
    dv = K * logQua1(mulQua1(invQua1(qEst(nt,:)),qMea(nt,:)))';
    qEst(nt,:) = mulQua1(qEst(nt,:),expQua(dv)');
    P(:,:,nt) = (eye(3)-K)*P(:,:,nt);
end

qEstM = qEst;
figure;
subplot(2,1,1)
plot(qEstM - qTrue);
subplot(2,1,2)
plot(qMea - qTrue);

%% my own (肯定就飘了)
function [ q ] = mulQua1( q1, q2)
% 计算两个单位四元数的乘法
q1 = q1';
q2 = q2';
% multiplicate
q(1,:) = q1(1,:).*q2(1,:)-q1(2,:).*q2(2,:)-q1(3,:).*q2(3,:)-q1(4,:).*q2(4,:);
q(2,:) = q1(1,:).*q2(2,:)+q1(2,:).*q2(1,:)+q1(3,:).*q2(4,:)-q1(4,:).*q2(3,:);
q(3,:) = q1(1,:).*q2(3,:)-q1(2,:).*q2(4,:)+q1(3,:).*q2(1,:)+q1(4,:).*q2(2,:);
q(4,:) = q1(1,:).*q2(4,:)+q1(2,:).*q2(3,:)-q1(3,:).*q2(2,:)+q1(4,:).*q2(1,:);
q = q';
end

function [ v, u, theta ] = logQua1( q )
% 四元数的对数映射
% check size and unitness
q = q';
% calculate log
normQv = sqrt(sum(q(2:4,:).^2));
u = q(2:4,:)./normQv;
theta = wrapToPi(atan2(normQv,q(1,:))*2);

% identity
indi = find(q(1,:)==1);
if ~isempty(indi)
    u(:,indi) = [1;0;0];
    theta(indi) = 0;
end
% format result
v = u.*theta;
v = v';
end

function [ invq ] = invQua1( q, checku )
q = q';
invq = [q(1,:);-q(2:4,:)];
invq = invq';
end



function [ q ] = expQua( v, check0 )
% calculate the exponential map from pure quaternion to unit quaternion
% If v is a 3-by-n or n-by-3 matrix, treat v as n 3-vectors
% If v is a 4-by-n or n-by-4 matrix, treat v as n pure quaternions
% in this case, if check0==true (default), check if v is pure quaternions
% q returns n unit quaternions in the same dimension as v

if ~exist('check0','var') || isempty(check0)
    check0 = true;
end

% check dimensions and pure quaternion
if size(v,1)==3
    tran = false;
    v = v/2;
elseif size(v,2)==3
    v = v'/2;
    tran = true;
elseif size(v,1)==4
    if check0
        if ~isempty(find(v(1,:)~=0,1))
            error('v must be pure quaternions');
        end
    end
    tran = false;
    v = v(2:4,:);
elseif size(v,2)==4
    if check0
        if ~isempty(find(v(:,1)~=0,1))
            error('v must be pure quaternions');
        end
    end
    tran = true;
    v = v(:,2:4)';
else
    error('v must be of size 4-n, n-4 for pure quaternions or 3-n, n-3 for vectors')
end

% calculate exponential map
theta = sqrt(sum(v.^2));
q = [cos(theta);v./theta.*sin(theta)];

if ~isempty(find(theta==0,1))
    q(:,theta==0) = [1;0;0;0];
end

% format result
if tran
    q = q';
end

end

function [omega]=omegaMatrix(data)

% wx=data(1)*pi/180;
% wy=data(2)*pi/180;
% wz=data(3)*pi/180;
wx=data(1);
wy=data(2);
wz=data(3);

omega=[
    0,-wz,wy;
    wz,0,-wx;
    -wy,wx,0
    ];

end