基于误差状态卡尔曼滤波惯性导航理论
惯性导航备注
1、坐标系之间换算
一般有两个坐标系：大地基准坐标系w系（或者G系）与机器人本体坐标系b系（或者I系），两坐标系之间的旋转矩阵表示为：
坐标系计算的matlab方法：
clear
syms x y z g
Rx = [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://fzheng.me/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 vehicle
sCourse = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehicle
sVelocity= 8.8*dt # assume 8.8m/s2 as maximum acceleration, forcing the vehicle
sYaw = 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);
%groundTruth
for 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;
end
rad2deg = 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 initialization
dt_theta = zeros(3,1);
dt_omega_b = zeros(3,1);
% Keep updated status
err_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;
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
function 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)];
end
function 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))];%cnb
roll = 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