基于误差状态卡尔曼滤波惯性导航理论

作者丨擦擦擦大侠

编辑丨古月居

点击进入—>3D视觉工坊学习交流群

惯性导航备注

1、坐标系之间换算

一般有两个坐标系：大地基准坐标系w系（或者G系）与机器人本体坐标系b系（或者I系），两坐标系之间的旋转矩阵表示为：

坐标系计算的matlab方法：

clearsyms x y z gRx = [1      0      0;   0 cos(x) -sin(x);   0 sin(x) cos(x)];Ry = [cos(y)  0 sin(y);   0       1      0;   -sin(y) 0 cos(y)];Rz = [cos(z) -sin(z) 0;   sin(z) cos(z)  0;   0      0       1];Rwb = Rz*Ry*Rx;Rwb.'*[0;0;g]

SLAM中的坐标系定义：

欧拉角角速度与陀螺仪测量的本体角速度之间（eulerRates与bodyRates）有如下关系：

这个关系常用于欧拉角的运动学更新中。其中，W是可逆矩阵。

2、几种表示姿态的方法

• 四元数表示角度

更新

可以这样表示：

在MEKF算法的差分化之后，有这样的结果：

也可以表示为：

这里面一个很重要的性质就是：

• 四元数运算

四元数相乘可以写成：

其中

• 李代数表示角度

旋转向量V与李群之间的转换，需要经过中间量“李代数”。

也可以用罗德里格斯旋转公式直接转换：

• 链式求导方法

这个方法好像在eskf中用到的比较多，

惯性导航算法理论

主要讨论基于“误差状态”方法的kalman滤波设计方法。该算法为“ESKF”算法，主要参考文献1和github上的一些代码。

1.科式加速度

科式加速度的表示

参考网页：

https:///2016/11/20/imu_model_eq/

科式加速度主要用来描述参考系与惯性系之间的关系

我们来逐项分析上面这个式子。第一项中 αα 为 {B} 的角加速度，所以第一项的物理意义是 {B} 旋转所造成的 P 的切向加速度。

第二项是 {B} 旋转所造成的向心加速度。第四项为 P 相对于 {B} 的加速度，但在惯性系 {A} 下表达——类似于 vrvr，定义相对加速度 arar。

第三项比较特殊，为 {B} 的旋转运动与 P 相对 {B} 的平移运动耦合产生的加速度，称为「科氏加速度」。

可以看到，除了第四项外，另外三项都和 {B} 的旋转有关。

什么时候能够用到科式加速度呢？

在MSCKF1中，考虑的是参考系（b），因此需要考虑；但在MSCKF2中，考虑的是惯性系（a），所以就不需要考虑科式加速度了。

这里存在一个问题：既然不考虑科式加速度又简便也符合逻辑，那么为什么一些文献中采用考虑科式加速度的做法呢？这个问题保留。

2.误差状态更新方法

惯性导航算法常常用误差状态量来表示。实际上IMU组件给系统系统的运动学方程，在状态估计中可以认为是一种约束。

同时测量方程也可以认为是另外一种约束。在优化系统中，将这两种约束都写入到优化函数中，并且保证惩罚函数是线性的，这样就可以利用梯度下降法简单求解了。

但是因为方法2中需要知道状态量的连续导数关系（随时间变化的关系），很多系统都没办法求解这部分（在PVQ系统中可以做）。方法1中的离散形式比较符合对EKF的解释和EKF中协方差更新的方法，因此更加常用。

总结来说，如果系统是连续方程，那么需要先进行error state化之后再进行离散化。如果系统是离散方程，那么直接进行error state化即可。

下面是详细定义：

• 第一种方法：

• 第二种方法：

在上面的两种方法中，第一种是符合EKF的状态方程的递推关系的；第二种是目前VIO算法中的主流做法。应该是殊途同归的两种做法，但是第一种更为通用。

其中，下面的推导是怎么完成的？

最符合逻辑的就是：两个小量相乘，最后的结果就是无穷小，可以忽略。

