2022-01-24 13:53:07
浏览数 (9)
卡尔曼滤波推导
我们先给出离散时间现行系统的状态、观测方程：
begin{array}{l}
x_{k}=A * x_{k-1} B * u_{k} w_{k-1} \
z_{k}=H * x_{k} v_{k}
end{array}
其中，x_k 表示状态矩阵，z_k 表示状态矩阵的观测量，A 表示状态转移矩阵，B 表示输入矩阵，其具体影响状态矩阵，H 表示观测矩阵，omega_{k-1} 表示过程噪声，v_k表示测量噪声。
测量噪声很容易理解，数据采集中都包含噪声，这个噪声就是测量噪声。对于过程噪声，大部分线性时变系统并不严格，具有一定不确定性，会导致状态矩阵有一定偏差，因而存在omega_{k 1} 。两种噪声均建模为高斯白噪声，服从p(omega) in N(0, Q),p(v) in N(0, R) ，为对应的协方差矩阵。Q,R 在此，定义三种状态矩阵的表示方式：
- x_k 表示第k次状态真实值；$
- hat{x}_{k mid k-1} 表示第k-1次测量后对第k次的估计值，即先验状态估计值；
- hat{x}_{k mid k} 表示第k次测量后对第k次的估计值，即后验状态估计值；
先验状态估计值由状态预测方程得到：hat{x}_{k mid k-1}=A*hat{x}_{k-1 mid k-1} B*u_k 后验状态估计值由状态更新方程得到（第二种加法表达式）：hat{x}_{k mid k}=hat{x}_{k mid k-1} K(z_k-H*hat{x}_{k mid k-1}) 误差e_{k mid k-1} ：
begin{aligned}
e_{k mid k-1} &=x_{k}-hat{x}_{k mid k-1} \
&=left(A x_{k-1} B u_{k-1} omega_{k-1}right)-left(A hat{x}_{k-1 mid k-1} B u_{k-1}right) \
&=Aleft(x_{k-1}-hat{x}_{k-1 mid k-1}right) omega_{k-1}
end{aligned}
误差e_{k mid k} ：
begin{aligned}
e_{k mid k} &=x_{k}-hat{x}_{k mid k} \
&=x_{k}-left(hat{x}_{k mid k-1} Kleft(z_{k}-H hat{x}_{k mid k-1}right)right) \
&=x_{k}-left[hat{x}_{k mid k-1} Kleft(H x_{k} v_{k}-H hat{x}_{k mid k-1}right)right] \
&=x_{k}-hat{x}_{k mid k-1}-K Hleft(x_{k}-hat{x}_{k mid k-1}right)-K v_{k} \
&=left(I-K Hright) e_{k mid k-1}-K v_{k}
end{aligned}
先验协方差：
begin{aligned}
P_{k mid k-1}=Eleft(e_{k mid k-1} e_{k mid k-1}^{T}right)
&=Eleft[left(x_{k}-hat{x}_{k mid k-1}right)left(x_{k}-hat{x}_{k mid k-1}right)^{T}right] \
&=Eleft{left[Aleft(x_{k-1}-hat{x}_{k-1 mid k-1}right) omega_{k-1}right]left[Aleft(x_{k-1}-hat{x}_{k-1 mid k-1}right) omega_{k-1}right]^{T}right} \
&=A Eleft[left(x_{k-1}-hat{x}_{k-1 mid k-1}right)left(x_{k-1}-hat{x}_{k-1 mid k-1}right)^{T}right] A^{T} Eleft[omega_{k-1} omega_{k-1}^{T}right] \
&=A P_{k-1 mid k-1} A^{T} Q
end{aligned}
后验协方差：
begin{aligned}
P_{k mid k} &=Eleft[e_{k mid k} e_{k mid k}^{T}right] \
&=Eleft{left[left(I-KHright) e_{k mid k-1}-K v_{k}right]left[left(I-K Hright) e_{k mid k-1}-K v_{k}right]^{T}right} \
&=Eleft{left[left(I-KHright) e_{k mid k-1}-K v_{k}right]left[e_{k mid k-1}^{T}left(I-K Hright)^{T}-v_{k}^{T} K^{T}right]right} \
&=Eleft[left(I-K Hright) e_{k mid k-1} e_{k mid k-1}^{T}left(I-KHright)^{T} K v_{k} v_{k}^{T} K^{T}right] \
&=left(I-K Hright) P_{k mid k-1}left(I-KHright)^{T} KR K^{T} \
&=P_{k mid k-1}-K H P_{k mid k-1}-P_{k mid k-1} H^{T} K^{T} K H P_{k mid k-1} H^{T} K^{T} K R K^{T}
end{aligned}
卡尔曼代价函数为P_{k mid k} ，即：J=sum_{min } P_{k} ，对K 求导：frac{partial P_{k mid k}}{partial K}=-2left(P_{k mid k-1} H^{T}right) 2 Kleft(H P_{k mid k-1} H^{T} Rright) 令导数为0，可以得卡尔曼增益K 为：K=P_{k mid k-1}H^T(HP_{k mid k-1}H^T R)^{-1} 将其代入公式7中，可得：P_{k mid k}=left(I-K Hright) P_{k mid k-1} 至此，完成所有公式的推导，预测即先验，更新即后验。
