久久国产成人av_抖音国产毛片_a片网站免费观看_A片无码播放手机在线观看,色五月在线观看,亚洲精品m在线观看,女人自慰的免费网址,悠悠在线观看精品视频,一级日本片免费的,亚洲精品久,国产精品成人久久久久久久

分享

基于誤差狀態(tài)卡爾曼濾波慣性導(dǎo)航理論

 taotao_2016 2023-03-11 發(fā)布于廣東

作者丨擦擦擦大俠
編輯丨古月居
點(diǎn)擊進(jìn)入—>3D視覺(jué)工坊學(xué)習(xí)交流群

慣性導(dǎo)航備注

1,、坐標(biāo)系之間換算

一般有兩個(gè)坐標(biāo)系:大地基準(zhǔn)坐標(biāo)系w系(或者G系)與機(jī)器人本體坐標(biāo)系b系(或者I系),兩坐標(biāo)系之間的旋轉(zhuǎn)矩陣表示為:

圖片

坐標(biāo)系計(jì)算的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)系定義:

歐拉角角速度與陀螺儀測(cè)量的本體角速度之間(eulerRates與bodyRates)有如下關(guān)系:

圖片

這個(gè)關(guān)系常用于歐拉角的運(yùn)動(dòng)學(xué)更新中,。其中,,W是可逆矩陣。

2,、幾種表示姿態(tài)的方法

  • 四元數(shù)表示角度

更新

可以這樣表示:

圖片

在MEKF算法的差分化之后,,有這樣的結(jié)果:

圖片

也可以表示為:

圖片

這里面一個(gè)很重要的性質(zhì)就是:

圖片
  • 四元數(shù)運(yùn)算

四元數(shù)相乘可以寫(xiě)成:

圖片

其中

圖片
  • 李代數(shù)表示角度

旋轉(zhuǎn)向量V與李群之間的轉(zhuǎn)換,需要經(jīng)過(guò)中間量“李代數(shù)”,。

圖片

也可以用羅德里格斯旋轉(zhuǎn)公式直接轉(zhuǎn)換:

圖片
  • 鏈?zhǔn)角髮?dǎo)方法

這個(gè)方法好像在eskf中用到的比較多,,

圖片

慣性導(dǎo)航算法理論

主要討論基于“誤差狀態(tài)”方法的kalman濾波設(shè)計(jì)方法。該算法為“ESKF”算法,,主要參考文獻(xiàn)1和github上的一些代碼,。

1.科式加速度

科式加速度的表示

參考網(wǎng)頁(yè):

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

科式加速度主要用來(lái)描述參考系與慣性系之間的關(guān)系

圖片

我們來(lái)逐項(xiàng)分析上面這個(gè)式子。第一項(xiàng)中 αα 為 {B} 的角加速度,,所以第一項(xiàng)的物理意義是 {B} 旋轉(zhuǎn)所造成的 P 的切向加速度,。

第二項(xiàng)是 {B} 旋轉(zhuǎn)所造成的向心加速度。第四項(xiàng)為 P 相對(duì)于 {B} 的加速度,,但在慣性系 {A} 下表達(dá)——類(lèi)似于 vrvr,,定義相對(duì)加速度 arar。

第三項(xiàng)比較特殊,,為 {B} 的旋轉(zhuǎn)運(yùn)動(dòng)與 P 相對(duì) {B} 的平移運(yùn)動(dòng)耦合產(chǎn)生的加速度,,稱(chēng)為「科氏加速度」。

可以看到,,除了第四項(xiàng)外,,另外三項(xiàng)都和 {B} 的旋轉(zhuǎn)有關(guān)。

什么時(shí)候能夠用到科式加速度呢,?

在MSCKF1中,,考慮的是參考系(b),因此需要考慮;但在MSCKF2中,,考慮的是慣性系(a),,所以就不需要考慮科式加速度了。

這里存在一個(gè)問(wèn)題:既然不考慮科式加速度又簡(jiǎn)便也符合邏輯,,那么為什么一些文獻(xiàn)中采用考慮科式加速度的做法呢,?這個(gè)問(wèn)題保留。

2.誤差狀態(tài)更新方法

慣性導(dǎo)航算法常常用誤差狀態(tài)量來(lái)表示,。實(shí)際上IMU組件給系統(tǒng)系統(tǒng)的運(yùn)動(dòng)學(xué)方程,,在狀態(tài)估計(jì)中可以認(rèn)為是一種約束。

同時(shí)測(cè)量方程也可以認(rèn)為是另外一種約束,。在優(yōu)化系統(tǒng)中,,將這兩種約束都寫(xiě)入到優(yōu)化函數(shù)中,并且保證懲罰函數(shù)是線性的,,這樣就可以利用梯度下降法簡(jiǎn)單求解了,。

圖片
圖片