与ESKF的区别是什么？下面给出ESKF的定义。

实际上是一样的，不过在ESKF中多了Ran量。

3.卡尔曼滤波算法部分

• 确定方差矩阵

算法部分常用卡尔曼滤波，kalman滤波中需要确定几个参量：过程协方差矩阵Q、测量协方差矩阵R和不确定方差矩阵P。

这里的问题是：这个部分到底是不是为了确定Q的大小？

于是我们得到随机游走模型的完整表达。实际上，观察离散模型的表达式，可以发现它生动阐释了「随机游走」的含义：每一时刻都是上一个采样时刻加上一个高斯白噪声得到的，犹如一个游走的粒子，踏出的下一步永远是随机的。

在我们前面给出的 IMU 的运动模型中，bias 就设定为服从随机游走模型2.

在之前的论文里是这么做的，可见与上面的并不一致。

为什么呢不一致呢？参考博客的内容，推断的计算方法如下：但是这里是白噪声还是随机游走？理论上应该是白噪声，因为没有讨论随机游走。

博客：https://zhuanlan.zhihu.com/p/71202672

在博客和文献1，提到一种ESKF的姿态角度估计方法，对于Q的求解，给出下面的结果：

这里描述的方法也不一致，这就需要了解什么是连续的标准差和离散的标准差了。暂且以文献1为标准。

• 如何确定协方差矩阵的更新？

要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量之间的线性递推关系。协方差矩阵可以这样推导：这个方差的积累公式需要注意一下，实际上状态估计大多都是这么做的。

其中，Σn是测量噪声的协方差矩阵，方差从 i 时刻开始进行递推Σ i i = 0

其中F是非线性系统函数对状态量的雅可比矩阵，G代表对控制量的雅可比？

问题是：噪声现在是不是就是控制量？噪声可以做控制量，但是如果有明确含义的控制量，比如说：里程计的左右轮，那么左右轮的误差就是sigma_n。

但是在传统的卡尔曼滤波器的递推公式中，有下面的结果：

在这里更新的过程并没有考虑到控制量u的雅可比矩阵，这是为什么呢？因为u的雅可比矩阵只影响到P矩阵的更新，而并不影响标称值的更新。（标称值的更新依赖于更加准确的非线性状态更新方程）。

这里Q代表的过程误差方差矩阵，对应的维度是状态量。利用控制量与运动学方程，确实可以求出来状态量的误差方差矩阵。

相当于状态量的方差有一部分是由控制量决定的。但是如果系统中没有控制量，只有状态量，那么就需要直接给出Q矩阵的值。实际上这种系统也是比较多的。

在一个开源项目中是这样确定Q的大小的：

sGPS     = 0.5*8.8*dt**2  # assume 8.8m/s2 as maximum acceleration, forcing the vehiclesCourse  = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehiclesVelocity= 8.8*dt # assume 8.8m/s2 as maximum acceleration, forcing the vehiclesYaw     = 1.0*dt # assume 1.0rad/s2 as the maximum turn rate acceleration for the vehicle
Q = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2, sYaw**2])

可以看到这里的Q是用来描述连续微分运动学方程中的微分量的。因为这里的Q只是在定义误差量的过程噪声，所以是比较合理的。

• 如何确定控制量u和状态量x？

最终在MEKF的博客中找到答案

https://zhuanlan.zhihu.com/p/71202815

在文献1中找到下面的佐证（附录部分）。下面的差分化是利用了第二种状态方程的递推方法，然后进行离散化。

由此便得知协方差Q矩阵的确定过程。实际上在实验中，并没有特别严格的定义，实际上两种都可以。

• 为什么要用误差状态量去表示呢？

这部分参考Quaternion kinematics for the error-state Kalman filte和博客

https://zhuanlan.zhihu.com/p/88756311

定向误差状态是最小的，避免了与过度参数化相关的问题，以及相关协方差矩阵奇异性的风险，这通常是由强制约束引起的

