慣性導(dǎo)航備注
1、坐標(biāo)系之間換算
一般有兩個坐標(biāo)系：大地基準(zhǔn)坐標(biāo)系w系（或者G系）與機(jī)器人本體坐標(biāo)系b系（或者I系），兩坐標(biāo)系之間的旋轉(zhuǎn)矩陣表示為：
坐標(biāo)系計算的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中的坐標(biāo)系定義：
歐拉角角速度與陀螺儀測量的本體角速度之間（eulerRates與bodyRates）有如下關(guān)系：
這個關(guān)系常用于歐拉角的運(yùn)動學(xué)更新中。其中，W是可逆矩陣。
2、幾種表示姿態(tài)的方法
四元數(shù)表示角度
更新
可以這樣表示：
在MEKF算法的差分化之后，有這樣的結(jié)果：
也可以表示為：
這里面一個很重要的性質(zhì)就是：
四元數(shù)運(yùn)算
四元數(shù)相乘可以寫成：
其中
李代數(shù)表示角度
旋轉(zhuǎn)向量V與李群之間的轉(zhuǎn)換，需要經(jīng)過中間量“李代數(shù)”。
也可以用羅德里格斯旋轉(zhuǎn)公式直接轉(zhuǎn)換：
鏈?zhǔn)角髮?dǎo)方法
這個方法好像在eskf中用到的比較多，
慣性導(dǎo)航算法理論
主要討論基于“誤差狀態(tài)”方法的kalman濾波設(shè)計方法。該算法為“ESKF”算法，主要參考文獻(xiàn)1和github上的一些代碼。
1.科式加速度
科式加速度的表示
參考網(wǎng)頁：
https://fzheng.me/2016/11/20/imu_model_eq/
科式加速度主要用來描述參考系與慣性系之間的關(guān)系
我們來逐項分析上面這個式子。第一項中 αα 為 {B} 的角加速度，所以第一項的物理意義是 {B} 旋轉(zhuǎn)所造成的 P 的切向加速度。
第二項是 {B} 旋轉(zhuǎn)所造成的向心加速度。第四項為 P 相對于 {B} 的加速度，但在慣性系 {A} 下表達(dá)——類似于 vrvr，定義相對加速度 arar。
第三項比較特殊，為 {B} 的旋轉(zhuǎn)運(yùn)動與 P 相對 {B} 的平移運(yùn)動耦合產(chǎn)生的加速度，稱為「科氏加速度」。
可以看到，除了第四項外，另外三項都和 {B} 的旋轉(zhuǎn)有關(guān)。
什么時候能夠用到科式加速度呢？
在MSCKF1中，考慮的是參考系（b），因此需要考慮；但在MSCKF2中，考慮的是慣性系（a），所以就不需要考慮科式加速度了。
這里存在一個問題：既然不考慮科式加速度又簡便也符合邏輯，那么為什么一些文獻(xiàn)中采用考慮科式加速度的做法呢？這個問題保留。
2.誤差狀態(tài)更新方法
慣性導(dǎo)航算法常常用誤差狀態(tài)量來表示。實際上IMU組件給系統(tǒng)系統(tǒng)的運(yùn)動學(xué)方程，在狀態(tài)估計中可以認(rèn)為是一種約束。
同時測量方程也可以認(rèn)為是另外一種約束。在優(yōu)化系統(tǒng)中，將這兩種約束都寫入到優(yōu)化函數(shù)中，并且保證懲罰函數(shù)是線性的，這樣就可以利用梯度下降法簡單求解了。
但是因為方法2中需要知道狀態(tài)量的連續(xù)導(dǎo)數(shù)關(guān)系（隨時間變化的關(guān)系），很多系統(tǒng)都沒辦法求解這部分（在PVQ系統(tǒng)中可以做）。方法1中的離散形式比較符合對EKF的解釋和EKF中協(xié)方差更新的方法，因此更加常用。
總結(jié)來說，如果系統(tǒng)是連續(xù)方程，那么需要先進(jìn)行error state化之后再進(jìn)行離散化。如果系統(tǒng)是離散方程，那么直接進(jìn)行error state化即可。
下面是詳細(xì)定義：
第一種方法：
第二種方法：
在上面的兩種方法中，第一種是符合EKF的狀態(tài)方程的遞推關(guān)系的；第二種是目前VIO算法中的主流做法。應(yīng)該是殊途同歸的兩種做法，但是第一種更為通用。
其中，下面的推導(dǎo)是怎么完成的？
最符合邏輯的就是：兩個小量相乘，最后的結(jié)果就是無窮小，可以忽略。
與ESKF的區(qū)別是什么？下面給出ESKF的定義。
實際上是一樣的，不過在ESKF中多了Ran量。
3.卡爾曼濾波算法部分
確定方差矩陣
算法部分常用卡爾曼濾波，kalman濾波中需要確定幾個參量：過程協(xié)方差矩陣Q、測量協(xié)方差矩陣R和不確定方差矩陣P。
這里的問題是：這個部分到底是不是為了確定Q的大??？
于是我們得到隨機(jī)游走模型的完整表達(dá)。實際上，觀察離散模型的表達(dá)式，可以發(fā)現(xiàn)它生動闡釋了「隨機(jī)游走」的含義：每一時刻都是上一個采樣時刻加上一個高斯白噪聲得到的，猶如一個游走的粒子，踏出的下一步永遠(yuǎn)是隨機(jī)的。
在我們前面給出的 IMU 的運(yùn)動模型中，bias 就設(shè)定為服從隨機(jī)游走模型2.
在之前的論文里是這么做的，可見與上面的并不一致。
為什么呢不一致呢？參考博客的內(nèi)容，推斷的計算方法如下：但是這里是白噪聲還是隨機(jī)游走？理論上應(yīng)該是白噪聲，因為沒有討論隨機(jī)游走。
博客：https://zhuanlan.zhihu.com/p/71202672
在博客和文獻(xiàn)1，提到一種ESKF的姿態(tài)角度估計方法，對于Q的求解，給出下面的結(jié)果：
這里描述的方法也不一致，這就需要了解什么是連續(xù)的標(biāo)準(zhǔn)差和離散的標(biāo)準(zhǔn)差了。暫且以文獻(xiàn)1為標(biāo)準(zhǔn)。
如何確定協(xié)方差矩陣的更新？
要推導(dǎo)預(yù)積分量的協(xié)方差，我們需要知道 imu 噪聲和預(yù)積分量之間的線性遞推關(guān)系。協(xié)方差矩陣可以這樣推導(dǎo)：這個方差的積累公式需要注意一下，實際上狀態(tài)估計大多都是這么做的。
其中，Σn是測量噪聲的協(xié)方差矩陣，方差從 i 時刻開始進(jìn)行遞推Σ i i = 0
其中F是非線性系統(tǒng)函數(shù)對狀態(tài)量的雅可比矩陣，G代表對控制量的雅可比？
問題是：噪聲現(xiàn)在是不是就是控制量？噪聲可以做控制量，但是如果有明確含義的控制量，比如說：里程計的左右輪，那么左右輪的誤差就是sigma_n。
但是在傳統(tǒng)的卡爾曼濾波器的遞推公式中，有下面的結(jié)果：
在這里更新的過程并沒有考慮到控制量u的雅可比矩陣，這是為什么呢？因為u的雅可比矩陣只影響到P矩陣的更新，而并不影響標(biāo)稱值的更新。（標(biāo)稱值的更新依賴于更加準(zhǔn)確的非線性狀態(tài)更新方程）。
這里Q代表的過程誤差方差矩陣，對應(yīng)的維度是狀態(tài)量。利用控制量與運(yùn)動學(xué)方程，確實可以求出來狀態(tài)量的誤差方差矩陣。
相當(dāng)于狀態(tài)量的方差有一部分是由控制量決定的。但是如果系統(tǒng)中沒有控制量，只有狀態(tài)量，那么就需要直接給出Q矩陣的值。實際上這種系統(tǒng)也是比較多的。
在一個開源項目中是這樣確定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 vehicleQ = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2, sYaw**2])
可以看到這里的Q是用來描述連續(xù)微分運(yùn)動學(xué)方程中的微分量的。因為這里的Q只是在定義誤差量的過程噪聲，所以是比較合理的。
如何確定控制量u和狀態(tài)量x？
最終在MEKF的博客中找到答案
https://zhuanlan.zhihu.com/p/71202815
在文獻(xiàn)1中找到下面的佐證（附錄部分）。下面的差分化是利用了第二種狀態(tài)方程的遞推方法，然后進(jìn)行離散化。
由此便得知協(xié)方差Q矩陣的確定過程。實際上在實驗中，并沒有特別嚴(yán)格的定義，實際上兩種都可以。
為什么要用誤差狀態(tài)量去表示呢？
這部分參考Quaternion kinematics for the error-state Kalman filte和博客
https://zhuanlan.zhihu.com/p/88756311
定向誤差狀態(tài)是最小的，避免了與過度參數(shù)化相關(guān)的問題，以及相關(guān)協(xié)方差矩陣奇異性的風(fēng)險，這通常是由強(qiáng)制約束引起的
誤差狀態(tài)系統(tǒng)總是在接近原始系統(tǒng)的情況下運(yùn)行，因此遠(yuǎn)離可能的參數(shù)奇點、萬向鎖問題等，從而保證線性化的有效性始終保持不變
錯誤狀態(tài)總是很小，這意味著所有二階部分都可以忽略不計。這使得雅可比矩陣的計算變得非常簡單和快速。有些雅可比數(shù)甚至可能是常數(shù)或等于可用狀態(tài)量。
誤差動力學(xué)是緩慢的，因為所有的大信號動力學(xué)都已集成到標(biāo)稱狀態(tài)。這意味著我們可以以低于預(yù)測的速度應(yīng)用KF修正
什么是動力學(xué)是緩慢的？變化是緩慢的？是不是用來描述振動變化，有著更為出色的性能？如果是的話，這也是一個可以發(fā)論文的點。是不是和李代數(shù)去表示是有關(guān)系的？只有在小量的時候，在切空間的性質(zhì)才可以成立。
ESKF完整內(nèi)容
ESKF就是用了上述“基于誤差隨時間變化”求解方法，首先寫成error state，然后進(jìn)行離散化得到IMU的遞推方程。
參數(shù)表如下：
三種狀態(tài)之間的轉(zhuǎn)換關(guān)系：
首先是運(yùn)動學(xué)方程中：
給出下面：
其中 ???? ???? 為高斯隨機(jī)脈沖噪聲，均值為0，協(xié)方差為 ???? ，?????? 在 ????時間內(nèi)的積分值。
因此給出誤差傳播方程：
其次是測量方程：
測量方程求取雅可比矩陣采用鏈?zhǔn)椒▌t。
此外有：
總結(jié)來說，求取差分的偏導(dǎo)數(shù)如下，需要注意的是，我用到的是左欄的結(jié)果，右欄中未作考慮。
4.kalman濾波實例
5.實現(xiàn)代碼
參考項目：
https://github.com/cacacadaxia/ESKF-Attitude-Estimation
% 功能：1.實現(xiàn)ESKF算法，加深對于狀態(tài)估計的理解% 2.其中的問題：% 1） 測量加上地磁計% 2） 注意誤差量與標(biāo)稱量% 3） 四元數(shù)轉(zhuǎn)角度時有誤差，就是這個導(dǎo)致了誤差量%%%%--------------------------------------------------------------------------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; % 計算標(biāo)稱值 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---------------------- % 參考之前的函數(shù)，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)%四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣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)%旋轉(zhuǎn)矩陣轉(zhuǎn)四元數(shù)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);%歐拉角轉(zhuǎn)四元數(shù)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)%四元數(shù)轉(zhuǎn)歐拉角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
參考文獻(xiàn)：
Quaternion kinematics for the error-state Kalman Filter.pdf
https://fzheng.me/2016/11/20/imu_model_eq/
本文僅做學(xué)術(shù)分享，如有侵權(quán)，請聯(lián)系刪文。