但是因?yàn)榉椒?中需要知道狀態(tài)量的連續(xù)導(dǎo)數(shù)關(guān)系(隨時(shí)間變化的關(guān)系),很多系統(tǒng)都沒(méi)辦法求解這部分(在PVQ系統(tǒng)中可以做),。方法1中的離散形式比較符合對(duì)EKF的解釋和EKF中協(xié)方差更新的方法,,因此更加常用。

總結(jié)來(lái)說(shuō),,如果系統(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)是怎么完成的,?

圖片

最符合邏輯的就是:兩個(gè)小量相乘,,最后的結(jié)果就是無(wú)窮小,可以忽略,。

與ESKF的區(qū)別是什么,?下面給出ESKF的定義。

圖片

實(shí)際上是一樣的,,不過(guò)在ESKF中多了Ran量,。

3.卡爾曼濾波算法部分

  • 確定方差矩陣

算法部分常用卡爾曼濾波,kalman濾波中需要確定幾個(gè)參量:過(guò)程協(xié)方差矩陣Q、測(cè)量協(xié)方差矩陣R和不確定方差矩陣P,。

圖片

這里的問(wèn)題是:這個(gè)部分到底是不是為了確定Q的大?。?/p>

于是我們得到隨機(jī)游走模型的完整表達(dá),。實(shí)際上,,觀察離散模型的表達(dá)式,可以發(fā)現(xiàn)它生動(dòng)闡釋了「隨機(jī)游走」的含義:每一時(shí)刻都是上一個(gè)采樣時(shí)刻加上一個(gè)高斯白噪聲得到的,,猶如一個(gè)游走的粒子,,踏出的下一步永遠(yuǎn)是隨機(jī)的。

在我們前面給出的 IMU 的運(yùn)動(dòng)模型中,,bias 就設(shè)定為服從隨機(jī)游走模型2.

在之前的論文里是這么做的,,可見(jiàn)與上面的并不一致。

圖片

為什么呢不一致呢,?參考博客的內(nèi)容,,推斷的計(jì)算方法如下:但是這里是白噪聲還是隨機(jī)游走?理論上應(yīng)該是白噪聲,,因?yàn)闆](méi)有討論隨機(jī)游走,。

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

圖片

在博客和文獻(xiàn)1,提到一種ESKF的姿態(tài)角度估計(jì)方法,,對(duì)于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):這個(gè)方差的積累公式需要注意一下,實(shí)際上狀態(tài)估計(jì)大多都是這么做的,。

圖片

其中,,Σn是測(cè)量噪聲的協(xié)方差矩陣,方差從 i 時(shí)刻開(kāi)始進(jìn)行遞推Σ i i = 0

其中F是非線性系統(tǒng)函數(shù)對(duì)狀態(tài)量的雅可比矩陣,,G代表對(duì)控制量的雅可比,?

問(wèn)題是:噪聲現(xiàn)在是不是就是控制量?噪聲可以做控制量,,但是如果有明確含義的控制量,,比如說(shuō):里程計(jì)的左右輪,那么左右輪的誤差就是sigma_n,。

但是在傳統(tǒng)的卡爾曼濾波器的遞推公式中,,有下面的結(jié)果:

圖片

在這里更新的過(guò)程并沒(méi)有考慮到控制量u的雅可比矩陣,,這是為什么呢?因?yàn)閡的雅可比矩陣只影響到P矩陣的更新,,而并不影響標(biāo)稱(chēng)值的更新,。(標(biāo)稱(chēng)值的更新依賴(lài)于更加準(zhǔn)確的非線性狀態(tài)更新方程)。

這里Q代表的過(guò)程誤差方差矩陣,,對(duì)應(yīng)的維度是狀態(tài)量,。利用控制量與運(yùn)動(dòng)學(xué)方程,確實(shí)可以求出來(lái)狀態(tài)量的誤差方差矩陣,。

相當(dāng)于狀態(tài)量的方差有一部分是由控制量決定的,。但是如果系統(tǒng)中沒(méi)有控制量,只有狀態(tài)量,,那么就需要直接給出Q矩陣的值,。實(shí)際上這種系統(tǒng)也是比較多的。

在一個(gè)開(kāi)源項(xià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 vehicle
Q = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2, sYaw**2])

可以看到這里的Q是用來(lái)描述連續(xù)微分運(yùn)動(dòng)學(xué)方程中的微分量的,。因?yàn)檫@里的Q只是在定義誤差量的過(guò)程噪聲,,所以是比較合理的。

  • 如何確定控制量u和狀態(tài)量x,?

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

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

在文獻(xiàn)1中找到下面的佐證(附錄部分),。下面的差分化是利用了第二種狀態(tài)方程的遞推方法,然后進(jìn)行離散化,。

圖片

