IMU比较详细的内容(主要是针对卡尔曼滤波器)(摘自于ipythonnotebook)

C++代码，编的比较好，可以仿照。下面是对他的资料的整理。

• 代码均是基于面向对象设计的，调用函数后发现参数输入太少了吧
  • 运行后发现结果不对
  • 对于GPS的模块的调用显得很复杂，最后给位置算积分也不太对
    实际上是仿照这个代码 Kalman-master ，内容主要包含下面两张图：
    
    The coordinate system is as in DIN70000:
    
    非常重要：横滚（roll）(fai)和俯仰（pitch）(seta)摇头（yaw）(psi)
    单位如下：

	The units are:
			Lat/Lon in decimal degree (from GPS)
			speed in km/h (from GPS)
			course in degree (from GPS)
			roll/pitch/yaw in degree (from IMU)
			roll-/pitch-/yawrate in degrees/second (from IMU)
			acclerations in m/s² (from IMU)

该开源项目主要是针对“汽车”而言，所以很多情况下是一个二维的系统，只有xy两个方向。为此，首先的简化如下：

1. 恒定速度行驶，汽车过隧道
   这里阐述了相应矩阵的确定：

		P： 初始化不确定性
		R： 观测误差协方差
		Q： 过程噪声协方差(类似于风的印象，加速度的干扰)

$P_{0}=\left[\begin{array}{cccc}{\sigma_{x}^{2}} & {0} & {0} & {0} \\ {0} & {\sigma_{y}^{2}} & {0} & {0} \\ {0} & {0} & {\sigma_{x}^{2}} & {0} \\ {0} & {0} & {0} & {\sigma_{y}^{2}}\end{array}\right]$

$R=\left[\begin{array}{cc}{\sigma_{\dot{x}}^{2}} & {0} \\ {0} & {\sigma_{\dot{y}}^{2}}\end{array}\right]$
过程噪声协方差比较复杂：
$Q=\left[\begin{array}{cccc}{\sigma_{x}^{2}} & {\sigma_{x y}} & {\sigma_{x x}} & {\sigma_{x y}} \\ {\sigma_{y x}} & {\sigma_{y}^{2}} & {\sigma_{y \dot{x}}} & {\sigma_{y \dot{y}}} \\ {\sigma_{x x}} & {\sigma_{x y}} & {\sigma_{x}^{2}} & {\sigma_{x \dot{y}}} \\ {\sigma_{y x}} & {\sigma_{y y}} & {\sigma_{y x}} & {\sigma_{y}^{2}}\end{array}\right]$
有如下：
$Q=G \cdot G^{T} \cdot \sigma_{v}^{2}$
其中，
$G=\left[\begin{array}{llll}{0.5 d t^{2}} & {0.5 d t^{2}} & {d t} & {d t}\end{array}\right]^{T}$
代码如下：

sa = 0.1
G = np.matrix([[1/2.0*dt**2],
               [1/2.0*dt**2],
               [dt],
               [dt],
               [1.0],
               [1.0]])
Q = G*G.T*sa**2
print(Q, Q.shape)

但是有如下结果，是不是代表已经发散了？

事实证明已经发散了。因为这里观测的是加速度，但是位置的初始误差是很难估计出来的。
2. 球在三维中运动，观测球的位置
模拟球的运动：

Hz = 100.0 # Frequency of Vision System
dt = 1.0/Hz
T = 1.0 # s measuremnt time
m = int(T/dt) # number of measurements

px= 0.0 # x Position Start
py= 0.0 # y Position Start
pz= 1.0 # z Position Start

vx = 10.0 # m/s Velocity at the beginning
vy = 0.0 # m/s Velocity
vz = 0.0 # m/s Velocity

c = 0.1 # Drag Resistance Coefficient阻力阻力系数
d = 0.9 # Damping阻尼

Xr=[]
Yr=[]
Zr=[]
for i in range(int(m)):
    accx = -c*vx**2  # Drag Resistance
    
    vx += accx*dt
    px += vx*dt

    accz = -9.806 + c*vz**2 # Gravitation + Drag
    vz += accz*dt
    pz += vz*dt
    
    if pz<0.01:
        vz=-vz*d
        pz+=0.02
    if vx<0.1:
        accx=0.0
        accz=0.0
        
    Xr.append(px)
    Yr.append(py)
    Zr.append(pz)
