為什么不能用2dof的姿態(tài)估計(jì)：因?yàn)槠矫婕僭O(shè)只是近似滿足愁铺，實(shí)際還是有很多不平整凿菩、顛簸帜讲、低高度從而可駛過的地板等
為什么不能直接用3dof的姿態(tài)估計(jì)：因?yàn)檫\(yùn)動(dòng)受限（近似平面運(yùn)動(dòng)），從而導(dǎo)致額外的不可觀的自由度
一個(gè)單目系統(tǒng)似将，6自由度位姿和1自由度尺度不可觀
一個(gè)單目IMU系統(tǒng)蚀苛，3自由度平移（trans）和1自由度旋轉(zhuǎn)（yaw）不可觀玷氏，但是這是在IMU有足夠的3D運(yùn)動(dòng)的時(shí)候情況
在近似平面假設(shè)的自動(dòng)駕駛車輛上，額外的一個(gè)自由度變得不可觀了盏触，這就是尺度
這樣的情況例如：車輛在恒定加速度下運(yùn)動(dòng)，特別的雌芽，在完全已知陀螺儀偏差bg的簡化假設(shè)下辨嗽，對于某些運(yùn)動(dòng)世落，VINS的初始狀態(tài)不能唯一的確定
我們來看一看糟需，在更一般的情況下：未知的陀螺儀偏差bg，來確定不可觀的自由度
單目VINS系統(tǒng)洲押，狀態(tài)向量：
還要注意的是杈帐，q是G（大地坐標(biāo)）在I（IMU坐標(biāo)体箕，或者叫機(jī)體坐標(biāo)的位置），而p和v是更好理解的I在G中的位置
為什么q要這么寫娘荡？繼續(xù)往下看
而且這里為什么狀態(tài)只有一幀的，如果這是狀態(tài)的一部分，為什么特征點(diǎn)深度也在這里志秃？
根據(jù)上面這個(gè)嚼酝，推導(dǎo)連續(xù)系統(tǒng)微分方程：
然后是圖像特征：
zj可以理解為歸一化平面的像點(diǎn)，z=1所以省略钧舌，fj本來是在g系的圖像坐標(biāo)涎跨，減去自車位置P，再進(jìn)行自車旋轉(zhuǎn)碘赖，并用外參C變換到相機(jī)坐標(biāo)系
那么外构，在恒定加速度情況下：
定理一：這個(gè)VINS系統(tǒng)除了3軸平移和yaw不可觀以外播掷，還有一個(gè)不可觀的方向，當(dāng)且僅當(dāng)剛才這個(gè)條件5滿足
這個(gè)不可觀的方向就是尺度
證明：
我們來檢驗(yàn)線性化VINS模型的能觀性矩陣的右零空間垒酬，為了表示清楚件炉，我們只在狀態(tài)向量中包含一個(gè)特征點(diǎn)(擴(kuò)展到多個(gè)特征點(diǎn)很簡單)
能觀性矩陣的任意塊行是這樣的結(jié)構(gòu)：
下面的 x k+1|k 的這種表示 應(yīng)該是條件概率?
另外，下面這兩個(gè)傳播和測量方程寫得可能有些迷惑人斟冕，但實(shí)際上?第一個(gè)矩陣就是誤差狀態(tài)轉(zhuǎn)移矩陣雅克比磕蛇，第二個(gè)矩陣也就是H就是觀測矩陣雅克比
而能觀性矩陣的零空間為：（以下的O我確認(rèn)了就是上面的M），注意M有m行塊秀撇，這個(gè)m是啥？就是觀測方程的個(gè)數(shù)棠绘，也就是特征點(diǎn)的數(shù)量
以EKF為例
回到剛才的Mk拜效，其中Ti矩陣的定義就不寫在這里了，從這個(gè)Mk中可以得到：想要尺度維不可觀紧憾，當(dāng)且僅當(dāng)對于任意的K，都有MkNs=0
而Ns是這樣的
這里的NS構(gòu)建就是根據(jù)狀態(tài)向量構(gòu)建的，第一個(gè)0對應(yīng)q旋轉(zhuǎn) 第二個(gè)0對應(yīng)bg陀螺儀偏置甸赃，第三個(gè)對應(yīng)速度，第四個(gè)對應(yīng)ba加速度偏置苛茂，這里應(yīng)該是認(rèn)為在恒定加速度假設(shè)下鸠窗，加速度偏置就等于恒定的加速度，這個(gè)似乎是很自然的
然后我們就看看稍计，MKNS是否真的等于0
32等式成立是由于相機(jī)投影矩陣（觀測矩陣）的雅克比Hck的右零空間是imu坐標(biāo)下的特征點(diǎn)位置，完全看不懂在說什么净刮，但是在公式上我們注意到，如果我們有定理“Hck的右零空間是imu坐標(biāo)系下的特征點(diǎn)位置”株婴，從32看出暑认，特征點(diǎn)f在g中的位置-當(dāng)前imu坐標(biāo)系在g中的位置就等于特征點(diǎn)f在i中的位置，那就必有Hck * f=0
而且蘸际，這個(gè)新的N的零空間的向量Ns是和之前的4個(gè)不可觀維度N1不一樣的，因?yàn)镹1的第四個(gè)塊元是0根穷，而Ns中不是导坟，所以這時(shí)一個(gè)新的不可觀維度
我們來整理一下剛剛到底做了什么
首先我們在時(shí)刻L有一個(gè)m行的能觀性矩陣M或者叫O，對于VINS系統(tǒng)乍迄，我們已經(jīng)知道零空間有四個(gè)維度士败，就是N1，然后現(xiàn)在我們再構(gòu)建一個(gè)N向量叫做Ns漾狼，這個(gè)向量是根據(jù)我們目前的假設(shè)（系統(tǒng)在恒定加速度下運(yùn)動(dòng)）而構(gòu)建的饥臂，那么接下來我們就要證明MkNs總是等于0（對于M的任意一行），最終隅熙，通過一系列計(jì)算，和定理“Hck的右零空間是imu坐標(biāo)系下的特征點(diǎn)位置”酵熙，我們證明了這一點(diǎn)驰坊，而且我們確認(rèn)Ns并不在N1撐出的空間之中，因?yàn)镹s第四維不等于0，而N1所有向量第四維都是0察藐，所以我們找到了一個(gè)新的不可觀向量
證明結(jié)束
注意皮璧，這個(gè)不可觀的維度對應(yīng)的就是尺度
再額外放一點(diǎn)分飞，
下面這是，滑動(dòng)窗口中惨寿，或者說理想狀態(tài)是在真實(shí)值附近展開删窒，而現(xiàn)在只能在當(dāng)前估計(jì)下展開，從而會(huì)導(dǎo)致能觀性變化
要記住的就只是蕉拢，在這種真實(shí)情況下诚亚，比起理想，能觀性矩陣多了一下Δ站宗，叫做擾動(dòng)項(xiàng)，所以O(shè)的零空間最后一列（也就是零空間最后一列的Ni梢灭，在理想狀態(tài)下零空間一共有4列，現(xiàn)在最后一列不再滿足Ni*M=0了）不再滿足库快，在這里就是繞重力的旋轉(zhuǎn)從不可觀變成可觀钥顽，因?yàn)樗辉诹憧臻g中了
回到正文：
這個(gè)結(jié)論的物理解釋是闽铐，當(dāng)局部加速度不變是奶浦，人們無法區(qū)分真實(shí)機(jī)體加速度的大小和加速度偏置的大小，因?yàn)樗麄儠簳r(shí)都是不變的财喳，從而真實(shí)加速度大小可以是任意的斩狱，所以導(dǎo)致了尺度的模糊
我們應(yīng)該要意識到扎瓶，在實(shí)踐中，自動(dòng)駕駛車輛都是恒定加速度的秕岛，比如沿著直線恒速行駛（這時(shí)误证，恒定加速度就是重力加速度）或者恒加速度行駛，或者沿著恒定速度圓弧轉(zhuǎn)彎愈捅，這導(dǎo)致了VINS系統(tǒng)尺度的模糊（或者叫不可觀）
來看第二種情況，零旋轉(zhuǎn)
例如灌具，在一段時(shí)間上譬巫，旋轉(zhuǎn)保持不動(dòng)
定理二：當(dāng)且僅當(dāng)7滿足，VINS系統(tǒng)就有以下不可觀方向（或者說M的零空間多了一個(gè)向量No）
這個(gè)的證明暫時(shí)不寫了
注意诱贿，這時(shí)的不可觀測方向是所有三個(gè)自由度的全局旋轉(zhuǎn)方向，而不僅僅是yaw
物理解釋：當(dāng)沒有旋轉(zhuǎn)時(shí)珠十，無法從恒定的旋轉(zhuǎn)中區(qū)分局部重力加速度的方向和加速度偏置的方向锨阿，從而roll和pitch都變得不可觀（本來yaw就不可觀了，所以所有三個(gè)自由度旋轉(zhuǎn)都不可觀）
也就是當(dāng)自動(dòng)駕駛車輛純直線運(yùn)動(dòng)的時(shí)候墅诡，由于能觀性的缺失桐智，旋轉(zhuǎn)估計(jì)變得不準(zhǔn)確
結(jié)論：當(dāng)車輛恒定加速度或者純直線運(yùn)動(dòng)的時(shí)候，會(huì)有額外的不可觀方向然磷。我們要注意到刊驴，在實(shí)踐中并不會(huì)有絕對的恒加速度或者絕對的直線運(yùn)動(dòng)寡润，但當(dāng)汽車近似于這種運(yùn)動(dòng)的時(shí)候舅柜，其在不可觀方向得到的信息很少，從而信息矩陣是病態(tài)的致份，甚至缺秩，從而影響定位性能
解決方式绍载，允許車輛偏離直線路徑可以解決全局定向問題滔蝉，但是尺度問題不太好解決，需要車輛不斷的改變加速度蝠引，這不現(xiàn)實(shí)也會(huì)造成機(jī)構(gòu)磨損，所以我們選擇使用輪速計(jì)來解決這個(gè)問題
