卡尔曼滤波 - 状态空间模型

flyfish

状态空间模型是什么和如何得到的状态空间模型
状态方程和观测方程统称为状态空间模型
状态空间模型如下

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（状态方程)

zk=Hxk+vk{z_k} = H{x_k} + {v_k}zk=Hxk+vk观测方程
还可以这样表示只是字母不同
x(t+1)=Φx(t)+Γw(t)\boldsymbol{x}(t+1)=\boldsymbol{\Phi} \boldsymbol{x}(t)+\boldsymbol{\Gamma} w(t) \quadx(t+1)=Φx(t)+Γw(t) （状态方程)
y(t)=Hx(t)+v(t)y(t)=\boldsymbol{H} \boldsymbol{x}(t)+v(t) \quady(t)=Hx(t)+v(t) （观测方程 )
以物理运动定律的方式来理解状态空间模型
s(t+1)=s(t)+s˙(t)T+T22w(t)s˙(t+1)=s˙(t)+Tw(t)y(t)=s(t)+v(t)\begin{array}{c} s(t+1)=s(t)+\dot{s}(t) T+\frac{T^{2}}{2} w(t) \\ \dot{s}(t+1)=\dot{s}(t)+T w(t) \\ y(t)=s(t)+v(t) \end{array} s(t+1)=s(t)+s˙(t)T+2T2w(t)s˙(t+1)=s˙(t)+Tw(t)y(t)=s(t)+v(t)
TTT是采样周期
s(t)s(t)s(t)是位置
s˙(t)\dot{s}(t)s˙(t)是速度
w(t)w(t)w(t)是加速度
y(t)y(t)y(t)是在位置s(t)的观测信号
v(t)v(t)v(t)是观测噪声

定义系统的状态变量
x(t)=[s(t)s˙(t)]=[位置速度 ]\boldsymbol{x}(t)=\left[\begin{array}{l} s(t) \\ \dot{s}(t) \end{array}\right]=\left[\begin{array}{l} \text { 位置 } \\ \text { 速度 } \end{array}\right] x(t)=[s(t)s˙(t)]=[ 位置速度 ]
状态方程和观测方程
[s(t+1)s˙(t+1)]=[1T01][s(t)s˙(t)]+[T22T]w(t)y(t)=[10][s(t)s˙(t)]+v(t)\begin{array}{l} {\left[\begin{array}{l} s(t+1) \\ \dot{s}(t+1) \end{array}\right]=\left[\begin{array}{ll} 1 & T \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} s(t) \\ \dot{s}(t) \end{array}\right]+\left[\begin{array}{c} \frac{T^{2}}{2} \\ T \end{array}\right] w(t)} \\ y(t)=\left[\begin{array}{ll} 1 & 0 \end{array}\right]\left[\begin{array}{l} s(t) \\ \dot{s}(t) \end{array}\right]+v(t) \end{array} [s(t+1)s˙(t+1)]=[10T1][s(t)s˙(t)]+[2T2T]w(t)y(t)=[10][s(t)s˙(t)]+v(t)
得到状态空间模型
x(t+1)=Φx(t)+Γw(t)\boldsymbol{x}(t+1)=\boldsymbol{\Phi} \boldsymbol{x}(t)+\boldsymbol{\Gamma} w(t) \quadx(t+1)=Φx(t)+Γw(t) （状态方程)
y(t)=Hx(t)+v(t)y(t)=\boldsymbol{H} \boldsymbol{x}(t)+v(t) \quady(t)=Hx(t)+v(t) （观测方程 )))
Φ=[1T01],Γ=[0.5T2T],H=[10]\boldsymbol{\Phi}=\left[\begin{array}{ll} 1 & T \\ 0 & 1 \end{array}\right], \quad \boldsymbol{\Gamma}=\left[\begin{array}{c} 0.5 T^{2} \\ T \end{array}\right], \quad \boldsymbol{H}=[1 \quad 0] \\ Φ=[10T1],Γ=[0.5T2T],H=[10]

从控制理论来理解状态空间模型

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

zk=Hxk+vk{z_k} = H{x_k} + {v_k}zk=Hxk+vk

这两行式子怎么来的呢

系统动态方程是

mx¨+bx˙+kx=F设 x1=x,x2=x˙,u=F则 x˙2=x¨=1mu−bmx˙−kmx=1mu−bmx2−kmx1\begin{array}{c} m \ddot{x}+b \dot{x}+k x=F \\ \text { 设 } x_{1}=x, \\ x_{2}=\dot{x},\\ u=F \text { 则 } \\ \qquad \begin{array}{r} \dot{x}_{2}=\ddot{x}=\frac{1}{m} u-\frac{b}{m} \dot{x}-\frac{k}{m} x \\ =\frac{1}{m} u-\frac{b}{m} x_{2}-\frac{k}{m} x_{1} \end{array} \end{array} mx¨+bx˙+kx=F 设 x1=x,x2=x˙,u=F 则 x˙2=x¨=m1u−mbx˙−mkx=m1u−mbx2−mkx1

假设

位置测量量是 z1=x=x1,z_{1}=x=x_{1}, \quadz1=x=x1,

速度测量量是 z2=x˙=x2z_{2}=\dot{x}=x_{2}z2=x˙=x2, 则:
[x˙1x˙2]=[01−km−bm][x1x2]+[01m]u[z1z2]=[1001][x1x2]\begin{array}{r} {\left[\begin{array}{c} \dot{x}_{1} \\ \dot{x}_{2} \end{array}\right]=\left[\begin{array}{cc} 0 & 1 \\ -\frac{k}{m} & -\frac{b}{m} \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \end{array}\right]+\left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right] u} \\ {\left[\begin{array}{l} z_{1} \\ z_{2} \end{array}\right]=\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \end{array}\right]} \end{array} [x˙1x˙2]=[0−mk1−mb][x1x2]+[0m1]u[z1z2]=[1001][x1x2]