由此便得知協(xié)方差Q矩陣的確定過(guò)程,。實(shí)際上在實(shí)驗(yàn)中,并沒(méi)有特別嚴(yán)格的定義,,實(shí)際上兩種都可以,。

  • 為什么要用誤差狀態(tài)量去表示呢?

這部分參考Quaternion kinematics for the error-state Kalman filte和博客

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

定向誤差狀態(tài)是最小的,,避免了與過(guò)度參數(shù)化相關(guān)的問(wèn)題,,以及相關(guān)協(xié)方差矩陣奇異性的風(fēng)險(xiǎn),這通常是由強(qiáng)制約束引起的

誤差狀態(tài)系統(tǒng)總是在接近原始系統(tǒng)的情況下運(yùn)行,,因此遠(yuǎn)離可能的參數(shù)奇點(diǎn),、萬(wàn)向鎖問(wèn)題等,從而保證線性化的有效性始終保持不變

錯(cuò)誤狀態(tài)總是很小,,這意味著所有二階部分都可以忽略不計(jì)。這使得雅可比矩陣的計(jì)算變得非常簡(jiǎn)單和快速,。有些雅可比數(shù)甚至可能是常數(shù)或等于可用狀態(tài)量,。

誤差動(dòng)力學(xué)是緩慢的,因?yàn)樗械拇笮盘?hào)動(dòng)力學(xué)都已集成到標(biāo)稱(chēng)狀態(tài),。這意味著我們可以以低于預(yù)測(cè)的速度應(yīng)用KF修正

什么是動(dòng)力學(xué)是緩慢的,?變化是緩慢的?是不是用來(lái)描述振動(dòng)變化,有著更為出色的性能,?如果是的話,,這也是一個(gè)可以發(fā)論文的點(diǎn)。是不是和李代數(shù)去表示是有關(guān)系的,?只有在小量的時(shí)候,,在切空間的性質(zhì)才可以成立。

  • ESKF完整內(nèi)容

ESKF就是用了上述“基于誤差隨時(shí)間變化”求解方法,,首先寫(xiě)成error state,,然后進(jìn)行離散化得到IMU的遞推方程。

參數(shù)表如下:

圖片

三種狀態(tài)之間的轉(zhuǎn)換關(guān)系:

圖片

首先是運(yùn)動(dòng)學(xué)方程中:

給出下面:

圖片

其中 ???? ???? 為高斯隨機(jī)脈沖噪聲,,均值為0,,協(xié)方差為 ???? ,?????? 在 ????時(shí)間內(nèi)的積分值,。

因此給出誤差傳播方程:

圖片

其次是測(cè)量方程:

測(cè)量方程求取雅可比矩陣采用鏈?zhǔn)椒▌t,。

此外有:

圖片
圖片

總結(jié)來(lái)說(shuō),求取差分的偏導(dǎo)數(shù)如下,,需要注意的是,,我用到的是左欄的結(jié)果,右欄中未作考慮,。

圖片

4.kalman濾波實(shí)例

圖片

5.實(shí)現(xiàn)代碼

參考項(xiàng)目:

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

%  功能:1.實(shí)現(xiàn)ESKF算法,,加深對(duì)于狀態(tài)估計(jì)的理解%        2.其中的問(wèn)題:%     1) 測(cè)量加上地磁計(jì)%     2) 注意誤差量與標(biāo)稱(chēng)量%     3) 四元數(shù)轉(zhuǎn)角度時(shí)有誤差,就是這個(gè)導(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);%%這個(gè)用到theta = zeros(3,1);%%這個(gè)用不到
% 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;    % 計(jì)算標(biāo)稱(chēng)值   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;%%這個(gè)固定偏差是什么鬼  yaw   = atan2(T(1,2),T(1,1))*rad2deg;%%這個(gè)固定偏差是什么鬼  end

參考文獻(xiàn):

Quaternion kinematics for the error-state Kalman Filter.pdf 

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


本文僅做學(xué)術(shù)分享,如有侵權(quán),,請(qǐng)聯(lián)系刪文,。

    本站是提供個(gè)人知識(shí)管理的網(wǎng)絡(luò)存儲(chǔ)空間,所有內(nèi)容均由用戶(hù)發(fā)布,,不代表本站觀點(diǎn),。請(qǐng)注意甄別內(nèi)容中的聯(lián)系方式、誘導(dǎo)購(gòu)買(mǎi)等信息,,謹(jǐn)防詐騙,。如發(fā)現(xiàn)有害或侵權(quán)內(nèi)容,請(qǐng)點(diǎn)擊一鍵舉報(bào),。
    轉(zhuǎn)藏 分享 獻(xiàn)花(0

    0條評(píng)論

    發(fā)表

    請(qǐng)遵守用戶(hù) 評(píng)論公約

    類(lèi)似文章 更多