# 模拟球的运动

估计位置距离球的位置0.25m。
卡尔曼滤波器仅用于线性动态系统。阻力系数在某种状态下是非线性的，但是滤波器可以处理该阻力，直到达到一定程度的阻力为止。但是这时，球撞到了地面，非线性度过高，滤波器提供了错误的解决方案。因此，必须在滤波器环路中对开关进行建模，这有助于滤波器获得它。
下面给出了离散的动力学模型：
$x_{k+1}=\left[\begin{array}{ccccccccc}{1} & {0} & {0} & {\Delta t} & {0} & {0} & {\frac{1}{2} \Delta t^{2}} & {0} & {0} \\ {0} & {1} & {0} & {0} & {\Delta t} & {0} & {0} & {\frac{1}{2} \Delta t^{2}} & {0} \\ {0} & {0} & {1} & {0} & {0} & {\Delta t} & {0} & {0} & {\Delta t^{2}} \\ {0} & {0} & {0} & {1} & {0} & {0} & {\Delta t} & {0} & {0} \\ {0} & {0} & {0} & {0} & {1} & {0} & {0} & {\Delta t} & {0} \\ {0} & {0} & {0} & {0} & {0} & {1} & {0} & {0} & {\Delta t} \\ {0} & {0} & {0} & {0} & {0} & {0} & {1} & {0} & {0} \\ {0} & {0} & {0} & {0} & {0} & {0} & {0} & {1} & {0} \\ {0} & {0} & {0} & {0} & {0} & {0} & {0} & {0} & {1}\end{array}\right]x_{k}$
这里同样结果很糟糕
3. 观测位置与加速度
结果不错。
4. 这里涉及到加速度计与陀螺仪之间的关系估算角度，观测是用加速度计和陀螺仪
转速传感器直接测量，加速度传感器直接测量重力，因此不直接测量倾斜角ф。倾斜角和由加速度传感器测量的垂直加速度a之间存在数学联系。它是余弦。如果自行车直立，则加速度a恰好为1g，如果ф= 90°，则加速度为0g。
$a=g \cdot \cos \left(90^{\circ}-\phi\right)$
有如下关系：
![]

page2 扩展卡尔曼滤波

概括如下：
$\tilde{P}_{k}=F_{k-1} \hat{P}_{k-1} F_{k-1}^{T}+Q_{k}^{\prime}$
$\tilde{x}_{k}=f\left(\hat{x}_{k-1}, v_{k}, 0\right)$
$K_{k}=\tilde{P}_{k} G_{k}^{T}\left(G_{k} \tilde{P}_{k} G_{k}^{T}+R_{k}^{\prime}\right)^{-1}$
$\hat{P}_{k}=\left(I-K_{k} G_{k}\right) \tilde{P}_{k}$ ,
$\hat{x}_{k}=\tilde{x}_{k}+K_{k}\left(y_{k}-g\left(\tilde{x}_{k}, 0\right)\right)$
其中，
$F_{k-1}=\left.\frac{\partial f}{\partial x_{k-1}}\right|_{\hat{x}_{k-1}}, G_{k}=\left.\frac{\partial h}{\partial x_{k}}\right|_{\tilde{x}_{k}}$
故而，下面整理了kalman滤波的普遍做法：

Labbe

$\begin{aligned} \overline{\mathbf x} &= \mathbf{Fx} + \mathbf{Bu} \\ \overline{\mathbf P} &= \mathbf{FPF}^\mathsf{T} + \mathbf Q \\ \\ \mathbf y &= \mathbf z - \mathbf{H}\overline{\mathbf x} \\ \mathbf S &= \mathbf{H}\overline{\mathbf P}\mathbf{H}^\mathsf{T} + \mathbf R \\ \mathbf K &= \overline{\mathbf P}\mathbf{H}^\mathsf{T}\mathbf{S}^{-1} \\ \mathbf x &= \overline{\mathbf x} +\mathbf{Ky} \\ \mathbf P &= (\mathbf{I}-\mathbf{KH})\overline{\mathbf P} \end{aligned}$