状态空间表达式增加随时间维度
x˙(t)=Ax(t)+Bu(t)z(t)=Hx(t)\begin{array}{c} \dot{\boldsymbol{x}}(t)=A \boldsymbol{x}(t)+B u(t) \\ \boldsymbol{z}(t)=H \boldsymbol{x}(t) \\ \end{array} x˙(t)=Ax(t)+Bu(t)z(t)=Hx(t)
状态空间表达式将连续的时间更改为离散的时间
xk=Axk−1+Bukzk=Hxk\begin{array}{c} \boldsymbol{x}_{k}=A \boldsymbol{x}_{k-1}+B u_{k} \\ \boldsymbol{z}_{k}=H \boldsymbol{x}_{k} \end{array} xk=Axk−1+Bukzk=Hxk
状态空间表达式增加噪声维度（Linear state space system）
xk=Axk−1+Buk+wk−1zk=Hxk+vk\begin{array}{c} \boldsymbol{x}_{k}=A \boldsymbol{x}_{k-1}+B u_{k}+\boldsymbol{w}_{k-1} \\ \boldsymbol{z}_{k}=H \boldsymbol{x}_{k}+\boldsymbol{v}_{k} \end{array} xk=Axk−1+Buk+wk−1zk=Hxk+vk

从速度控制来理解状态方程

连续系统
x˙(t)=Ax(t)+Bu(t)\begin{array}{c}\dot{\boldsymbol{x}}(t)=A \boldsymbol{x}(t)+B u(t)\\\end{array} x˙(t)=Ax(t)+Bu(t)
离散系统

xk=Axk−1+Buk\boldsymbol{x}_{k}=A \boldsymbol{x}_{k-1}+B u_{k} xk=Axk−1+Buk

[posk+1velk+1]=[a11a12a21a22][poskvelk]+[0b2]u\left[\begin{array}{c} p o s_{k+1} \\ v e l_{k+1} \end{array}\right]=\left[\begin{array}{ll} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right]\left[\begin{array}{c} p o s_{k} \\ v e l_{k} \end{array}\right]+\left[\begin{array}{l} 0 \\ b_{2} \end{array}\right] u [posk+1velk+1]=[a11a21a12a22][poskvelk]+[0b2]u

下一时刻的位置

与当前时刻的位置有关 a11a_11a11

与当前的速度有关 a12a_12a12

与当前的输入无关 b1=0b_1=0b1=0

下一时刻的速度

与当前时刻的位置有关 a21a21a21

与当前的速度有关 a22a22a22

与当前的输入有关 b2b2b2
A=[a11a12a21a22]=[11−0.2−0.21]A=\left[\begin{array}{ll} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right]= \left[\begin{array}{cc} 1 & 1-0.2 \\ -0.2 & 1 \end{array}\right] A=[a11a21a12a22]=[1−0.21−0.21]
下一时刻位置是当前位置+0.8*v

下一时刻速度是当前速度-0.2*p

从实际数值例子来理解

u(t)u(t)u(t)输入量

x˙(t)\dot{x}(t)x˙(t)是输出量
下面的x^\widehat {x}x头上本来写的是三个点，但是这个解析不了所以戴了个帽子
x^+5x¨+3x˙+2x=u\widehat {x}+5 \ddot{x}+3 \dot{x}+2 x=u x+5x¨+3x˙+2x=u

设

x1=x,x2=x˙,x3=x¨x_{1}=x, x_{2}=\dot{x}, x_{3}=\ddot{x} x1=x,x2=x˙,x3=x¨
除最高阶数x^\widehat xx之外，其余各个微分项xix_ixi都用替代。
{x˙1=x˙=x2x˙2=x¨=x3x˙3=x^=−5x3−3x2−2x1+u\left\{\begin{array}{l} \dot{x}_{1}=\dot{x}=x_{2} \\ \dot{x}_{2}=\ddot{x}=x_{3} \\ \dot{x}_{3}=\widehat x=-5 x_{3}-3 x_{2}-2 x_{1}+u \end{array}\right. ⎩⎨⎧x˙1=x˙=x2x˙2=x¨=x3x˙3=x=−5x3−3x2−2x1+u
yyy表示输出变量,只有一个输出量x˙(t)\dot{x}(t)x˙(t)

y=x2=x˙y=x_2=\dot{x}y=x2=x˙

变成矩阵形式
[x˙1x˙2x˙3]=[010001−2−3−5][x1x2x3]+[001]u[y]=[010][x1x2x3]+[0]u\begin{array}{l} {\left[\begin{array}{c} \dot{x}_{1} \\ \dot{x}_{2} \\ \dot{x}_{3} \end{array}\right]=\left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & 0 & 1 \\ -2 & -3 & -5 \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \\ x_{3} \end{array}\right]+\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right] u} \\ {[y]=\left[\begin{array}{lll} 0 & 1 & 0 \end{array}\right]\left[\begin{array}{l} x_{1} \\ x_{2} \\ x_{3} \end{array}\right]+[0] u} \end{array} x˙1x˙2x˙3=00−210−301−5x1x2x3+001u[y]=[010]x1x2x3+[0]u
即
x⃗˙=Ax⃗+Bu⃗y⃗=Cx⃗+Du⃗\begin{array}{l} \dot{\vec{x}}=A \vec{x}+B \vec{u} \\ \vec{y}=C \vec{x}+D \vec{u} \end{array} x˙=Ax+Buy=Cx+Du