首先，递归滤波器和kalman原理上滤波器有相似之处。 递归滤波器表示以下步骤kalman滤波器的想法相似。然而，随着时间的推移，卡尔曼滤波器采用的增益逐渐趋于稳定。当滤波器收敛时，系统理论上可以获得状态值的最佳后验估计。
s t e p 1 : 计算 k a l m a n 增益 : K k = e k ? 1 E S T e k ? 1 E S T e k M E A step1:计算kalman增益:K_k=\frac{e^{EST}_{k-1}}{e^{EST}_{k-1} e^{MEA}_k} step1:计算kalman增益:Kk=ek?1EST ekMEAek−1EST
s t e p 2 : 计算： x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) step2:计算：\hat x_k=\hat x_{k-1}+K_k(z_k-\hat x_{k-1}) step2:计算：x^k=x^k−1+Kk(zk−x^k−1)
e k E S T = ( 1 − K k ) e k − 1 E S T e^{EST}_k=(1-K_k)e^{EST}_{k-1} ekEST=(1−Kk)ek−1EST
首先，定义线性系统表示为：
x k = A x k − 1 + B u k − 1 + w k − 1 z k = H k − 1 + v k \begin{array}{l}{ {\rm{x}}_k} = A{x_{k - 1}} + B{u_{k - 1}} + {w_{k - 1}}\\{z_k} = {H_{k - 1}} + {v_k}\end{array} xk=Axk−1+Buk−1+wk−1zk=Hk−1+vk
误差可以表示为：
P ( w ) ∼ N ( 0 , Q ) P ( v ) ∼ N ( 0 , R ) \begin{array}{l} P(w) \sim N(0,Q)\\ P(v) \sim N(0,R) \end{array} 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 T + Q \begin{array}{l}\hat x_k^ - = A{ {\hat x}_{k - 1}} + B{u_{k - 1}}\\P_k^ - = A{P_{k - 1}}{A^T} + Q\end{array} x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q
更新方程表示为：
K k = P k − H T H P k − H T + R x ^ k = x ^ k − + K k ( z k − H x ^ k − ) P k = ( I − K k H ) P k − \begin{array}{l}{K_k} = \frac{ {P_k^ - {H^T}}}{ {HP_k^ - {H^T} + R}}\\{ {\hat x}_k} = \hat x_k^ - + {K_k}({z_k} - H\hat x_k^ - )\\{P_k} = (I - {K_k}H)P_k^ - \end{array} Kk=HPk−HT+RPk−HTx^k=x^k−+Kk(zk−Hx^k−)Pk=(I−KkH)Pk−
实际情况下，系统常常是非线性的。假设非线性系统表示为
x k = f ( x k − 1 , u k − 1 , w k − 1 ) z k = h ( x k , v k ) x_k=f(x_{k-1},u_{k-1},w_{k-1})\\z_k=h(x_k,v_k) xk=f(xk−1,uk−1,wk−1)zk=h(xk,vk)
设计非线性kalman滤波系统，首先需要对非线性系统进行线性化，其中线性化的点被称为**“operating point**”，通常认为选取真实点作为**“operating point**”是最合适的。但由于误差的存在真实值往往是未知的。因此采用泰勒展开的方式将更新矩阵在 x ^ k − 1 \hat x_{k-1} x^k−1处线性化，可以表示为**（其中 x ^ k − 1 \hat x_{k-1} x^k−1为k-1时刻的后验估计）**
x k = f ( x ^ k − 1 , u k − 1 , w k − 1 ) + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=f(\hat x_{k-1},u_{k-1},w_{k-1})+A(x_k-\hat x_{k-1})+W_kw_{k-1} xk=f(x^k−1,uk−1,wk−1)+A(xk−x^k−1)+Wkwk−1