状态方程
状态变量 $x$ 满足, 其中 u为控制变量：
$$ x = A x_prev + B u_prev + w_prev $$ (1)
观测变量 $z 满足：
$$ z = H x + v $$ (2)
随机信号 $w$ 和 $v$ 分别表示过程激励噪声和观测噪声，假定相互独立，且服从正态分布。
定义状态变量的先验估计 $x^-$， 即基于之前状态对当前状态的预测值； 定义后验估计 $x^$，即已知当前观测值所计算的当前状态变量。 定义先验误差 $e^-$, 后验误差 $e^$, 满足：
$$ e^- = x - x^- $$ (3)
$$ e^ = x - x^ $$ (4)
卡尔曼滤波表示： 后验估计（观测值所推导的状态变量值）与先验估计（预测的状态变量值）的差异，可表示为观测值与以先验估计为输入的观测值的差异的增益 K。
$$ x^ - x^- = K ( z - H x^- ) $$ (5)
K可由先验误差的协方差 P、观测噪声的协方差R 和观测增益H表示:
$$ K = P^- H^t / ( HP^-H^t + R ) $$ (6)
可见：
1） 当R 趋于0时， k 趋近于 h 的逆，此时 x^ = x。即当观测误差很小，观测值趋近真实值。
2） 当P趋于0时，即预测值趋近真实值。
算法设计
卡尔曼滤波器用反馈控制估计过程状态（变量）： 滤波器估计某一时刻的状态（时间更新/预估），然后以（含噪声的）测量变量获得反馈（测量更新/校政）。
时间更新，当前时步状态先验估计 x^- 及先验误差协方差近似P^-:
$$ x^- = A x^-_prev + B u_prev $$ (7.1)
$$ P^- = A P^_prev A^t + Q $$ (7.2)
其中 $ P(w) ~ N(0, Q) $
测量更新，使用(6)更新卡尔曼增益K, 使用（5)更新后验状态变量x^和当前步先验误差协方差值 P^：
$$ P = ( I - KH ) P^- $$ (8)
控制器调参
测量误差一般可观测得到；而过程误差q需要通过与一个已知误差的在线滤波器对比调整系数。调参一般是离线过程。一般当过程误差和卡尔曼增益会快速收敛并保持常数。但测量误差受环境影响不易保持不变。
扩展卡尔曼
当观测值与系统状态变量 或 系统本身是非线性关系，方程(1), (2)变非线性函数。
$$ x = f(x_prev, u_prev, w) $$ (1.2)
$$ z = h(x, v) $$ (2.2)