Kalman滤波及其应用(含仿真代码)
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
(一步预测)
目
• Kalman滤波原理简介
1 Kalman滤波器的由来 2 新息过程 3 Kalman滤波算法
录
• Kalman滤波的应用
1 卫星角速度估计 2 无人机地面目标跟踪
3 基于Kalman滤波的时变信道估计
卫星角速度估计
卫星滚动姿态角度方程：
H (n) H (n 1) TH (n 1)
G (n) ----Kalman增益矩阵
非自适应部分 自适应部分
Kalman增益的计算
Kalman增益的定义：G(n) E{x(n 1) H (n)}R-1 (n)
E{ x (n 1) H (n)} F (n 1, n)E{ x (n) H ( n)} F (n 1, n)E{ x (n)e H (n, n 1)}C H (n) F (n 1, n)E{e (n, n 1)e H (n, n 1)}C H (n) F (n 1, n) K (n, n 1)C H (n)
Kalman滤波算法
初始条件：
ˆ 1 (1) E{ x(1)} x
ˆ 1 (1)][ x(1) x ˆ 1 (1)]H } K (1,0) E{[ x(1) x
输入观测向量过程：{ y(1),..., y(n)} 已知参数： F (n 1, n); C (n); Q1 (n); Q2 (n) 计算： n 1, 2,3,...
录
• Kalman滤波的应用
1 卫星角速度估计 2 无人机地面目标跟踪
3 基于Kalman滤波的时变信道估计
Wiener滤波器
输入为有用信号和噪声之和，两者均为广义平稳过程且知它们的二阶 统计特性，根据最小均方误差准则(输出信号与需要信号之差的均方值最 小)，求得了最佳线性滤波器的参数，这种滤波器被称为Wiener滤波器。
新息过程相关矩阵： R(n) E{ (n) H (n)} C (n) K (n, n 1)C H (n) Q2 (n) 其中 K (n, n 1) E{e(n, n 1)e H (n, n 1)}表示预测状态误差的相关矩阵。
def
Kalman滤波算法构造
基于前面的知识，现在可转入Kalman滤波算法的核心问题的讨论： 如何利用新息过程估计状态向量的预测？最自然的方法是通过新息过程 序列 (1),..., (n) 的线性组合直接构造状态空间的一步预测：
n 1
ˆ 1 ( n) F (n 1, n) E{ x (n) H (k )}R-1 (k ) (k ) F (n 1, n) x
k 1
n 1
定义：G(n) E{x(n 1) H (n)}R-1 (n) ，那么状态误差向量的一步预测为：
ˆ 1 (n 1) F (n 1, n) x ˆ 1 (n) G(n) (n) x
E{ x (n 1) H (k )}R-1 (k ) (k ) E{ x (n 1) H ( n)}R-1 ( n) ( n)
k 1 H -1 E{ x ( n 1) ( k )} R ( k ) ( k ) E{[ F ( n 1, n ) x ( n ) v ( n )] ( k )} R ( k ) ( k ) 1 H -1 k 1 k 1 n 1 n 1
Kalman滤波器的由来
已知待估计信号二阶统计特性 待估计信号二阶统计特性未知 Wiener滤波器 ？
Kalman滤波理论是Wiener滤波理论的发展，最早用于随机过程的参 数估计，后来很快在各种最优滤波和最优控制问题中得到了极其广泛的应 用。 Kalman滤波器具有以下特点： 1. 其数学公式用状态空间描述； 2. 它的解是递推计算的，即与Wiener滤波器不同，Kalman滤波器是 一种自适应滤波器。
ˆ 1 (n 1) x ˆ (n 1| y (1),..., y (n)) x W1 (k ) (k )
k 1 n
def
W1 (k ) 表示与一步预测相对应的权矩阵。现在的问题是如何确定 其中， 这个权矩阵？
状态向量的一步预测
正交性原理：线性滤波器工作在最优条件的充分必要条件是估计误差 eopt (n) 与输入 u(0),..., u(n) 正交。 正交性原理引理：当滤波器工作在最优条件时，由滤波器输出定义的期 望响应的估计 yopt (n) 与相应的估计误差 eopt (n)正交。
根据正交性原理
ˆ 1 (n 1)] H (k )} E{e (n 1, n) H (k )} E{[ x(n 1) x 0 , k 1,..., n
ˆ 1 (n 1) 代人上式，并利用新息过程的正 将构造的状态向量的一步预测 x 交性有：
E{x(n 1) H (k )} W1 (k )E{ (k ) H (k )} W1 (k ) R(k )
{ y(1),..., y(n)} { (1),..., (n)}
新息过程（cont.）
ˆ 1 ( n) ， 在Kalman滤波中，并不直接估计观测数据向量的一步预测 y 而是先计算状态向量的一步预测 ˆ 1 (n) x(n | y(1),..., y(n 1)) x
然后
def
ˆ 1 (n) C (n) x ˆ 1 (n) y
新息过程
考虑一步预测问题：给定观测值 y(1),..., y(n 1) ，求观测向量最小 def ˆ ˆ (n | y(1),..., y(n 1)) ，利用新息方法，很容易求解。 二乘估计 y1 (n) y
y (n) 的新息过程（innovation process）定义为：
ˆ 1 (n), n 1, 2,... (n) y(n) y
N时刻的滚动 姿态角度 更新时间间隔， T较小
卫星滚动姿态角速度方程：
H (n) H (n 1) TH (n 1)
卫星滚动姿态角加速度方程： 定义状态向量：
H (n) u(n) w(n)
系统喷气产生
的加速度
干扰，零均值
白噪声
x1 (n) H (n) (n) x ( n) x ( n ) 2 H H (n) x3 (n)
(利用 (n) C (n)e(n, n 1) v2 (n) ) (利用正交性原理引理)
ˆ 1 (n) e ( n, n 1)]e H (n, n 1)}C H (n) F (n 1, n)E{[ x
Kalman增益的计算表达式：
G(n) F (n 1, n) K (n, n 1)C H (n) R-1 (n)
ˆ(t ) g (t ) [s(t ) n(t )] g ( )[s(t ) n(t )]d s
根据已知的输入信号和噪声的二阶统计特性（比如频谱）获取的 滤波器系数。属于非自适应滤波。
Wiener, Norbert (1949). “Extrapolation, Interpolation, and Smoothing of Stationary Time Series”.
(1)观测方程：
y(n) C (n) x(n) v2 (n)
M×M维观测矩阵
（已知）
M×1维观测向量
M×1维观测噪声
向量，v2 (n) ~ N (0, Q2 (n))
Kalman滤波问题：利用观测数据向量 y(1),..., y(n 1)，对 n 1 求状态向量 x (i ) 各个分量的最小二乘估计。
Kalman. R. E.1960. “A New Approach to Linear Filtering and Problems” .
动态系统模型
考虑一离散时间的动态系统：
(1)过程方程：
x(n 1) F (n 1, n) x(n) v1 (n)
M×1维状态向量 M×M维状态转移 矩阵（已知） M×1维过程噪声 向量，v1 (n) ~ N (0, Q1 (n))
卫星角速度估计
状态方程可以写成：
x(n) Fx(n 1) u(n) w(n)
其中：
1 T 0 状态转移矩阵 F 0 1 T 0 0 0
系统噪声 系统输入向量
u(n) 0, 0, u(n)
T
w (n) 0, 0, w(n)
T
系统噪声 wenku.baidu.com关矩阵
故权矩阵可以表示为： W1 (k ) E{x(n 1) H (k )}R-1 (k )
状态向量的一步预测(cont.)
状态向量的一步预测的最小均方误差估计可表示为：
ˆ 1 (n 1) E{ x (n 1) H ( k )}R -1 ( k ) ( k ) x
k 1 n
Kalman滤波及其应用
目
• Kalman滤波原理简介
1 Kalman滤波器的由来 2 新息过程 3 Kalman滤波算法
录
• Kalman滤波的应用
1 卫星角速度估计 2 无人机地面目标跟踪
3 基于Kalman滤波的时变信道估计
目
• Kalman滤波原理简介
1 Kalman滤波器的由来 2 新息过程 3 Kalman滤波算法
性质1：新息 (n) 与过去观测数据 y(1),..., y(n 1)正交，即
E{ (n) y H (k )} 0,1 k n 1
性质2：新息过程由彼此正交的随机向量序列 { (n)} 组成，即
E{ (n) H (k )} 0,1 k n 1
性质3：表示观测数据的随机向量序列{ y(1),..., y(n)} 与表示新息过程的 随机向量序列 { (1),..., (n)} 一一对应，即
R(n)是新息过程的相关矩阵。
Riccati方程
为了最后完成Kalman自适应滤波，还需要推导 K (n, n 1) 的递推公式。
考查状态向量的预测误差
ˆ 1 (n 1) e (n 1, n) x (n 1) x ˆ 1 ( n) G ( n) ( n)} {F (n 1, n) x (n) v1 (n)} {F ( n 1, n) x [ F (n 1, n) G (n)C (n)]e(n, n 1) G (n)v2 (n) v1 ( n)
状态向量一步预测误差向量的相关矩阵
K (n 1, n) E{e(n 1, n)e H (n 1, n)} F (n 1, n) P (n) F H (n 1, n) Q1 (n)
(请证明) (代入新息过程计算式)
上式成为Riccati差分方程，其中
P (n) K (n, n 1) F -1 (n 1, n)G(n)C (n) K (n, n 1)
ˆ 1 (n) C ( n)[ x( n) x ˆ 1 ( n)] v2 ( n) (n) y(n) C (n) x
新息过程重新写为：
C (n)e (n, n 1) v2 (n) def ˆ 1 (n) 表示状态向量的一步预测误差。 其中 e(n, n 1) x(n) x
G (n) F (n 1, n) K (n, n 1)C H (n)[C (n) K (n, n 1)C H ( n) Q2 ( n)]1 ˆ 1 ( n) ( n) y ( n) C ( n) x ˆ 1 (n 1) F (n 1, n) x ˆ 1 (n) G (n) (n) x P (n) K (n, n 1) F -1 (n 1, n)G (n)C (n) K (n, n 1) K (n 1, n) F (n 1, n) P (n) F H (n 1, n) Q1 (n)
目
• Kalman滤波原理简介
1 Kalman滤波器的由来 2 新息过程 3 Kalman滤波算法
录
• Kalman滤波的应用
1 卫星角速度估计 2 无人机地面目标跟踪
3 基于Kalman滤波的时变信道估计
卫星角速度估计
卫星滚动姿态角度方程：
H (n) H (n 1) TH (n 1)
G (n) ----Kalman增益矩阵
非自适应部分 自适应部分
Kalman增益的计算
Kalman增益的定义：G(n) E{x(n 1) H (n)}R-1 (n)
E{ x (n 1) H (n)} F (n 1, n)E{ x (n) H ( n)} F (n 1, n)E{ x (n)e H (n, n 1)}C H (n) F (n 1, n)E{e (n, n 1)e H (n, n 1)}C H (n) F (n 1, n) K (n, n 1)C H (n)
Kalman滤波算法
初始条件：
ˆ 1 (1) E{ x(1)} x
ˆ 1 (1)][ x(1) x ˆ 1 (1)]H } K (1,0) E{[ x(1) x
输入观测向量过程：{ y(1),..., y(n)} 已知参数： F (n 1, n); C (n); Q1 (n); Q2 (n) 计算： n 1, 2,3,...
录
• Kalman滤波的应用
1 卫星角速度估计 2 无人机地面目标跟踪
3 基于Kalman滤波的时变信道估计
Wiener滤波器
输入为有用信号和噪声之和，两者均为广义平稳过程且知它们的二阶 统计特性，根据最小均方误差准则(输出信号与需要信号之差的均方值最 小)，求得了最佳线性滤波器的参数，这种滤波器被称为Wiener滤波器。
新息过程相关矩阵： R(n) E{ (n) H (n)} C (n) K (n, n 1)C H (n) Q2 (n) 其中 K (n, n 1) E{e(n, n 1)e H (n, n 1)}表示预测状态误差的相关矩阵。
def
Kalman滤波算法构造
基于前面的知识，现在可转入Kalman滤波算法的核心问题的讨论： 如何利用新息过程估计状态向量的预测？最自然的方法是通过新息过程 序列 (1),..., (n) 的线性组合直接构造状态空间的一步预测：
n 1
ˆ 1 ( n) F (n 1, n) E{ x (n) H (k )}R-1 (k ) (k ) F (n 1, n) x
k 1
n 1
定义：G(n) E{x(n 1) H (n)}R-1 (n) ，那么状态误差向量的一步预测为：
ˆ 1 (n 1) F (n 1, n) x ˆ 1 (n) G(n) (n) x
E{ x (n 1) H (k )}R-1 (k ) (k ) E{ x (n 1) H ( n)}R-1 ( n) ( n)
k 1 H -1 E{ x ( n 1) ( k )} R ( k ) ( k ) E{[ F ( n 1, n ) x ( n ) v ( n )] ( k )} R ( k ) ( k ) 1 H -1 k 1 k 1 n 1 n 1
Kalman滤波器的由来
已知待估计信号二阶统计特性 待估计信号二阶统计特性未知 Wiener滤波器 ？
Kalman滤波理论是Wiener滤波理论的发展，最早用于随机过程的参 数估计，后来很快在各种最优滤波和最优控制问题中得到了极其广泛的应 用。 Kalman滤波器具有以下特点： 1. 其数学公式用状态空间描述； 2. 它的解是递推计算的，即与Wiener滤波器不同，Kalman滤波器是 一种自适应滤波器。
ˆ 1 (n 1) x ˆ (n 1| y (1),..., y (n)) x W1 (k ) (k )
k 1 n
def
W1 (k ) 表示与一步预测相对应的权矩阵。现在的问题是如何确定 其中， 这个权矩阵？
状态向量的一步预测
正交性原理：线性滤波器工作在最优条件的充分必要条件是估计误差 eopt (n) 与输入 u(0),..., u(n) 正交。 正交性原理引理：当滤波器工作在最优条件时，由滤波器输出定义的期 望响应的估计 yopt (n) 与相应的估计误差 eopt (n)正交。
根据正交性原理
ˆ 1 (n 1)] H (k )} E{e (n 1, n) H (k )} E{[ x(n 1) x 0 , k 1,..., n
ˆ 1 (n 1) 代人上式，并利用新息过程的正 将构造的状态向量的一步预测 x 交性有：
E{x(n 1) H (k )} W1 (k )E{ (k ) H (k )} W1 (k ) R(k )
{ y(1),..., y(n)} { (1),..., (n)}
新息过程（cont.）
ˆ 1 ( n) ， 在Kalman滤波中，并不直接估计观测数据向量的一步预测 y 而是先计算状态向量的一步预测 ˆ 1 (n) x(n | y(1),..., y(n 1)) x
然后
def
ˆ 1 (n) C (n) x ˆ 1 (n) y
新息过程
考虑一步预测问题：给定观测值 y(1),..., y(n 1) ，求观测向量最小 def ˆ ˆ (n | y(1),..., y(n 1)) ，利用新息方法，很容易求解。 二乘估计 y1 (n) y
y (n) 的新息过程（innovation process）定义为：
ˆ 1 (n), n 1, 2,... (n) y(n) y
N时刻的滚动 姿态角度 更新时间间隔， T较小
卫星滚动姿态角速度方程：
H (n) H (n 1) TH (n 1)
卫星滚动姿态角加速度方程： 定义状态向量：
H (n) u(n) w(n)
系统喷气产生
的加速度
干扰，零均值
白噪声
x1 (n) H (n) (n) x ( n) x ( n ) 2 H H (n) x3 (n)
(利用 (n) C (n)e(n, n 1) v2 (n) ) (利用正交性原理引理)
ˆ 1 (n) e ( n, n 1)]e H (n, n 1)}C H (n) F (n 1, n)E{[ x
Kalman增益的计算表达式：
G(n) F (n 1, n) K (n, n 1)C H (n) R-1 (n)
ˆ(t ) g (t ) [s(t ) n(t )] g ( )[s(t ) n(t )]d s
根据已知的输入信号和噪声的二阶统计特性（比如频谱）获取的 滤波器系数。属于非自适应滤波。
Wiener, Norbert (1949). “Extrapolation, Interpolation, and Smoothing of Stationary Time Series”.
(1)观测方程：
y(n) C (n) x(n) v2 (n)
M×M维观测矩阵
（已知）
M×1维观测向量
M×1维观测噪声
向量，v2 (n) ~ N (0, Q2 (n))
Kalman滤波问题：利用观测数据向量 y(1),..., y(n 1)，对 n 1 求状态向量 x (i ) 各个分量的最小二乘估计。
Kalman. R. E.1960. “A New Approach to Linear Filtering and Problems” .
动态系统模型
考虑一离散时间的动态系统：
(1)过程方程：
x(n 1) F (n 1, n) x(n) v1 (n)
M×1维状态向量 M×M维状态转移 矩阵（已知） M×1维过程噪声 向量，v1 (n) ~ N (0, Q1 (n))
卫星角速度估计
状态方程可以写成：
x(n) Fx(n 1) u(n) w(n)
其中：
1 T 0 状态转移矩阵 F 0 1 T 0 0 0
系统噪声 系统输入向量
u(n) 0, 0, u(n)
T
w (n) 0, 0, w(n)
T
系统噪声 wenku.baidu.com关矩阵
故权矩阵可以表示为： W1 (k ) E{x(n 1) H (k )}R-1 (k )
状态向量的一步预测(cont.)
状态向量的一步预测的最小均方误差估计可表示为：
ˆ 1 (n 1) E{ x (n 1) H ( k )}R -1 ( k ) ( k ) x
k 1 n
Kalman滤波及其应用
目
• Kalman滤波原理简介
1 Kalman滤波器的由来 2 新息过程 3 Kalman滤波算法
录
• Kalman滤波的应用
1 卫星角速度估计 2 无人机地面目标跟踪
3 基于Kalman滤波的时变信道估计
目
• Kalman滤波原理简介
1 Kalman滤波器的由来 2 新息过程 3 Kalman滤波算法
性质1：新息 (n) 与过去观测数据 y(1),..., y(n 1)正交，即
E{ (n) y H (k )} 0,1 k n 1
性质2：新息过程由彼此正交的随机向量序列 { (n)} 组成，即
E{ (n) H (k )} 0,1 k n 1
性质3：表示观测数据的随机向量序列{ y(1),..., y(n)} 与表示新息过程的 随机向量序列 { (1),..., (n)} 一一对应，即
R(n)是新息过程的相关矩阵。
Riccati方程
为了最后完成Kalman自适应滤波，还需要推导 K (n, n 1) 的递推公式。
考查状态向量的预测误差
ˆ 1 (n 1) e (n 1, n) x (n 1) x ˆ 1 ( n) G ( n) ( n)} {F (n 1, n) x (n) v1 (n)} {F ( n 1, n) x [ F (n 1, n) G (n)C (n)]e(n, n 1) G (n)v2 (n) v1 ( n)
状态向量一步预测误差向量的相关矩阵
K (n 1, n) E{e(n 1, n)e H (n 1, n)} F (n 1, n) P (n) F H (n 1, n) Q1 (n)
(请证明) (代入新息过程计算式)
上式成为Riccati差分方程，其中
P (n) K (n, n 1) F -1 (n 1, n)G(n)C (n) K (n, n 1)
ˆ 1 (n) C ( n)[ x( n) x ˆ 1 ( n)] v2 ( n) (n) y(n) C (n) x
新息过程重新写为：
C (n)e (n, n 1) v2 (n) def ˆ 1 (n) 表示状态向量的一步预测误差。 其中 e(n, n 1) x(n) x
G (n) F (n 1, n) K (n, n 1)C H (n)[C (n) K (n, n 1)C H ( n) Q2 ( n)]1 ˆ 1 ( n) ( n) y ( n) C ( n) x ˆ 1 (n 1) F (n 1, n) x ˆ 1 (n) G (n) (n) x P (n) K (n, n 1) F -1 (n 1, n)G (n)C (n) K (n, n 1) K (n 1, n) F (n 1, n) P (n) F H (n 1, n) Q1 (n)