对于线性系统，状态方程和观测方程如下：
X k = A X k − 1 + B u k − 1 + W k − 1 Z k = H k − 1 + V k \begin{aligned}&X_{k}=A X_{k-1}+B u_{k-1}+W_{k-1} \\&Z_{k}=H_{k-1}+V_{k}\end{aligned} Xk=AXk−1+Buk−1+Wk−1Zk=Hk−1+Vk
其中
P ( w ) ∼ N ( 0 , Q ) P ( v ) ∼ N ( 0 , R ) \begin{aligned}&P(w) \sim N(0, Q) \\&P(v) \sim N(0, R)\end{aligned} P(w)∼N(0,Q)P(v)∼N(0,R)
预测方程
x ^ k − = A x ^ k + 1 + B u k − 1 P k − = A P k − 1 A ⊤ + Q \begin{aligned}&\hat{x}_{k}^{-}=A \hat{x}_{k+1}+B u_{k-1} \\&P_{k}^{-}=A P_{k-1} A^{\top}+Q\end{aligned} x^k−=Ax^k+1+Buk−1Pk−=APk−1A⊤+Q
校正方程
k k = P k − H ⊤ H P k − H ⊤ + R x ^ k = x ^ k − k k ( z k − H x ^ k − ) P k = ( I − k k H ) P k \begin{aligned}k_{k} &=\frac{P_{k}^{-} H^{\top}}{H P_{k}-H^{\top}+R} \\\hat{x}_{k} &=\hat{x}_{k}-k_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right) \\P_{k} &=\left(I-k_{k} H\right) P_{k} \end{aligned} kkx^kPk=HPk−H⊤+RPk−H⊤=x^k−kk(zk−Hx^k−)=(I−kkH)Pk
对于非线性系统，有以下表达形式：
x k = f ( x k − 1 , ω k − 1 , w k − 1 ) z k = h ( x k , v k ) \begin{aligned}&x_{k}=f\left(x_{k-1}, \omega_{k-1}, w_{k-1}\right) \\&z_{k}=h\left(x_{k}, v_{k}\right)\end{aligned} xk=f(xk−1,ωk−1,wk−1)zk=h(xk,vk)
f, h 为非线性函数
正态分布的随机变量通过非线性系统后就不再是正态分布了
如果对于以上系统使用卡尔曼滤波，则需进行线性化（泰勒线性近似）
采用方法：泰勒展开
f ( x ) = f ( λ 0 ) + ∂ f ∂ x ( x − x 0 ) f(x)=f\left(\lambda_{0}\right)+\frac{\partial f}{\partial x}\left(x-x_{0}\right) f(x)=f(λ0)+∂x∂f(x−x0)
以上展开式对于二维函数采用求导方式，对于高维函数采用求 Jacobian 矩阵方式
（这部分参考 DR_CAN 的工程数学基础——线性化与泰勒级数）
已知问题：系统有误差，无法在真实点线性化
因此，过程方程函数 f ( x k ) f(x_{k}) f(xk)在 x ^ k − 1 \hat{x}_{k-1} x^k−1（k-1处的后验估计）处线性化
x k = f ( x ^ k − 1 , u k − 1 , ω k − 1 ) + A ( x k − x ^ k − 1 ) + ω k ω k − 1 x_{k}=f\left(\hat{x}_{k-1}, u_{k-1}, \omega_{k-1}\right)+A\left(x_{k}-\hat{x}_{k-1}\right)+\omega_{k} \omega_{k-1} xk=f(x^k−1,uk−1,ωk−1)+A(xk−x^k−1)+ωkωk−1
ω k − 1 \omega_{k-1} ωk−1为误差项，此处假设为0
令 f ( x ^ k − 1 , u k − 1 , 0 ) = x ~ k f(\hat{x}_{k-1},u_{k-1},0)=\tilde{x}_{k} f(x^k−1,uk−1,0)=x~k
A = ∂ f ∂ x ∣ x ^ k − 1 , u k − 1 A=\frac{\partial f}{\partial x} \mid \hat{x}_{k-1}, u_{k-1} A=∂x∂f∣x^k−1,uk−1（求 k-1 点处的 Jacobian 矩阵）
A矩阵随k的变化而不断变化
对于噪声方程：
W k = ∂ f ∂ w ∣ x ^ k − 1 u k − 1 W_{k}=\frac{\partial f}{\partial w} \mid \hat{x}_{k-1} u_{k-1} Wk=∂w∂f∣x^k−1uk−1
观测函数 z k z_{k} zk在 x ~ k \tilde{x}_{k} x~k处线性化
z k = h ( x ~ k , v k ) + H ( x k − x ~ k ) + v v k z_{k}=h\left(\tilde{x}_{k}, v_{k}\right)+H\left(x_{k}-\tilde{x}_{k}\right)+v_{v_{k}} zk=h(x~k,vk)+H(xk−x~k)+vvk
v k v_{k} vk为误差项，此处假设为0
h ( x ~ k 2 , 0 ) = z ~ k 2 h\left(\tilde{x}_{k}^{2}, 0\right)=\tilde{z}_{k}^{2} h(x~k2,0)=z~k2
H = ∂ h ∂ x ∣ x ~ k V = ∂ h ∂ v ∣ x ~ k \begin{aligned}&H=\frac{\partial h}{\partial x} \mid \tilde{x}_{k} \\&V=\frac{\partial h}{\partial v} \mid \tilde{x}_{k}\end{aligned} H=∂x∂h∣x~kV=∂v∂h∣x~k
将 x k x_{k} xk和 z k z_{k} zk非线性系统函数在 k-1 处线性化，得出
X k = x ~ k + A ( x k − x ^ k − 1 ) + W ω k − 1 Z k = z ~ k + H ( x k − x ~ k ) + V v k \begin{aligned}&X_{k}=\tilde{x}_{k}+A\left(x_{k}-\hat{x}_{k-1}\right)+W \omega_{k-1} \\&Z_{k}=\tilde{z}_{k}+H\left(x_{k}-\tilde{x}_{k}\right)+V v_{k}\end{aligned} Xk=x~k+A(xk−x^k−1)+Wωk−1Zk=z~k+H(xk−x~k)+Vvk
对于噪声函数：
P ( ω ) ∼ N ( 0 , Q ) P ( ω ω ) ∼ N ( 0 , ω Q ω ⊤ ) P(\omega) \sim N(0, Q) \\P\left(\omega_{\omega}\right) \sim N\left(0, \omega Q \omega^{\top}\right) P(ω)∼N(0,Q)P(ωω)∼N(0,ωQω⊤)
综上所述，对于非线性系统来说
非线性预测方程为
x ^ k − = f ( x k − 1 2 , u k − 1 , 0 ) P k − = A P k − 1 A ⊤ + W Q ω ⊤ \begin{aligned}&\hat{x}_{k}^{-}=f\left(x_{k-1}^{2}, u_{k-1}, 0\right) \\&P_{k}^{-}=A P_{k-1} A^{\top}+W Q \omega^{\top}\end{aligned} x^k−=f(xk−12,uk−1,0)Pk−=APk−1A⊤+WQω⊤
非线性校正方程为
k
k
=
P
k
−
H
⊤
H
P
k
−
H
⊤
+
V
R
V
⊤
x
^
k
=
x
^
k
−
+
k
k
(
z
k
−
h
(
x
^
k
,
0
)
)
P
k
=
(
I
−
k
k
H
)
P
k
−
\begin{aligned}k_{k} &=\frac{P_{k}^{-} H^{\top}}{H P_{k}-H^{\top}+V R V^{\top}} \\\hat{x}_{k} &=\hat{x}_{k}^{-}+k_{k}\left(z_{k}-h\left(\hat{x}_{k}, 0\right)\right) \\P_{k} &=\left(I-k_{k} H\right) P_{k}^{-}\end{aligned}
kkx^kPk=HPk−H⊤+VRV⊤Pk−H⊤=x^k−+kk(zk−h(x^k,0))=(I−kkH)Pk−
323
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
