找人做网站去哪里找什么叫网站空间

找人做网站去哪里找,什么叫网站空间,访问国外网站的软件,网站备案信息如何注销吗相关内容地址#xff1a;SLAM之——卡尔曼滤波算法#xff08; KF#xff09;之扩展卡尔曼滤波#xff08;EKF, Extended Kalman Filter#xff09;深度详解 一、符号与约定 使用右侧乘法/右扰动约定#xff1a; 旋转矩阵表示 R∈SO(3)R\in SO(3)R∈SO(3) 将机体系向量变…相关内容地址SLAM之——卡尔曼滤波算法 KF之扩展卡尔曼滤波EKF, Extended Kalman Filter深度详解一、符号与约定使用右侧乘法/右扰动约定旋转矩阵表示R ∈ S O ( 3 ) R\in SO(3)R∈SO(3)将机体系向量变换到惯性系v I R v B v_I R v_BvI​RvB​。角速度ω \omegaω表示机体系测得的角速度body frame。旋转微扰以右乘小旋转表达真正的旋转R R ^ exp ⁡ ( [ δ θ ] × ) R \hat R \exp([\delta\theta]\times)RR^exp([δθ]×)其中δ θ ∈ R 3 \delta\theta\in\mathbb R^3δθ∈R3切空间的误差向量[ ⋅ ] × [\cdot]\times[⋅]×是反对称矩阵。四元数也可用但此处用S O ( 3 ) SO(3)SO(3)矩阵和指数映射表示以便导数清晰。重力向量用g gg在惯性系中g [ 0 , 0 , − 9.81 ] ⊤ g[0,0,-9.81]^\topg[0,0,−9.81]⊤。IMU 测量模型离散时间采样Δ t \Delta tΔt测量的角速度带偏置与噪声ω m ω b g n g \omega_m \omega b_g n_gωm​ωbg​ng​。测量的线加速度机体系a m a b a n a a_m a b_a n_aam​aba​na​其中a aa是机体在机体坐标下去重力的加速度实际关系见下。二、状态与误差定义1. 全状态估计值时刻k kk的全状态augmented可取为x k [ R k p k v k b g , k b a , k ] , x_k \begin{bmatrix} R_k p_k v_k b_{g,k} b_{a,k} \end{bmatrix},xk​[Rk​​pk​​vk​​bg,k​​ba,k​​],其中R k ∈ S O ( 3 ) R_k\in SO(3)Rk​∈SO(3)机体到惯性系的旋转p k ∈ R 3 p_k\in\mathbb R^3pk​∈R3惯性系下位置v k ∈ R 3 v_k\in\mathbb R^3vk​∈R3惯性系下速度b g , k ∈ R 3 b_{g,k}\in\mathbb R^3bg,k​∈R3陀螺偏置b a , k ∈ R 3 b_{a,k}\in\mathbb R^3ba,k​∈R3加速度计偏置。2. 误差状态在切空间或者manifold空间manifold流行空间参考SLAM文献之A micro Lie theory for state estimation in robotic1我们定义误差状态局部为δ x [ δ θ δ p δ v δ b g δ b a ] ∈ R 15 , \delta x \begin{bmatrix} \delta\theta \ \delta p \ \delta v \ \delta b_g \ \delta b_a \end{bmatrix}\in\mathbb R^{15},δx[δθδpδvδbg​δba​​]∈R15,其中δ θ ∈ R 3 \delta\theta\in\mathbb R^3δθ∈R3是右扰动so(3) 的向量使得真旋转R R ^ exp ⁡ ( [ δ θ ] × ) R \hat R \exp([\delta\theta]_\times)RR^exp([δθ]×​)其余分量为欧氏向量差位置、速度、偏置。状态协方差定义在误差空间上P E [ δ x , δ x ⊤ ] P \mathbb{E}[\delta x,\delta x^\top]PE[δx,δx⊤]。三、IMU 预积分连续到离散与线性化IMU 预积分的思路在两次关键帧或滤波更新时刻i ii和j jj之间对 IMU 测量进行积分得到相对增量( Δ R i j , Δ v i j , Δ p i j ) (\Delta R_{ij}, \Delta v_{ij}, \Delta p_{ij})(ΔRij​,Δvij​,Δpij​)它们在估计偏置的基础上被线性化并保留关于偏置的一阶雅可比。1. 连续推进的理想无噪模型离散化在理想无噪无偏条件下使用去偏测量从t i t_iti​到t j t_jtj​时长Δ t i j \Delta t_{ij}Δtij​连续积分得到相对旋转R ˙ ( t ) R ( t ) [ ω ( t ) ] × . \dot R(t) R(t) [\omega(t)]_\times.R˙(t)R(t)[ω(t)]×​.速度惯性系v ˙ I ( t ) R ( t ) a ( t ) g . \dot v_I(t) R(t) a(t) g.v˙I​(t)R(t)a(t)g.位置p ˙ I ( t ) v I ( t ) . \dot p_I(t) v_I(t).p˙​I​(t)vI​(t).离散化后的预积分定义在小步法下Δ R i j ≜ R i ⊤ R j \Delta R_{ij} \triangleq R_i^\top R_jΔRij​≜Ri⊤​Rj​估计的相对旋转Δ v i j ≜ R i ⊤ ( v j − v i − g Δ t i j ) \Delta v_{ij} \triangleq R_i^\top (v_j - v_i - g\Delta t_{ij})Δvij​≜Ri⊤​(vj​−vi​−gΔtij​)Δ p i j ≜ R i ⊤ ( p j − p i − v i Δ t i j − 1 2 g Δ t i j 2 ) \Delta p_{ij} \triangleq R_i^\top (p_j - p_i - v_i \Delta t_{ij} - \tfrac12 g\Delta t_{ij}^2)Δpij​≜Ri⊤​(pj​−pi​−vi​Δtij​−21​gΔtij2​)这些量可从原始 IMU 测量逐步累积得到参见微分推进公式。2. 预积分的离散递推常用 Euler / mid-point在每个小时间步δ t \delta tδt用测量( ω m , a m ) (\omega_m, a_m)(ωm​,am​)已减去当前偏置估计b ^ g , b ^ a \hat b_g,\hat b_ab^g​,b^a​做更新例如 mid-point步k → k 1 k\to k1k→k1短时计算去偏角速度ω ~ ω m − b ^ g \tilde\omega \omega_m - \hat b_gω~ωm​−b^g​。δ R ← δ R exp ⁡ ( [ ω ~ δ t ] × ) \delta R \leftarrow \delta R \exp([\tilde\omega \delta t]_\times)δR←δRexp([ω~δt]×​)。更新速度增量δ v ← δ v δ R ( a ~ ) δ t \delta v \leftarrow \delta v \delta R (\tilde a)\delta tδv←δvδR(a~)δt其中a ~ a m − b ^ a \tilde a a_m - \hat b_aa~am​−b^a​。更新位置增量:δ p ← δ p δ v δ t 1 2 δ R a ~ δ t 2 \delta p \leftarrow \delta p \delta v \delta t \tfrac12 \delta R\tilde a\delta t^2δp←δpδvδt21​δRa~δt2或更稳定的中点法。实际实现里通常使用mid-point 或 RK2/RK4提升精度。3. 预积分的偏置雅可比必需为了在后续滤波中把偏置不确定性与偏置更新影响纳入需要保存雅可比矩阵J Δ R b g ∈ R 3 × 3 J_{\Delta Rb_g}\in\mathbb R^{3\times3}JΔRbg​​∈R3×3∂ Log ⁡ ( Δ R ) / ∂ b g \partial \operatorname{Log}(\Delta R)/\partial b_g∂Log(ΔR)/∂bg​的线性化J Δ v b g , J Δ v b a ∈ R 3 × 3 J_{\Delta vb_g}, J_{\Delta vb_a}\in\mathbb R^{3\times3}JΔvbg​​,JΔvba​​∈R3×3J Δ p b g , J Δ p b a ∈ R 3 × 3 J_{\Delta pb_g}, J_{\Delta pb_a}\in\mathbb R^{3\times3}JΔpbg​​,JΔpba​​∈R3×3。这些雅可比可以在预积分递推中逐步更新有标准递推公式见下。原则上预积分把Δ R , Δ v , Δ p \Delta R,\Delta v,\Delta pΔR,Δv,Δp用当前偏置线性化为Δ R ( b g ) ≈ Δ R ( b ^ g ) exp ⁡ ( J Δ R , b g ( b g − b ^ g ) ) , \Delta R(b_g) \approx \Delta R(\hat b_g)\exp\big(J_{\Delta R,b_g} (b_g-\hat b_g)\big),ΔR(bg​)≈ΔR(b^g​)exp(JΔR,bg​​(bg​−b^g​)),Δ v ( b g , b a ) ≈ Δ v ( b ^ ) J Δ v b g ( b g − b ^ g ) J Δ v b a ( b a − b ^ a ) , \Delta v(b_g,b_a) \approx \Delta v(\hat b) J_{\Delta vb_g}(b_g-\hat b_g) J_{\Delta vb_a}(b_a-\hat b_a),Δv(bg​,ba​)≈Δv(b^)JΔvbg​​(bg​−b^g​)JΔvba​​(ba​−b^a​),Δ p ( b g , b a ) ≈ Δ p ( b ^ ) J Δ p b g ( b g − b ^ g ) J Δ p b a ( b a − b ^ a ) . \Delta p(b_g,b_a) \approx \Delta p(\hat b) J_{\Delta pb_g}(b_g-\hat b_g) J_{\Delta pb_a}(b_a-\hat b_a).Δp(bg​,ba​)≈Δp(b^)JΔpbg​​(bg​−b^g​)JΔpba​​(ba​−b^a​).实现上将这些雅可比以递推形式累积参见 Forster 等人的 Preintegration 论文或 IMU library。4. 预积分噪声协方差预积分还要累积测量噪声IMU 白噪声、随机游走等到一个3 × 3 3\times33×3或9 × 9 9\times99×9的预积分协方差矩阵Σ Δ Cov ( [ Log ⁡ ( Δ R ) , Δ v , Δ p ] ⊤ ) . \Sigma_{\Delta} \text{Cov}\big([\operatorname{Log}(\Delta R), \Delta v, \Delta p]^\top\big).ΣΔ​Cov([Log(ΔR),Δv,Δp]⊤).该矩阵在递推中通过线性化映射G GG累积 IMU 噪声。四、误差状态的预测利用预积分当我们用预积分从关键时刻i ii传播到j jj并且用当前估计偏置b ^ g i , b ^ a i \hat b_{gi},\hat b_{ai}b^gi​,b^ai​进行预积分预测模型观测模型形式为州态预测基于估计R ^ j R ^ i Δ R i j ( b ^ i ) \hat R_j \hat R_i \Delta R_{ij}(\hat b_i) \R^j​R^i​ΔRij​(b^i​)v ^ j v ^ i g Δ t i j R ^ i Δ v i j ( b ^ i , b ^ a i ) \hat v_j \hat v_i g \Delta t_{ij} \hat R_i \Delta v_{ij}(\hat b_i,\hat b_{ai})v^j​v^i​gΔtij​R^i​Δvij​(b^i​,b^ai​)p ^ j p ^ i v ^ i Δ t i j 1 2 g Δ t i j 2 R ^ i Δ p i j ( b ^ i , b ^ a i ) \hat p_j \hat p_i \hat v_i \Delta t_{ij} \tfrac12 g \Delta t_{ij}^2 \hat R_i \Delta p_{ij}(\hat b_i,\hat b_{ai})p^​j​p^​i​v^i​Δtij​21​gΔtij2​R^i​Δpij​(b^i​,b^ai​)偏置预测若在无模型假设下常用随机游走b ^ g j b ^ g i , b ^ a j b ^ a i \hat b_{gj} \hat b_{gi},\qquad \hat b_{aj} \hat b_{ai}b^gj​b^gi​,b^aj​b^ai​误差变换线性化构造F FF把误差δ x i \delta x_iδxi​在时刻i ii线性传播到时刻j jj可写成δ x j ≈ F δ x i G n \delta x_j \approx F\delta x_i Gnδxj​≈Fδxi​Gn其中n nn包括 IMU 测量噪声角速噪声n g n_gng​、加速度噪声n a n_ana​和偏置随机游走噪声如果建模的话。矩阵F ∈ R 15 × 15 F\in\mathbb R^{15\times15}F∈R15×15与G ∈ R 15 × m G\in\mathbb R^{15\times m}G∈R15×mm mm为噪声维度来自对上述非线性传播方程关于误差的线性化一个常见的块结构近似如下写误差顺序为[ δ θ , δ p , δ v , δ b g , δ b a ] [\delta\theta,\ \delta p,\ \delta v,\ \delta b_g,\ \delta b_a][δθ,δp,δv,δbg​,δba​]则线性化近似示意F ≈ [ I 3 0 0 − I 3 Δ t ω → θ 0 F p θ I 3 F p v F p b g F p b a F v θ 0 I 3 F v b g F v b a 0 0 0 I 3 0 0 0 0 0 I 3 ] . F \approx \begin{bmatrix} I_3 0 0 -I_3 \Delta t_{\omega\to\theta} 0 \\ F_{p\theta} I_3 F_{pv} F_{pb_g} F_{pb_a} \\ F_{v\theta} 0 I_3 F_{vb_g} F_{vb_a} \\ 0 0 0 I_3 0 \\ 0 0 0 0 I_3 \end{bmatrix}.F≈​I3​Fpθ​Fvθ​00​0I3​000​0Fpv​I3​00​−I3​Δtω→θ​Fpbg​​Fvbg​​I3​0​0Fpba​​Fvba​​0I3​​​.更具体的每块在离散预积分上下文常见写法Forster 等Lupton 等F v θ − R ^ i [ Δ v i j ] × F_{v\theta} -\hat R_i [\Delta v_{ij}]_\timesFvθ​−R^i​[Δvij​]×​或相似项取决于约定F p θ − R ^ i [ Δ p i j ] × F_{p\theta} -\hat R_i [\Delta p_{ij}]_\timesFpθ​−R^i​[Δpij​]×​F v b g − R ^ i J Δ v , b g F_{vb_g} -\hat R_i J_{\Delta v,b_g}Fvbg​​−R^i​JΔv,bg​​F p b g − R ^ i J Δ p , b g F_{pb_g} -\hat R_i J_{\Delta p,b_g}Fpbg​​−R^i​JΔp,bg​​F v b a − R ^ i J Δ v , b a F_{vb_a} -\hat R_iJ_{\Delta v,b_a}Fvba​​−R^i​JΔv,ba​​等注准确的F FF元素要看用的预积分累积方程midpoint / first-order及扰动约定常用参考为 Forster et al., “IMU Preintegration on Manifold” (RSS 2015) —— 该文给出了详细递推式。协方差传播用线性化结果更新协方差P j F P i F ⊤ G Q n G ⊤ , P_j F P_i F^\top G Q_n G^\top,Pj​FPi​F⊤GQn​G⊤,其中Q n Q_nQn​是 IMU 噪声角速噪声、加速度噪声、偏置随机游走等在离散步上的协方差G GG将这些噪声映射到误差状态。五、测量更新以外部位姿测量为例假设在时刻j jj有一个外部位姿测量例如来自视觉 or LiDAR 构建的位姿约束测量模型可写成以i ii为参考测量提供相对或绝对位姿。以绝对位姿量测( R ˉ j , p ˉ j ) (\bar R_j,\bar p_j)(Rˉj​,pˉ​j​)为例其观测残差在误差空间为旋转残差右扰动r R Log ⁡ ( R ˉ j ⊤ R ^ j ) ≈ R ^ j ⊤ ( R ^ j exp ⁡ ( [ δ θ j ] × ) ) ≈ δ θ j ( 线性近似 ) r_R \operatorname{Log}\big(\bar R_j^\top \hat R_j\big)\approx \hat R_j^\top(\hat R_j \exp([\delta\theta_j]_\times)) \approx \delta\theta_j \quad (\text{线性近似})rR​Log(Rˉj⊤​R^j​)≈R^j⊤​(R^j​exp([δθj​]×​))≈δθj​(线性近似)更常用写法量测模型给出r R ≈ δ θ j n R r_R \approx \delta\theta_j n_RrR​≈δθj​nR​位置残差r p p ˉ j − p ^ j ≈ − δ p j n p r_p \bar p_j - \hat p_j \approx -\delta p_j n_prp​pˉ​j​−p^​j​≈−δpj​np​合并观测矩阵H HH将误差态映射到观测残差例如r [ r R r p ] ≈ H δ x j n z , r \begin{bmatrix} r_R \ r_p \end{bmatrix} \approx H\delta x_j n_z,r[rR​rp​​]≈Hδxj​nz​,其中H HH取形如下若量测只与R , p R,pR,p相关H [ I 3 0 0 0 0 0 − I 3 0 0 0 ] . H \begin{bmatrix} I_3 0 0 0 0 \\ 0 -I_3 0 0 0 \end{bmatrix}.H[I3​0​0−I3​​00​00​00​].然后 EKF 标准更新卡尔曼增益K P j H ⊤ ( H P j H ⊤ R z ) − 1 K P_j H^\top (H P_j H^\top R_z)^{-1}KPj​H⊤(HPj​H⊤Rz​)−1误差态更新δ x ^ K r \delta \hat x K rδx^Kr协方差更新Joseph form 更数值稳定P j ← ( I − K H ) P j ( I − K H ) ⊤ K R z K ⊤ P_j \leftarrow (I - K H) P_j (I - K H)^\top K R_z K^\topPj​←(I−KH)Pj​(I−KH)⊤KRz​K⊤将误差注入到全状态状态修正位置/速度/偏置直接加法修正p ^ j ← p ^ j δ p ^ , v ^ j ← v ^ j δ v ^ , b ^ g ← b ^ g δ b ^ g , b ^ a ← b ^ a δ b ^ a . \hat p_j \leftarrow \hat p_j \delta \hat p,\quad \hat v_j \leftarrow \hat v_j \delta \hat v,\quad \hat b_g \leftarrow \hat b_g \delta \hat b_g,\quad \hat b_a \leftarrow \hat b_a \delta \hat b_a.p^​j​←p^​j​δp^​,v^j​←v^j​δv^,b^g​←b^g​δb^g​,b^a​←b^a​δb^a​.旋转修正右扰动R ^ j ← R ^ j exp ⁡ ( [ δ θ ^ ] × ) . \hat R_j \leftarrow \hat R_j \exp([\delta\hat\theta]_\times).R^j​←R^j​exp([δθ^]×​).重要修正后需把误差态置零resetδ x ← 0 , \delta x \leftarrow 0,δx←0,但注意协方差仍保留为更新后的P j P_jPj​因为P j P_jPj​表示误差的不确定度。重线性化 / 重新预积分如果有偏置更改如果在注入后偏置估计发生了变化δ b ^ ≠ 0 \delta \hat b \neq 0δb^0原先基于b ^ i \hat b_ib^i​做的预积分需要重约束 / 重线性化常见做法保留原始 IMU 测量序列或增量信息使用新的偏置b ^ i δ b ^ \hat b_i\delta \hat bb^i​δb^重新计算Δ R , Δ v , Δ p \Delta R,\Delta v,\Delta pΔR,Δv,Δp或者用预积分的偏置雅可比对当前预积分做一阶校正效率更高Δ v ← Δ v J Δ v b g δ b ^ g J Δ v b a δ b ^ a , \Delta v \leftarrow \Delta v J_{\Delta vb_g}\delta \hat b_g J_{\Delta vb_a}\delta \hat b_a,Δv←ΔvJΔvbg​​δb^g​JΔvba​​δb^a​,Δ p ← Δ p J Δ p b g δ b ^ g J Δ p b a δ b ^ a , \Delta p \leftarrow \Delta p J_{\Delta pb_g}\delta \hat b_g J_{\Delta pb_a}\delta \hat b_a,Δp←ΔpJΔpbg​​δb^g​JΔpba​​δb^a​,Δ R ← Δ R exp ⁡ ( J Δ R b g δ b ^ g ) . \Delta R \leftarrow \Delta R \exp\big(J_{\Delta Rb_g}\delta \hat b_g\big).ΔR←ΔRexp(JΔRbg​​δb^g​).以上校正通常足够因为预积分已线性化到偏置比重积分原始 IMU 更高效。六、算法伪代码初始化: \hat R_0, \hat p_0, \hat v_0, \hat b_{g,0}, \hat b_{a,0}, P_0$ 每到新的关键时刻 j: 1) 用 IMU 测量从 i - j 做预积分得到: ΔR_{ij}, Δv_{ij}, Δp_{ij}, J_{ΔR,bg}, J_{Δv,bg}, J_{Δv,ba}, J_{Δp,bg}, J_{Δp,ba}, Σ_Δ (预积分噪声协方差) 2) 预测: \hat R_j \hat R_i ΔR_{ij} \hat v_j \hat v_i g Δt \hat R_i Δv_{ij} \hat p_j \hat p_i \hat v_i Δt 0.5 g Δt^2 \hat R_i Δp_{ij} \hat b_{* j} \hat b_{* i} 3) 线性化计算 F, G由 Δ 与 J 确定 4) 协方差传播: P_j F P_i F^T G Q_n G^T 5) 若有外部观测pose / pos / landmarks: 构造残差 r 和 H K P_j H^T (H P_j H^T R_z)^{-1} δx K r 注入误差: \hat R_j ← \hat R_j exp([δθ]_×) \hat p_j ← \hat p_j δp ... 协方差更新: P_j ← (I-KH) P_j (I-KH)^T K R_z K^T 将误差状态清零 6) 如果偏置更新 δb ≠ 0: 根据 J 的一阶修正 ΔR, Δv, Δp不用重积分七、实现要点与常见陷阱约定一致性务必在代码/文档里明确“扰动是左乘还是右乘”“角速度在哪一坐标系表示”“R RR定义body-inertial or inverse”。不同约定会改变雅可比符号与F FF的具体块项。重投影/重线性化 vs 预积分雅可比修正采用雅可比一阶修正通常足够且高效如果偏置变化很大或预积分步数很少考虑重积分原始 IMU稳健但慢。数值稳定性在做exp ⁡ ( [ δ θ ] × ) \exp([\delta\theta]_\times)exp([δθ]×​)或log ⁡ ( R ) \log(R)log(R)时使用稳定的 Rodrigues / series 展开当角度很小时用小角近似避免数值问题。协方差对称性与正定性用 Joseph 形式更新协方差以保障数值稳定性若出现非正定数值误差可做微正则P ← ( P P ⊤ ) / 2 P \leftarrow (PP^\top)/2P←(PP⊤)/2或加小对角量。观测信息的参考系输入的外部位姿需转换到滤波器的惯性/世界基准。偏置随机游走建模若把偏置建模为随机游走Q n Q_nQn​要包含偏置驱动项从而G Q n G ⊤ G Q_n G^\topGQn​G⊤中包含偏置过程噪声对P PP的贡献。观测延迟与时序视觉或 LiDAR 常有延迟需做时序对齐或滑动窗口或基于关键帧 预积分的滑动窗口优化。八、参考实现点用 Eigen 等库实现S O ( 3 ) SO(3)SO(3)运算[ ⋅ ] × , exp ⁡ , log ⁡ [\cdot]_\times,\exp,\log[⋅]×​,exp,log并封装稳定的小角实现。预积分模块应支持累积Δ R , Δ v , Δ p \Delta R,\Delta v,\Delta pΔR,Δv,Δp同步更新并导出雅可比J ⋅ , b J_{\cdot,b}J⋅,b​导出预积分噪声协方差Σ Δ \Sigma_\DeltaΣΔ​支持在偏置修正下做一阶校正。使用 double 精度以避免长时间积分误差累积。单元测试对合成数据验证无偏场景下残差期望为零对已知旋转/偏置合成场景验证雅可比数值一致性数值差分检验。九、关键公式总结预测利用预积分R ^ j R ^ i Δ R i j , v ^ j v ^ i g Δ t R ^ i Δ v i j , p ^ j p ^ i v ^ i Δ t 1 2 g Δ t 2 R ^ i Δ p i j . \hat R_j \hat R_i \Delta R_{ij},\quad \\ \hat v_j \hat v_i g\Delta t \hat R_i \Delta v_{ij},\quad\\ \hat p_j \hat p_i \hat v_i \Delta t \tfrac12 g\Delta t^2 \hat R_i \Delta p_{ij}.R^j​R^i​ΔRij​,v^j​v^i​gΔtR^i​Δvij​,p^​j​p^​i​v^i​Δt21​gΔt2R^i​Δpij​.偏置一阶修正注入后Δ v ← Δ v J Δ v , b g , δ b g J Δ v , b a , δ b a , \Delta v \leftarrow \Delta v J_{\Delta v,b_g},\delta b_g J_{\Delta v,b_a},\delta b_a,Δv←ΔvJΔv,bg​​,δbg​JΔv,ba​​,δba​,Δ p ← Δ p J Δ p , b g , δ b g J Δ p , b a , δ b a , \Delta p \leftarrow \Delta p J_{\Delta p,b_g},\delta b_g J_{\Delta p,b_a},\delta b_a,Δp←ΔpJΔp,bg​​,δbg​JΔp,ba​​,δba​,Δ R ← Δ R exp ⁡ ( J Δ R , b g , δ b g ) . \Delta R \leftarrow \Delta R \exp\big(J_{\Delta R,b_g},\delta b_g\big).ΔR←ΔRexp(JΔR,bg​​,δbg​).协方差传播P j F P i F ⊤ G Q n G ⊤ . P_j F P_i F^\top G Q_n G^\top.Pj​FPi​F⊤GQn​G⊤.观测更新位姿K P j H ⊤ ( H P j H ⊤ R z ) − 1 , δ x K r , K P_j H^\top (H P_j H^\top R_z)^{-1},\quad \delta x K r,KPj​H⊤(HPj​H⊤Rz​)−1,δxKr,R ^ j ← R ^ j exp ⁡ ( [ δ θ ] × ) , p ^ j ← p ^ j δ p , P j ← ( I − K H ) P j ( I − K H ) ⊤ K R z K ⊤ . \hat R_j \leftarrow \hat R_j \exp([\delta\theta]_\times),\quad \\ \hat p_j \leftarrow \hat p_j \delta p,\\ \quad P_j \leftarrow (I-KH)P_j(I-KH)^\top K R_z K^\top.R^j​←R^j​exp([δθ]×​),p^​j​←p^​j​δp,Pj​←(I−KH)Pj​(I−KH)⊤KRz​K⊤.