状态估计之kalman滤波

？
求现在时刻的 状态值 滤波
？
时间k
求将来时间段某 一点的状态值 预测
平滑、滤波和预测
2012/6/6 11
5.1 卡尔曼滤波
二、卡尔曼滤波算法推导
⎧ Xk = Φk ,k −1 Xk −1 + Γ k ,k −1Wk −1 ⎨ ⎩Zk = Hk Xk +Vk
W （1）为推导简化，假设 Φ, H 不随时间变化， k ,Vk 都是均值
K 其中， k 为增益矩阵，实质是一加权矩阵。
校正后的状态变量的估计误差用 X k 表示，误差的均方值用
Pk 表示，未校正的状态变量估计误差的均方值用 Pk '表示，即
ˆ ˆ ˆ X k = X k − X k , Pk = E ⎡( X k − X k )( X k − X k )T ⎤ ⎣ ⎦
ˆ ˆ Pk ' = E ⎡( X k − X k ')( X k − X k ')T ⎤ ⎣ ⎦
系 统 初始 条 件 为 ：
E [ X 0 ] = μ0 ⎡( X - μ0 )( X - μ0 )T ⎦ = p0 ⎤ var [ X 0 ] = E ⎣ cov [ X 0 ,Wk ] = E ⎡ X 0WkT ⎤ = 0 ⎣ ⎦ cov [ X 0 ,Vk ] = E ⎡ X 0VkT ⎤ = 0 ⎣ ⎦
2012/6/6 16
5.1 卡尔曼滤波
ˆ X k = ( I − K k H k ) ⎡Φk ( X k −1 − X k −1 ) + Wk −1 ⎤ − K kVk ⎣ ⎦
X k 由三部分组成，可记为
Xk = a + b − c
其中
ˆ a = ( I − K k H k )Φk ( X k −1 − X k −1 ) b = ( I − K k H k )Wk −1 c = K kVk
ˆ E ⎡ba T ⎤ = E ⎡ ( I − K k H k )Wk −1 ( X k −1 − X k −1 ) T ΦkT ( I − K k H k ) T ⎤ ⎣ ⎦ ⎣ ⎦
T E ⎡bc T ⎤ = E ⎡( I − K k H k )Wk −1VkT K k ⎤ ⎣ ⎦ ⎣ ⎦
ˆ E ⎡ca T ⎤ = E ⎡ K kVk ( X k −1 − X k −1 )T ΦkT ( I − K k H k )T ⎤ ⎣ ⎦ ⎣ ⎦ E ⎡cb T ⎤ = E ⎡ K kVkWkT ( I − K k H k )T ⎤ -1 ⎣ ⎦ ⎣ ⎦
ˆ ˆ = Φk X k −1 + K k ( H k X k + Vk − H kΦk X k −1 )
ˆ = ( I − K k H k )Φk X k −1 + K k H k X k + K kVk
ˆ Xk = Xk − Xk
X k = Φk X k −1 + Wk −1
ˆ = Φk X k −1 + Wk −1 − ⎡( I − K k H k )Φk X k −1 + K k H k X k + K kVk ⎤ ⎣ ⎦
ˆ 设 X k , j 表示利用输出zj 来对X进行估计的值，分三种情况：
• • •
Yj
当 k>j 时，称为预测； 当 k=j 时，称为滤波； 当 k<j 时，称为平滑。
2012/6/6
10
5.1 卡尔曼滤波
已知 过去 ←--- 现在时刻 ---→ 将来
？
求过去时间段某 一点的状态值
数据平滑、插 值、拟合
1. 连续型状态空间模型
● SISO线性时不变连续系统状态空间表达式为：
确 定 性 状 态 空 间 模 型
⎧ x (t ) = Ax (t ) + b u (t ) ⎨ ⎩ y (t ) = cx (t )
● 对应状态空间模型的传递函数为：
G(s) = c(sI − A)−1 b
2. 离散型状态空间模型
ˆ ˆ E ⎡ aaT ⎤ =E ⎡( I − K k H k )Φk ( X k −1 − X k −1 )( X k −1 − X k −1 )T ΦkT ( I − K k H k )T ⎤ ⎣ ⎦ ⎣ ⎦ =( I − K k H k )Φk Pk -1ΦkT ( I − K k H k )T E ⎡bbT ⎤ =E ⎡( I − K k H k )Wk −1WkT ( I − K k H k )T ⎤ -1 ⎣ ⎦ ⎣ ⎦ = ( I − K k H k )Qk -1 ( I − K k H k )T E ⎡ccT ⎤ =E ⎡ K kVkVkT K kT ⎤ = K k Rk K kT ⎣ ⎦ ⎣ ⎦
2012/6/6
4
5.1 卡尔曼滤波
卡尔曼滤波的应用
• 机器人导航、控制 • 传感器数据融合 • 雷达系统以及导弹追踪 • 计算机图像处理 – 头脸识别 – 图像分割 – 图像边缘检测
Apollo Navigation
目标跟踪
2012/6/6 5
5.1 卡尔曼滤波
比如，在雷达中，人们感兴趣的是跟踪目标，但目标
为零的高斯白噪声随机过程向量序列，且两者互不相关，即
2 ⎡ ⎤ ⎡ ⎤ Wk：E [Wk ] = 0，cov ⎣Wk , W j ⎦ = E ⎣WkW jT ⎦ = Qk δ kj，σ W = Qk
Vk： E [Vk ] = 0， cov ⎡Vk , V j ⎤ = E ⎡VkV jT ⎤ = Rk δ kj , ⎣ ⎦ ⎣ ⎦ cov ⎡Wk , V j ⎤ = E ⎡WkV jT ⎤ = 0, ⎣ ⎦ ⎣ ⎦ k , j = 0,1, 2…
则状态变量的估计误差的均方值 Pk 由9项组成。
⎡ ⎤ Pk = E ⎣ X k X kT ⎦ = E ⎡(a + b − c)(a + b − c)T ⎤ ⎣ ⎦
= E ⎡ aa T + bb T + cc T + ab T − ac T + ba T − bc T − ca T − cbT ⎤ ⎣ ⎦
2012/6/6
13
5.1 卡尔曼滤波
（2）基本思想：先不考虑输入信号 Wk 和观测噪声 Vk 的影响,
得到状态变量和输出信号（即观测数据）的估计值，再用输出信 号的估计误差加权后校正状态变量的估计值，使状态变量估计误 差的均方值最小。卡尔曼滤波的关键是计算加权矩阵的最佳值。
（3）推导过程
当不考虑 Wk 和 Vk 时，状态方程和量测方程为
2 σ V = Rk
式中 Qk＝ var [Wk ] =E ⎡WkWkT ⎤ 称为 动态 噪声协方差阵， ⎣ ⎦ Rk＝ var [Vk ] = E ⎡VkVkT ⎤ 称为 测量 噪声协方差阵。 ⎣ ⎦
2012/6/6
另外 δ kj = ⎨
⎧1 ⎩0
k= j k≠ j
克罗尼克 δ 函数
12
5.1 卡尔曼滤波
2012/6/6 17
5.1 卡尔曼滤波
ˆ 因为： a = ( I − K k H k )Φk ( X k −1 − X k −1 ) b = ( I − K k H k )Wk −1 ,
b T =WkT ( I − K k H k )T , -1
c = K kVk
c T =VkT K kT
ˆ a T =( X k −1 − X k −1 )T ΦkT ( I − K k H k )T 所以：
线性时不变 (LTI) 系统 LTI系统的输入u为离散控制量，采样周期为T，采样期间零阶保持， 则采样保持期间的系统可以计算得到：
Π
其中：
Π
进而：
2012/6/6
Π
8
5.1 卡尔曼滤波
● 针对随机线性时不变离散系统，用状态方程描述如下：
⎧ X k = Φk ,k −1 X k −1 + Γ k ,k −1Wk −1 状态(转移)方程 ⎨ 观测方程 ⎩Zk = Hk X k + Vk
卡尔曼滤波的任务：选择合适的 K k ，使 Pk 取得最小值。
2012/6/6 15
5.1 卡尔曼滤波
ˆ 下边推导 X k 和 X k ，然后计算 X k 的均方值 Pk ，简化 Pk ，最后得到一组卡尔曼滤波的递推公式。
ˆ ˆ ˆ X k = Φk X k −1 + K k ( Z k − H kΦk X k −1 )
ˆ ˆ X k ' = Φk X k −1 ˆ ˆ ˆ Z k ' = H k X k ' = H kΦk X k −1
际值有误差）
2012/6/6
⎧ X k = Φk X k −1 + Wk −1 ⎨ ⎩ Z k = H k X k + Vk
输出信号的误差为 Z （由于不考虑噪声，输出信号估计值与实 k
第5章 状态估计
5.1
卡尔曼滤波 广义卡尔曼滤波 增广广义卡尔曼滤波
5.2
5.3
2012/6/6
2
第5章 状态估计
引入状态向量是为了对系统内部结构进行数学描述, 但在许多情况下，系统的状态是无法直接测量到的, 有时甚至全部不能量测到。或者，需要从夹杂随机 干扰的观测信号中分离出系统的状态值。
例如，飞机在飞行过程中所处的位置、速度等状态参数需要通过 雷达或其它测量装置进行观测，而这些测量装置也存在着随机干 扰，因此观测到飞机的位置、速度等信号中就夹杂着随机干扰， 我们只能根据观测到的信号来估计和预测飞机的状态。
● SISO线性时不变离散系统状态空间模型为：
⎧ x ( k + 1) = Α x ( k ) + b u ( k ) ⎨ ⎩ y ( k ) = cx ( k )
● 对应状态空间模型的传递函数为： G( z −1 ) = c( zI − A)−1 b