文章目录

MIT Cheetah Learning (一)：State Estimate
- Summary
- 先验知识
- - A.Extended Kalman Filter
  - B.四元数、旋转矩阵、李群李代数等
- Part 1——Sensor Device and Measurement Models
- - A. Encoders
  - B. IMU
- Part 2——State Estimate
- - A.Filter State Definition
  - B.Prediction Model(IMU)
  - C.Measurement Model(Encoders)
  - D.Extended Kalman Filter
- Part 3——Observability Analysis
- - A.Nonlinear Model
  - B.Extended Kalman Filter
- Part 4——Conclusion
- - A.实验结论
  - B.后续

MIT Cheetah Learning (一)：State Estimate

MIT Cheetah 3中融合了许多论文中的算法和技术。
论文链接：MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot
MIT Cheetah Learning系列将解析Cheetah的算法部分，尝试将其中每一个模块解释清楚。

Cheetah 3 中的State Estimate是参考2013年ETH的论文——
“State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU”
论文链接：State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU

因此，只需搞明白ETH的论文即可。
下面将具体介绍论文是如何用IMU与编码器实现State Estimate

Summary

以Extended Kalman Filter（EKF）算法为核心，仅使用IMU与电机的编码器，可以准确估计除了yaw和absolute position以外的机器人状态（yaw和absolute position在短距离运动中误差也仅在10%），包括roll、pitch、velocity。
在任意步态、任意地形下，都可以实现对机器人的状态估计。前提是机器人至少有一条腿与地面接触，并且假设机器人与地面接触时，仅会发生非常小的的滑动。

先验知识

A.Extended Kalman Filter

Definition:
θ k \theta_k θk为真实值， θ k ′ \theta'_k θk′为模型预测值， ⟨ θ k ⟩ \langle \theta_k \rangle ⟨θk⟩为估计值（系统输出）， z k z_k zk为系统观测值， s k s_k sk为状态转移噪声，服从高斯分布， v k v_k vk为观测噪声，也服从高斯分布

