状态方程和观测方程如下：
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,u