Wikipedia

$\begin{aligned} \hat{\mathbf x}_{k\mid k-1} &= \mathbf{F}_{k}\hat{\mathbf x}_{k-1\mid k-1} + \mathbf{B}_{k} \mathbf{u}_{k} \\ \mathbf P_{k\mid k-1} &= \mathbf{F}_{k} \mathbf P_{k-1\mid k-1} \mathbf{F}_{k}^{\textsf{T}} + \mathbf Q_{k}\\ \tilde{\mathbf{y}}_k &= \mathbf{z}_k - \mathbf{H}_k\hat{\mathbf x}_{k\mid k-1} \\ \mathbf{S}_k &= \mathbf{H}_k \mathbf P_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k \\ \mathbf{K}_k &= \mathbf P_{k\mid k-1}\mathbf{H}_k^\textsf{T}\mathbf{S}_k^{-1} \\ \hat{\mathbf x}_{k\mid k} &= \hat{\mathbf x}_{k\mid k-1} + \mathbf{K}_k\tilde{\mathbf{y}}_k \\ \mathbf P_{k|k} &= (I - \mathbf{K}_k \mathbf{H}_k) \mathbf P_{k|k-1} \end{aligned}$

Brookner

$\begin{aligned} X^*_{n+1,n} &= \Phi X^*_{n,n} \\ X^*_{n,n} &= X^*_{n,n-1} +H_n(Y_n - MX^*_{n,n-1}) \\ H_n &= S^*_{n,n-1}M^\mathsf{T}[R_n + MS^*_{n,n-1}M^\mathsf{T}]^{-1} \\ S^*_{n,n-1} &= \Phi S^*_{n-1,n-1}\Phi^\mathsf{T} + Q_n \\ S^*_{n-1,n-1} &= (I-H_{n-1}M)S^*_{n-1,n-2} \end{aligned}$

Gelb

$\begin{aligned} \underline{\hat{x}}_k(-) &= \Phi_{k-1} \underline{\hat{x}}_{k-1}(+) \\ \underline{\hat{x}}_k(+) &= \underline{\hat{x}}_k(-) +K_k[Z_k - H_k\underline{\hat{x}}_k(-)] \\ K_k &= P_k(-)H_k^\mathsf{T}[H_kP_k(-)H_k^\mathsf{T} + R_k]^{-1} \\ P_k(+) &= \Phi_{k-1} P_{k-1}(+)\Phi_{k-1}^\mathsf{T} + Q_{k-1} \\ P_k(-) &= (I-K_kH_k)P_k(-) \end{aligned}$

Brown

$\begin{aligned} \hat{\mathbf x}^-_{k+1} &= \mathbf{\phi}_{k}\hat{\mathbf x}_{k} \\ \hat{\mathbf x}_k &= \hat{\mathbf x}^-_k +\mathbf{K}_k[\mathbf{z}_k - \mathbf{H}_k\hat{\mathbf{}x}^-_k] \\ \mathbf{K}_k &= \mathbf P^-_k\mathbf{H}_k^\mathsf{T}[\mathbf{H}_k\mathbf P^-_k\mathbf{H}_k^T + \mathbf{R}_k]^{-1}\\ \mathbf P^-_{k+1} &= \mathbf{\phi}_k \mathbf P_k\mathbf{\phi}_k^\mathsf{T} + \mathbf Q_{k} \\ \mathbf P_k &= (\mathbf{I}-\mathbf{K}_k\mathbf{H}_k)\mathbf P^-_k \end{aligned}$

Zarcha

$\begin{aligned} \hat{x}_{k} &= \Phi_{k}\hat{x}_{k-1} + G_ku_{k-1} + K_k[z_k - H\Phi_{k}\hat{x}_{k-1} - HG_ku_{k-1} ] \\ M_{k} &= \Phi_k P_{k-1}\phi_k^\mathsf{T} + Q_{k} \\ K_k &= M_kH^\mathsf{T}[HM_kH^\mathsf{T} + R_k]^{-1}\\ P_k &= (I-K_kH)M_k \end{aligned}$
或者也可以表示如下：
![]