EKF的状态转移方程和观测方程如下：
{ θ k = f ( θ k − 1 ) + s k ( 1 ) z k = h ( θ k ) + v k ( 2 ) \left\{ \begin{array}{ll} \theta_k = f(\theta_{k-1})+s_k & (1)\\ z_k = h(\theta_{k}) + v_k & (2) \end{array}\right. {θk=f(θk−1)+skzk=h(θk)+vk(1)(2)
对(1)(2)式进行泰勒展开（雅克比矩阵性质： f ( x ) = f ( x 0 ) + J ⋅ ( x − x 0 ) f(x) = f(x_0)+J \cdot(x-x_0) f(x)=f(x0)+J⋅(x−x0)），得到
{ θ k = f ( θ k − 1 ) + s k = f ( ⟨ θ k − 1 ⟩ ) + F k − 1 ( θ k − 1 − ⟨ θ k − 1 ⟩ ) + s k ( 3 ) z k = h ( θ k ) + v k = h ( θ k ′ ) + H ( θ − θ k ′ ) + v k ( 4 ) \left\{ \begin{array}{ll} \theta_k = f(\theta_{k-1})+s_k = f(\langle \theta_{k-1} \rangle)+F_{k-1}(\theta_{k-1}-\langle \theta_{k-1} \rangle)+s_k & (3)\\ z_k = h(\theta_{k}) + v_k = h(\theta'_k) + H(\theta-\theta'_k) +v_k& (4) \end{array}\right. {θk=f(θk−1)+sk=f(⟨θk−1⟩)+Fk−1(θk−1−⟨θk−1⟩)+skzk=h(θk)+vk=h(θk′)+H(θ−θk′)+vk(3)(4)
引入反馈
⟨ θ k ⟩ = θ k ′ + K k ( z k − h ( θ k ′ ) ) \langle \theta_{k} \rangle = \theta'_k+K_k(z_k-h(\theta'_k)) ⟨θk⟩=θk′+Kk(zk−h(θk′))
由以上式子可以得到EKF的Predict和Update部分
Predict:
{ θ k ′ = f ( ⟨ θ k − 1 ⟩ ) Σ k ′ = F k − 1 Σ k − 1 ′ F k − 1 T + Q \left\{ \begin{array}{ll} \theta'_k = f(\langle \theta_{k-1} \rangle)\\ \Sigma'_k = F_{k-1} \Sigma'_{k-1} F_{k-1}^T + Q \end{array}\right. {θk′=f(⟨θk−1⟩)Σk′=Fk−1Σk−1′Fk−1T+Q
Update:
{ S k = ( H k Σ k ′ H k T + R ) − 1 K k = Σ k ′ H k T S k ⟨ θ k ⟩ = θ k ′ + K k ( z k − h ( θ k ′ ) ) Σ k = ( I − K k H k ) Σ k ′ \left\{ \begin{array}{ll} S_k = (H_{k} \Sigma'_{k} H_{k}^T+R)^{-1}\\ K_k = \Sigma'_k H^T_{k} S_k \\ \langle \theta_{k} \rangle = \theta'_k+K_k(z_k-h(\theta'_k)) \\ \Sigma_k = (I-K_k H_k)\Sigma'_k \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧Sk=(HkΣk′HkT+R)−1Kk=Σk′HkTSk⟨θk⟩=θk′+Kk(zk−h(θk′))Σk=(I−KkHk)Σk′

其中，雅克比矩阵 F k = ∂ f ∂ θ ∣ ⟨ θ k − 1 ⟩ F_k = \frac{\partial f}{\partial \theta}|_{\langle \theta_{k-1} \rangle} Fk=∂θ∂f∣⟨θk−1⟩

雅克比矩阵 H k = ∂ h ∂ θ ∣ θ k ′ H_k = \frac{\partial h}{\partial \theta}|_{\theta'_k} Hk=∂θ∂h∣θk′

协方差矩阵 Σ k = ⟨ ( θ k − ⟨ θ k ⟩ ) ( θ k − ⟨ θ k ⟩ ) T ⟩ \Sigma_k = \langle (\theta_k-\langle \theta_k \rangle)(\theta_k-\langle \theta_k \rangle)^T \rangle Σk=⟨(θk−⟨θk⟩)(θk−⟨θk⟩)T⟩，表示估计值与真实值之间的误差

协方差矩阵 Σ k ′ = ⟨ ( θ k − θ k ′ ) ( θ k − θ k ′ ) T ⟩ \Sigma'_k = \langle (\theta_k-\theta'_k) (\theta_k-\theta'_k)^T \rangle Σk′=⟨(θk−θk′)(θk−θk′)T⟩，表示预测值与真实值之间的误差

状态转移噪声协方差矩阵 Q = ⟨ s k s k T ⟩ Q = \langle s_k s_k^T\rangle Q=⟨skskT⟩

观测噪声协方差矩阵 R = ⟨ v k v k T ⟩ R = \langle v_k v_k^T\rangle R=⟨vkvkT⟩

K k K_k Kk为卡尔曼增益

具体推导可参考扩展卡尔曼滤波

B.四元数、旋转矩阵、李群李代数等

参考书籍：《视觉SLAM十四讲(第二版)》—— 高翔著

以下定义会在后续会使用到：

( ⋅ ) × (\cdot)^{\times} (⋅)×表示将向量转化成反对称矩阵
Ω ( ⋅ ) \Omega(\cdot) Ω(⋅)将任意的角速度转化为4 × 4 {\times}4 ×4矩阵，表示相应的四元速率
Ω : ω ↦ Ω ( ω ) = [ 0 ω z − ω y ω x − ω z 0 ω x ω y ω y − ω x 0 ω z − ω x − ω y − ω z 0 ] \Omega : \omega \mapsto \Omega(\omega) = \begin{bmatrix} 0 & \omega_z & -\omega_y & \omega_x\\ -\omega_z & 0 & \omega_x & \omega_y \\ \omega_y & -\omega_x & 0 & \omega_z \\ -\omega_x & -\omega_y & -\omega_z & 0 \end{bmatrix} Ω:ω↦Ω(ω)=⎣⎢⎢⎡0−ωzωy−ωxωz0−ωx−ωy−ωyωx0−ωzωxωyωz0⎦⎥⎥⎤
ζ ( ⋅ ) \zeta(\cdot) ζ(⋅) 将旋转向量的误差转化为四元数误差
ζ : v ↦ ζ ( v ) = [ s i n ( 1 2 ∣ ∣ v ∣ ∣ ) v ∣ ∣ v ∣ ∣ c o s ( 1 2 ∣ ∣ v ∣ ∣ ) ] \zeta : v \mapsto \zeta(v) = \begin{bmatrix} sin(\frac{1}{2}||v||)\frac{v}{||v||} \\ cos(\frac{1}{2}||v||) \end{bmatrix} ζ:v↦ζ(v)=[sin(21∣∣v∣∣)∣∣v∣∣vcos(21∣∣v∣∣)]

Part 1——Sensor Device and Measurement Models

A. Encoders

增量式编码器提供了所有电机的转动真实角度 α \alpha α，该反馈信息受到 n α n_{\alpha} nα高斯噪声的影响
α ~ = α + n α \tilde{\alpha} = \alpha + n_{\alpha} α~=α+nα
因此由正运动学可知所有足端在 B o d y F r a m e Body Frame BodyFrame( B B B)下的坐标 s i s_i si
s i = l k i n i ( α ) + n s , i s_i = lkin_i(\alpha) + n_{s,i} si=lkini(α)+ns,i
l k i n i ( ⋅ ) lkin_i(\cdot) lkini(⋅)表示腿部正运动学模型， n s , i n_{s, i} ns,i表示离散高斯噪声（校准误差与运动学模型误差）
定义—— R α R_{\alpha} Rα为 n α n_{\alpha} nα的协方差矩阵， R s R_s Rs为 n i , s n_{i,s} ni,s的协方差矩阵

B. IMU

由Part 1中IMU模型可知

加速度计得到在 B B B下的加速度 f f f，陀螺仪得到在 B B B下的角速度 ω \omega ω

旋转矩阵 C C C表示从世界坐标系 I I I到机器人坐标系 B B B变换

a a a表示 I I I下的加速度

因此可得
f = C ( a − g ) f = C(a-g) f=C(a−g) f ~ = f + b f + w f \tilde{f} = f+b_f+w_f f~=f+bf+wf b ˙ f = ω b f \dot b_f = \omega_{bf} b˙f=ωbf ω ~ = ω + b ω + w ω \tilde{\omega} = \omega+b_{\omega}+w_{\omega} ω~=ω+bω+wω b ˙ ω = ω b ω \dot b_{\omega} = \omega_{b\omega} b˙ω=ωbω
其中 f ~ , ω ~ \tilde{f},\tilde{\omega} f~,ω~是受高斯噪声项 ω f , w ω \omega_{f},w_{\omega} ωf,wω和偏置项 b f , b ω b_f,b_{\omega} bf,bω影响得到的测量值，且偏置项的导数可以由高斯噪声 ω b f , ω b ω \omega_{bf},\omega_{b\omega} ωbf,ωbω表示

定义以上四个高斯噪声的协方差矩阵为 Q f , Q b f , Q ω , Q b ω Q_f, Q_{bf},Q_{\omega},Q_{b\omega} Qf,Qbf,Qω,Qbω

Part 2——State Estimate

A.Filter State Definition

Definition:
r r r为在世界坐标系 I I I下，机器人身体中心的位置

v v v为机器人在世界坐标系 I I I下的速度

q q q为世界坐标系 I I I到机器人坐标系 B B B的旋转四元数

p 1 , p 2 , . . . , p N p_1, p_2, ..., p_N p1,p2,...,pN为机器人足端在世界坐标系 I I I下的位置

b f , b ω b_f, b_{\omega} bf,bω为在机器人坐标系 B B B下，IMU的偏置项（可由旋转矩阵转化到世界坐标系 I I I下）

因此我们定义
x : = ( r , v , q , p 1 , . . . , p N , b f , b ω ) x:= (r, v, q, p_1, ..., p_N, b_f, b_{\omega}) x:=(r,v,q,p1,...,pN,bf,bω) P : = c o v ( Δ x ) P:= cov(\Delta x) P:=cov(Δx) Δ x : = ( Δ r , Δ v , Δ q , Δ p 1 , . . . , Δ p N , Δ b f , Δ b ω ) \Delta x := (\Delta r, \Delta v, \Delta q, \Delta p_1, ..., \Delta p_N, \Delta b_f, \Delta b_{\omega}) Δx:=(Δr,Δv,Δq,Δp1,...,ΔpN,Δbf,Δbω)

B.Prediction Model(IMU)

由Part 1中IMU模型可知
r ˙ = v v ˙ = a = C T ( f ~ − b f − ω f ) + g q ˙ = 1 2 Ω ( ω ) q = 1 2 Ω ( ω ~ − b ω − ω ω ) q p i ˙ = C T ω p , i , ∀ i ∈ 1 , . . . , N b f ˙ = ω b f b ω ˙ = ω b ω \begin{aligned} &\dot{r} = v \\ & \dot{v}=a=C^T(\tilde{f}-b_f-\omega_f)+g \\ &\dot{q} = \frac{1}{2}\Omega(\omega) q = \frac{1}{2}\Omega(\tilde{\omega}-b_{\omega}-\omega_{\omega})q \\ & \dot{p_i} = C^T\omega_{p,i},\forall i \in {1, ..., N} \\ & \dot{b_f} = \omega_{bf} \\ & \dot{b_\omega} = \omega_{b\omega} \end{aligned} r˙=vv˙=a=CT(f~−bf−ωf)+gq˙=21Ω(ω)q=21Ω(ω~−bω−ωω)qpi˙=CTωp,i,∀i∈1,...,Nbf˙=ωbfbω˙=ωbω
白噪声 ω p , i \omega_{p,i} ωp,i表示足端与地面接触可能产生的微小滑动

Q p , i Q_{p,i} Qp,i为 ω p , i \omega_{p,i} ωp,i的协方差矩阵，定义如下:
Q p , i : = [ ω p , i , x 0 0 0 ω p , i , y 0 0 0 ω p , i , z ] Q_{p,i} : = \begin{bmatrix} \omega_{p,i,x} & 0 & 0 \\ 0 & \omega_{p,i,y} & 0 \\ 0 & 0 & \omega_{p,i,z} \end{bmatrix} Qp,i:=⎣⎡ωp,i,x000ωp,i,y000ωp,i,z⎦⎤

C.Measurement Model(Encoders)

由Part 1中Encoders模型可知
s i ~ : = l k i n i ( α ~ ) ≈ l k i n i ( α ) + J l k i n , i n α ≈ s i − n s , i + J l k i n , i n α ⏟ n i \tilde{s_i}:=lkin_i(\tilde{\alpha}) \\ \approx lkin_i(\alpha)+J_{lkin,i}n_{\alpha} \\ \approx s_i \underbrace{- n_{s,i}+J_{lkin,i}n_{\alpha}}_{n_{i}} si~:=lkini(α~)≈lkini(α)+Jlkin,inα≈sini −ns,i+Jlkin,inα
其中，雅克比矩阵 J l k i n , i : = ∂ l k i n i ( α ) ∂ α i J_{lkin,i}:=\frac{\partial lkin_i(\alpha)}{\partial \alpha_i} Jlkin,i:=∂αi∂lkini(α)
n i n_{i} ni可视为经过线性离散化的噪声，包含编码器噪声和正运动学计算噪声，即观测误差
R i R_i Ri是每一条腿的观测误差协方差矩阵，同时由Part 1.A可知
R i = R s + J l k i n , i R α J l k i n , i T R_i = R_s+J_{lkin,i}R_{\alpha}J_{lkin,i}^T Ri=Rs+Jlkin,iRαJlkin,iT

由定义可知， s i ~ \tilde{s_i} si~可以表示 s i ~ = C ( p i − r ) + n i \tilde{s_i} = C(p_i-r)+n_i si~=C(pi−r)+ni，即足端坐标与机器人中心坐标（世界坐标系 I I I）的差值乘上旋转矩阵

D.Extended Kalman Filter

将编码器视为观测值， Δ x \Delta x Δx视为真实值，即可建立EKF状态转移方程与观测方程，如下
{ Δ x k = f ( Δ x k − 1 ) + q k s k = h ( Δ x k ) + v k \left\{ \begin{array}{ll} \Delta x_k = f(\Delta x_{k-1})+q_k \\ s_k = h(\Delta x_{k}) + v_k \end{array}\right. {Δxk=f(Δxk−1)+qksk=h(Δxk)+vk
Predict:
经过模型线性化处理之后（参考文章Computing integrals involving the matrix exponential），得到关于 Δ x \Delta x Δx的协方差矩阵 F k F_k Fk，以及离散过程噪声的协方差矩阵 Q k Q_k Qk（ Q k Q_k Qk包含了 Q f , Q b f , Q ω , Q b ω Q_f, Q_{bf},Q_{\omega},Q_{b\omega} Qf,Qbf,Qω,Qbω）
两个协方差矩阵 F k , Q k F_k,Q_k Fk,Qk的具体表达式在论文中由展示，在这里便不展示了（会涉及反对称阵，四元数，e指数，罗德里格斯公式等）
因此可以得到
{ Δ ′ x = f ( ⟨ Δ x k − 1 ⟩ ) P k + 1 − = F k P k + F k T + Q k \left\{ \begin{array}{ll} \Delta' x = f(\langle \Delta x_{k-1} \rangle)\\ P^-_{k+1} = F_k P^+_k F_k^T + Q_k \end{array}\right. {Δ′x=f(⟨Δxk−1⟩)Pk+1−=FkPk+FkT+Qk

Update:
引入 y k y_k yk，定义如下：
y k : = ( ( s ~ 1 , k − C ^ k − ( p ^ 1 , k − − r ^ k − ) ⋮ ( s ~ N , k − C ^ k − ( p ^ N , k − − r ^ k − ) ) y_k : = \left(\begin{array}{l} (\tilde{s}_{1,k}-\hat{C}^-_k(\hat{p}^-_{1,k}-\hat{r}^-_k) \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vdots \\ (\tilde{s}_{N,k}-\hat{C}^-_k(\hat{p}^-_{N,k}-\hat{r}^-_k) \end{array} \right) yk:=⎝⎜⎛(s~1,k−C^k−(p^1,k−−r^k−) ⋮(s~N,k−C^k−(p^N,k−−r^k−)⎠⎟⎞
关于式子 s ~ i , k − C ^ k − ( p ^ i , k − − r ^ k − ) \tilde{s}_{i,k}-\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k) s~i,k−C^k−(p^i,k−−r^k−)，使用泰勒展开，忽略所有的高阶项，得到
s ~ i , k − C ^ k − ( p ^ i , k − − r ^ k − ) ≈ − C ^ k − Δ r k − + C ^ k − Δ p i , k − + ( C ^ k − ( p ^ i , k − − r ^ k − ) ) × Δ ϕ k − \tilde{s}_{i,k}-\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k) \approx -\hat{C}^-_k \Delta r^-_k+\hat{C}^-_k \Delta p^-_{i,k}+(\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k))^{\times}\Delta \phi^-_k s~i,k−C^k−(p^i,k−−r^k−)≈−C^k−Δrk−+C^k−Δpi,k−+(C^k−(p^i,k−−r^k−))×Δϕk−
证明如下：

定义 ∂ g ( x k ) ∂ x k : = s ~ i , k − C ^ k − ( p ^ i , k − − r ^ k − ) \frac{\partial g( x_k)}{\partial x_k} := \tilde{s}_{i,k}-\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k) ∂xk∂g(xk):=s~i,k−C^k−(p^i,k−−r^k−)
∵ ∂ g ( x k ) ∂ x k = − C ^ k − Δ r k − + C ^ k − Δ p i , k − + ( C ^ k − ( p ^ i , k − − r ^ k − ) ) × Δ ϕ k − + G ( Δ v k − ) \because \frac{\partial g( x_k)}{\partial x_k} = -\hat{C}^-_k \Delta r^-_k+\hat{C}^-_k \Delta p^-_{i,k}+(\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k))^{\times}\Delta \phi^-_k+G(\Delta v^-_k) ∵∂xk∂g(xk)=−C^k−Δrk−+C^k−Δpi,k−+(C^k−(p^i,k−−r^k−))×Δϕk−+G(Δvk−)
= − C ^ k − Δ r k − + C ^ k − Δ p i , k − + ( C ^ k − ( p ^ i , k − − r ^ k − ) ) × Δ ϕ k − =-\hat{C}^-_k \Delta r^-_k+\hat{C}^-_k \Delta p^-_{i,k}+(\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k))^{\times}\Delta \phi^-_k =−C^k−Δrk−+C^k−Δpi,k−+(C^k−(p^i,k−−r^k−))×Δϕk−
G ( Δ v k − ) G(\Delta v^-_k) G(Δvk−)可视为 − C ^ k − Δ r k − -\hat{C}^-_k \Delta r^-_k −C^k−Δrk−的高阶项，因此可以忽略
∴ s ~ i , k − C ^ k − ( p ^ i , k − − r ^ k − ) ≈ − C ^ k − Δ r k − + C ^ k − Δ p i , k − + ( C ^ k − ( p ^ i , k − − r ^ k − ) ) × Δ ϕ k − \therefore \tilde{s}_{i,k}-\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k) \approx -\hat{C}^-_k \Delta r^-_k+\hat{C}^-_k \Delta p^-_{i,k}+(\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k))^{\times}\Delta \phi^-_k ∴s~i,k−C^k−(p^i,k−−r^k−)≈−C^k−Δrk−+C^k−Δpi,k−+(C^k−(p^i,k−−r^k−))×Δϕk−
因此我们可以得到雅克比矩阵 H k = ∂ y k ∂ x k H_k = \frac{\partial y_k}{\partial x_k} Hk=∂xk∂yk
H k = [ − C ^ k − 0 ( C ^ k − ( p ^ 1 , k − − r ^ k − ) ) × C ^ k − ⋯ 0 0 0 ⋮ ⋮ ⋮ ⋮ ⋱ ⋮ ⋮ ⋮ − C ^ k − 0 ( C ^ k − ( p ^ N , k − − r ^ k − ) ) × C ^ k − ⋯ 0 0 0 ] H_k = \begin{bmatrix} -\hat{C}^-_k & 0 & (\hat{C}^-_k(\hat{p}^-_{1,k}-\hat{r}^-_k))^{\times} & \hat{C}^-_k & \cdots& 0 & 0 & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ -\hat{C}^-_k & 0 & (\hat{C}^-_k(\hat{p}^-_{N,k}-\hat{r}^-_k))^{\times} & \hat{C}^-_k & \cdots& 0 & 0 & 0 \end{bmatrix} Hk=⎣⎢⎡−C^k−⋮−C^k−0⋮0(C^k−(p^1,k−−r^k−))×⋮(C^k−(p^N,k−−r^k−))×C^k−⋮C^k−⋯⋱⋯0⋮00⋮00⋮0⎦⎥⎤
由Part 2.C可知， R k R_k Rk是观测误差的协方差矩阵
R k = [ R 1 , k ⋱ R N , k ] R_k = \begin{bmatrix} R_{1,k} & &\\ & \ddots \\ & & R_{N,k} \end{bmatrix} Rk=⎣⎡R1,k⋱RN,k⎦⎤
因此我们可以得到EFK的Update等式，如下：
{ S k = ( H k P k − H k T + R k ) K k = P k − H k T S k − 1 ⟨ Δ x k ⟩ = K k y k ( ∗ ) P k + = ( I − K k H k ) P k − \left\{ \begin{array}{ll} S_k = (H_{k} P^-_{k} H_{k}^T+R_k)\\ K_k = P^-_{k} H^T_{k} S^{-1}_k \\ \langle \Delta x_k \rangle = K_k y_k & (*)\\ P^+_{k} = (I-K_k H_k)P^-_k \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧Sk=(HkPk−HkT+Rk)Kk=Pk−HkTSk−1⟨Δxk⟩=KkykPk+=(I−KkHk)Pk−(∗)
( ∗ ) (*) (∗)式证明如下：
⟨ Δ x i , k ⟩ = Δ x i , k ′ + K k ( s ~ i , k − C ^ k − ( p ^ i , k − − r ^ k − ) ) \langle \Delta x_{i,k} \rangle = \Delta x'_{i,k}+K_k(\tilde{s}_{i,k}-\hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k)) ⟨Δxi,k⟩=Δxi,k′+Kk(s~i,k−C^k−(p^i,k−−r^k−))
由于 Δ x i , k ′ \Delta x'_{i,k} Δxi,k′与 C ^ k − ( p ^ i , k − − r ^ k − ) \hat{C}^-_k(\hat{p}^-_{i,k}-\hat{r}^-_k) C^k−(p^i,k−−r^k−)式等价的
∴ ⟨ Δ x i , k ⟩ = K k y k \therefore \langle \Delta x_{i,k} \rangle = K_k y_k ∴⟨Δxi,k⟩=Kkyk

Part 3——Observability Analysis

A.Nonlinear Model

参考文章Nonlinear controllability and observability

该部分从数学的角度证明了对于该机器人系统来说，有以下几个结论：

absolute position和yaw是无法被观测的，但是其他的所有状态都可以被准确估计。
对于任意的非线性地形，只要有至少三条腿与地面接触，就可以准确估计机器人状态。
当机器人与地面没有任何接触时，状态无法估计，收敛效果极差

具体证明过程可参考论文本身，数学部分较为复杂，便不一一阐述

B.Extended Kalman Filter

参考文章Analysis and improvement of the consistency of extended Kalman filter based SLAM

参考文章A counter example to the theory of simultaneous localization and map building

该部分从数学的角度证明了对于该机器人系统的EKF模型来说，有以下结论：

对于无法观测的非线性子空间系统，将其线性离散化之后，依然是无法观测的

Part 4——Conclusion

A.实验结论

对于任意地形和任意步态来说，absolute position和yaw是无法观测的，但是在移动短距离的实验中，误差还是能够控制在10%左右，并非无法衡量。
roll、pitch和velocity可以准确观测，可以很好的作为机器人的反馈信息。

（这个实验结果太牛了，必须附上）

B.后续

对于Part 3的证明，还有一些问题没有搞清楚，特别是非线性机器人模型的可观测性这个部分。

后面在搭建好四足机器人平台之后，尝试复现State Estimate。