卡尔曼滤波 - 状态空间模型中的状态方程

flyfish

状态方程和观测方程统称为状态空间模型

 位移 = Δ x = x f − x 0 \text { 位移}=\Delta x=x_f-x_0  位移=Δx=xf​−x0​ x 0 x_0 x0​ 是起始位置 x f x_f xf​ 是终止位置 在坐标轴里，右边是正，左边是负

绿色矩形的高度为 v 0 v_0 v0​ 宽度为 t t t 所以面积等于 v 0 v_0 v0​ t t t

黄色三角形的底是 t t t 高度为 v − v 0 v-v_0 v−v0​ 黄色三角形的面积为 1 2 t ( v − v 0 ) \large \frac {1}{2} t (v - v_0) 21​t(v−v0​)

两者求和时，我们得到位移公式 Δ x = v 0 t + 1 2 t ( v − v 0 ) \large \Delta x = v_0 t + \frac {1}{2} t (v - v_0) Δx=v0​t+21​t(v−v0​)

加速度 （ 1 ） a = Δ v Δ t （1）\Large a = \frac {\Delta v}{\Delta t} （1）a=ΔtΔv​ 速度差，也就是三角形的高 （ 2 ） Δ v = v − v 0 （2）\Large \Delta v = v - v\normalsize{_0} （2）Δv=v−v0​ （2）式代入（1）式 a = v − v 0 Δ t \Large a = \frac {v - v_0}{\Delta t} a=Δtv−v0​​

v = v 0 + a Δ t \Large v = v_0 + \Large a \Delta t v=v0​+aΔt 也就是我们常见的 v = v 0 + a t \LARGE v = v_0 + at v=v0​+at

黄色和绿色左右分布也一样

x ( t ) x(t) x(t)是位置 x ˙ ( t ) \dot{x}(t) x˙(t)是速度 a = w ( t ) a=w(t) a=w(t)是加速度

x ( t ) = [ x ( t ) x ˙ ( t ) ] = [  位置   速度  ] \boldsymbol{x}(t)=\left[\begin{array}{l} x(t) \\ \dot{x}(t) \end{array}\right]=\left[\begin{array}{l} \text { 位置 } \\ \text { 速度 } \end{array}\right] x(t)=[x(t)x˙(t)​]=[ 位置  速度 ​]

x ( t ) = x 0 + v 0 t + 1 2 a t 2 {x}(t) = x_0 + v_0 t + \frac {1}{2} at^2 x(t)=x0​+v0​t+21​at2 (位置)

x ˙ ( t ) = v ( t ) = v 0 + a t \dot{x}(t)=v(t) =v_0+a t x˙(t)=v(t)=v0​+at （速度）

x ¨ ( t ) = v ˙ ( t ) = a ( t ) = a \ddot{x}(t)=\dot{v}(t)=a(t) =a x¨(t)=v˙(t)=a(t)=a （加速度）

t t t 和 t − 1 t-1 t−1 时刻之间的时间差为 Δ t \Delta t Δt { x t = x t − 1 + x ˙ t − 1 Δ t + 1 2 a Δ t 2 x ˙ t = x ˙ t − 1 + a Δ t \left\{\begin{array}{l} x_t=x_{t-1}+\dot{x}_{t-1} \Delta t+\frac{1}{2} a \Delta t^2 \\ \dot{x}_t=\dot{x}_{t-1}+a \Delta t \end{array}\right. {xt​=xt−1​+x˙t−1​Δt+21​aΔt2x˙t​=x˙t−1​+aΔt​

矩阵形式 [ x t x ˙ t ] = [ 1 Δ t 0 1 ] [ x t − 1 x ˙ t − 1 ] + [ Δ t 2 2 Δ t ] a \left[\begin{array}{l} x_t \\ \dot{x}_t \end{array}\right]=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} x_{t-1} \\ \dot{x}_{t-1} \end{array}\right]+\left[\begin{array}{c} \frac{\Delta t^2}{2} \\ \Delta t \end{array}\right] a [xt​x˙t​​]=[10​Δt1​][xt−1​x˙t−1​​]+[2Δt2​Δt​]a

A = [ 1 Δ t 0 1 ] , B = [ Δ t 2 2 Δ t ] A=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right], B=\left[\begin{array}{c} \frac{\Delta t^2}{2} \\ \Delta t \end{array}\right] A=[10​Δt1​],B=[2Δt2​Δt​] x t = A x t − 1 + B u t − 1 （状态方程 ) {x_t} = A{x_{t - 1}} + B{u_{t - 1}}（状态方程) xt​=Axt−1​+But−1​（状态方程) t t t 换成 k k k x k = A x k − 1 + B u k − 1 （状态方程 ) {x_k} = A{x_{k - 1}} + B{u_{k - 1}}（状态方程) xk​=Axk−1​+Buk−1​（状态方程)

x k = A x k − 1 + B u k − 1 + w k − 1 （加入随机项状态方程 ) {x_k} = A{x_{k - 1}} + B{u_{k - 1}} + {w_{k - 1}}（加入随机项状态方程) xk​=Axk−1​+Buk−1​+wk−1​（加入随机项状态方程)

其他式子 先验估计 − 代表先验，ˆ代表估计 x ^ k − = A x ^ k − 1 + B u k − 1 \hat{x}_{k}^{-}=A \hat{x}_{k-1}+B u_{k-1} x^k−​=Ax^k−1​+Buk−1​