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

flyfish

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

位移

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

面积等于物体的位移

绿色矩形的高度为v0v_0v0​
宽度为ttt
所以面积等于v0v_0v0​ttt

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

两者求和时，我们得到位移公式
Δx=v0t+12t(v−v0)\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−v0（2）\Large \Delta v = v - v\normalsize{_0}（2）Δv=v−v0​
（2）式代入（1）式
a=v−v0Δt\Large a = \frac {v - v_0}{\Delta t}a=Δtv−v0​​

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

2. 简化位移公式
   Δx=v0t+12at2\large \Delta x = v_0 t + \frac {1}{2} at^2Δx=v0​t+21​at2

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

定义系统的状态变量

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)=x0+v0t+12at2{x}(t) = x_0 + v_0 t + \frac {1}{2} at^2x(t)=x0​+v0​t+21​at2 (位置)

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

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

ttt 和 t−1t-1t−1 时刻之间的时间差为 Δt\Delta tΔt
{xt=xt−1+x˙t−1Δt+12aΔt2x˙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​

矩阵形式
[xtx˙t]=[1Δt01][xt−1x˙t−1]+[Δt22Δ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Δt01],B=[Δt22Δ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​]
xt=Axt−1+But−1（状态方程){x_t} = A{x_{t - 1}} + B{u_{t - 1}}（状态方程)xt​=Axt−1​+But−1​（状态方程)
ttt 换成 kkk
xk=Axk−1+Buk−1（状态方程){x_k} = A{x_{k - 1}} + B{u_{k - 1}}（状态方程)xk​=Axk−1​+Buk−1​（状态方程)

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

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