误差状态系统总是在接近原始系统的情况下运行，因此远离可能的参数奇点、万向锁问题等，从而保证线性化的有效性始终保持不变

错误状态总是很小，这意味着所有二阶部分都可以忽略不计。这使得雅可比矩阵的计算变得非常简单和快速。有些雅可比数甚至可能是常数或等于可用状态量。

误差动力学是缓慢的，因为所有的大信号动力学都已集成到标称状态。这意味着我们可以以低于预测的速度应用KF修正

什么是动力学是缓慢的？变化是缓慢的？是不是用来描述振动变化，有着更为出色的性能？如果是的话，这也是一个可以发论文的点。是不是和李代数去表示是有关系的？只有在小量的时候，在切空间的性质才可以成立。

• ESKF完整内容

ESKF就是用了上述“基于误差随时间变化”求解方法，首先写成error state，然后进行离散化得到IMU的递推方程。

参数表如下：

三种状态之间的转换关系：

首先是运动学方程中：

给出下面：

其中 𝜃𝑖 𝑤𝑖 为高斯随机脉冲噪声，均值为0，协方差为 𝑤𝑛 ，𝑤𝑏𝑛 在 𝛥𝑡时间内的积分值。

因此给出误差传播方程：

其次是测量方程：

测量方程求取雅可比矩阵采用链式法则。

此外有：

总结来说，求取差分的偏导数如下，需要注意的是，我用到的是左栏的结果，右栏中未作考虑。

4.kalman滤波实例

5.实现代码

参考项目：

https://github.com/cacacadaxia/ESKF-Attitude-Estimation

%  功能：1.实现ESKF算法，加深对于状态估计的理解%        2.其中的问题：%     1） 测量加上地磁计%     2） 注意误差量与标称量%     3） 四元数转角度时有误差，就是这个导致了误差量%%%%--------------------------------------------------------------------------clear all;close all;addpath('../../ESKF-Attitude-Estimation-master')addpath('../utils')% -------------------- import data-------------------fileName = '../NAV_1';Data = importdata(sprintf('%s.mat',fileName));lengthtp = size(Data,1);
time    = zeros(lengthtp,1);roll    = zeros(lengthtp,1);pitch   = zeros(lengthtp,1);yaw     = zeros(lengthtp,1);imuNominalStates = cell(1,lengthtp);imuErrorStates   = cell(1,lengthtp);measurements = cell(1,lengthtp);%groundTruthfor state_k = 1:lengthtp   measurements{state_k}.dt    = 0.02;                      % sampling times 50Hz   measurements{state_k}.omega = Data(state_k,27:29)';               measurements{state_k}.acc   = Data(state_k,9:11)';   measurements{state_k}.mag   = Data(state_k,15:17)';   time(state_k)=state_k*0.02;endrad2deg = 180/pi;rollRef   = Data(:,30)*rad2deg;pitchRef  = Data(:,31)*rad2deg;yawRef    = Data(:,32)*rad2deg;% --------------------Data analysis------------------% ++++++++++++++++++++1.initialization++++++++++++++++dt = measurements{1}.dt;
% 怎么处理初始化的theta？omega_b = zeros(3,1);%%这个用到theta = zeros(3,1);%%这个用不到
% error state initializationdt_theta = zeros(3,1);dt_omega_b = zeros(3,1);
% Keep updated statuserr_state = [dt_theta;dt_omega_b];quat = zeros(4,1);
% --------Refer to previous practice for initialization-----------------------------------init_angle = [Data(1,30),Data(1,31),Data(1,32)]';init_quat = oula2q(init_angle);quat = init_quat';% -------------------------2.covariance matrix ---------------------p1 = 1e-5;p2 =  1e-7;P = blkdiag(p1,p1,p1,p2,p2,p2);%%初始化
sigma_wn = 1e-5;sigma_wbn = 1e-9;Theta_i = sigma_wn*dt^2*eye(3);Omega_i = sigma_wbn*dt^2*eye(3);Fi = eye(6);Qi = blkdiag(Theta_i , Omega_i);Q = Fi*Qi*Fi';
sigma_acc = 1e-3;sigma_mn = 1e-4;R = blkdiag(eye(3)*sigma_acc,eye(3)*sigma_mn);for index = 1:lengthtp-1   % --------------------------forecast------------   omega_m = (measurements{index+1}.omega + measurements{index}.omega)/2;   av = (omega_m - omega_b)*dt;    det_q = [1;0.5*av];    quat = quatLeftComp(quat)*det_q;    omega_b = omega_b;    % 计算标称值   F1 = Exp_lee((measurements{index+1}.omega - omega_b)*dt);   F1 = F1';   Fx = [F1  , -eye(3)*dt;   zeros(3)  , eye(3)];    P_ = Fx*P*Fx' + Q;    % -----------------------observation---------------------    % Prediction results and observations    [H,detZ] = calH(quat,  measurements{index+1});    % --------------------update-----------------    K = P_*H'*inv(H*P_*H' + R)/2;    err_state = K*detZ;    P = P_ - K*(H*P_*H' + R)*K';    % ----------------------update state----------------------    % 参考之前的函数，dt_theta-->quat, quat的左乘方法        dt_theta = err_state(1:3);    dt_omega_b = err_state(4:6);    dt_q = buildUpdateQuat(dt_theta);    quat = quatLeftComp(quat)*dt_q;    quat = quat/norm(quat);    omega_b = omega_b + dt_omega_b;        % ------save angle-----------------------------    [a1,a2,a3] = quattoeuler(quat);    oula(index+1,:) = [a1,a2,a3]/180*pi;    dt_theta_save(index+1,:) = err_state';    % ----------------------------reset-------------------    err_state = zeros(6,1);    G = blkdiag(eye(3) - omegaMatrix(dt_theta/2) ,eye(3));    P = G*P*G';end
% figure;% subplot(3,1,1)% plot(pitchRef);% hold on;plot(oula(:,2)/pi*180);% subplot(3,1,2)% plot(rollRef);% hold on;plot(oula(:,1)/pi*180);% subplot(3,1,3)% plot(yawRef);% hold on;plot(oula(:,3)/pi*180);% legend 1 2
rotLim = [-5 5];figure;subplot(3,1,1)plot(oula(:,1)/pi*180 - rollRef);subplot(3,1,2)plot(oula(:,2)/pi*180 - pitchRef);subplot(3,1,3)plot(oula(:,3)/pi*180 - yawRef);legend 1 2 % ylim(rotLim)


function R = q2R(q)%四元数转旋转矩阵R=[ 2*q(1).^2-1+2*q(2)^2    2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));   2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2     2*(q(3)*q(4)-q(1)*q(2));   2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];R2 = R;end
function Q_dt_theta = cal_Q_dt_theta(quat)Q_dt_theta = 0.5* [-quat(2)  -quat(3)   -quat(4); ...               quat(1)  -quat(4)    quat(3); ...               quat(4)   quat(1)   -quat(2); ...              -quat(3)   quat(2)    quat(1)]; end
function F = Exp_lee(in)S = omegaMatrix(in);   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;endfunction [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   ];endfunction q = R2q(R)%旋转矩阵转四元数t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];Q1 = q;end
function  q = oula2q(in)x = in(1);y = in(2);z = in(3);%欧拉角转四元数q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...   sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...   cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...   cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
endfunction Ang3 = q2oula(q)%四元数转欧拉角x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));y = asin(2*(q(1)*q(3) - q(2)*q(4)));z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));Ang3 = [x y z];end
function updateQuat = buildUpdateQuat(deltaTheta)   deltaq = 0.5 * deltaTheta;   updateQuat = [1; deltaq];   updateQuat = updateQuat / norm(updateQuat);end
function qLC = quatLeftComp(quat)   vector = quat(2:4);   scalar = quat(1);     qLC = [  scalar ,  -vector';            vector , scalar*eye(3) + crossMat(vector)  ];end
function [H,detZ] = calH(q,measurements_k)   % Normalise magnetometer measurement   if(norm(measurements_k.mag) == 0), return; end  %   measurements_k.mag = measurements_k.mag / norm(measurements_k.mag);  % normalise magnitude,very important!!!!   % Normalise accelerometer measurement   if(norm(measurements_k.acc) == 0), return; end  % handle NaN   measurements_k.acc  = measurements_k.acc / norm(measurements_k.acc);  % normalise accelerometer ,very important!!!!   % Reference direction of Earth's magnetic feild   h = quaternProd(q, quaternProd([0; measurements_k.mag], quatInv(q)));   b = [0 norm([h(2) h(3)]) 0 h(4)];   Ha = [2*q(3),                   -2*q(4),                    2*q(1),                         -2*q(2)        -2*q(2),                   -2*q(1),                   -2*q(4),                         -2*q(3)         0,                         4*q(2),                    4*q(3),                         0];   Hm = [-2*b(4)*q(3),                2*b(4)*q(4),               -4*b(2)*q(3)-2*b(4)*q(1),       -4*b(2)*q(4)+2*b(4)*q(2)         -2*b(2)*q(4)+2*b(4)*q(2),     2*b(2)*q(3)+2*b(4)*q(1),    2*b(2)*q(2)+2*b(4)*q(4),       -2*b(2)*q(1)+2*b(4)*q(3)          2*b(2)*q(3),                2*b(2)*q(4)-4*b(4)*q(2),     2*b(2)*q(1)-4*b(4)*q(3),        2*b(2)*q(2)];   Hx = [Ha, zeros(3,3)         Hm, zeros(3,3)];%     Hx = [Ha, zeros(3,3)];   Q_detTheta  = [-q(2),    -q(3),      -q(4)                   q(1),    -q(4),       q(3)                   q(4),     q(1),      -q(2)                  -q(3),     q(2),       q(1)];   Xx = [0.5*Q_detTheta , zeros(4,3)         zeros(3)       , eye(3)];   H = Hx*Xx;     detZ_a = [ 2*(q(2)*q(4)  - q(1)*q(3)) + measurements_k.acc(1)              2*(q(1)*q(2) + q(3)*q(4)) + measurements_k.acc(2)              2*(0.5 - q(2)^2 - q(3)^2) + measurements_k.acc(3)];%     detZ   = detZ_a;   detZ_m =[((2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))) + measurements_k.mag(1))            ((2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))) + measurements_k.mag(2))            ((2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)) + measurements_k.mag(3))];   detZ   = [detZ_a;detZ_m];end

function [roll,pitch,yaw] = quattoeuler(q)rad2deg=180/pi;T=[ 1 - 2 * (q(4) *q(4) + q(3) * q(3))  2 * (q(2) * q(3) +q(1) * q(4))         2 * (q(2) * q(4)-q(1) * q(3));   2 * (q(2) * q(3)-q(1) * q(4))       1 - 2 * (q(4) *q(4) + q(2) * q(2))     2 * (q(3) * q(4)+q(1) * q(2));   2 * (q(2) * q(4) +q(1) * q(3))      2 * (q(3) * q(4)-q(1) * q(2))          1 - 2 * (q(2) *q(2) + q(3) * q(3))];%cnbroll  = atan2(T(2,3),T(3,3))*rad2deg;pitch = asin(-T(1,3))*rad2deg;yaw   = atan2(T(1,2),T(1,1))*rad2deg-8.3;%%这个固定偏差是什么鬼  yaw   = atan2(T(1,2),T(1,1))*rad2deg;%%这个固定偏差是什么鬼  end


参考文献：

Quaternion kinematics for the error-state Kalman Filter.pdf 

https:///2016/11/20/imu_model_eq/ 

本文仅做学术分享，如有侵权，请联系删文。