目标跟踪的卡尔曼滤波算法分析案例
目录
目标跟踪的卡尔曼滤波算法仿真分析案例 1
1.1系统模型 1
1.2滤波过程 1
1.3算法仿真分析 3
1.4扩展卡尔曼滤波算法 6
1.1.1系统模型
应用KF的系统噪声是高斯分布的，且它的状态方程和观测方程必须满足线性关系，这样的卡尔曼滤波算法才能达到最优的估计效果。为了更清晰地描述系统的模型，将离散时间系统的动态模型的目标状态方程和观测到目标的状态方程用数学模型表达如下
X(k+1)=F(k)X(k)+T(k)W(k)(2-1)
Z(k+1)=Y(k)+V(k)=H(k+1)X(k+1)+V(k+1)(2-2)
其中，X(k)是目标运动的一个状态向量，k表示的是在第k个时刻的目标状态；F(k)是n×n阶的一个状态转移矩阵；I(k)表示为m×n阶的一个过程噪声矩阵；Z(k+1)是观测方程，表示在第k+1时刻的其对应的观测值；Y(k)表示系统输出的状态；H(k+1)是一个p×n阶的观测矩阵；W(k),V(k+1)表示具有均值为零的独立随机过程，即高斯白噪声，且对Vk,j满足
cov[W(k),W(j)]=Q(k)·δ(k-j)(2-3)
cov[V(k),V(j)]=R(k)·δ(k-j)(2-4)cov[W(k),V(
式中，Q为过噪声方差；R为观测噪声方差；δ是狄拉克函数；从式(2-5)中可看出，W(k),V(j)是互不相关的。
1.1.2滤波过程
卡尔曼滤波的主要思想是：在已知当前时刻的状态的情况下，去预测下一个时刻的状态值，再用当前时刻的值和所预测到的值进行对比校正，从而获得当前时刻的更新值，这个更新的值更接近于真实值。大多按“预测-测量-更新”的方式递推进行，递推卡尔曼公式如下
(1)状态一步预测
X(k+1|k)=F(k)X(k|k)(2-6)
(2)预测的协方差矩阵
P(k+1|k)=F(k)P(k|k)F(k)+TQrT(2-7)
(3)卡尔曼增益
K(k+1)=P(k+1)HT[HP(k+1|k)H?+R]-1(2-8)
(4)状态更新
X(k+1|k+1)=X(k+1|k)+K(k+1V(k+1)(2-9)
V(k+1)=Z(K+1)-HX(k+1|k)(2-10)
X(k+1|k+1)=X(k+1|k)+K(k+1)[Z(K+1)-HX(k+1|k)](2-11)
(5)误差协方差矩阵更新
P(k+1|k+1)=[1n-K(k+1)H]P(k+1|k)(2-12)
在上式中，X(k+1|k)表示最佳一步预测值，P(k+1|k)为预测协方差矩阵，K(k+1)为最优滤波增益，X(k+1|k+1)为最佳滤波值，P(k+1|k+1)为平滑估计协方差矩阵，In是一个n×m的单位矩阵。上述递推过程的具体过程可用图错误!文档中没有指定样式的文字。.1表示
开始
开始
目标信息状态初始化
初始化协方差
预测下一个状态信息
预测误差协方差
代入观测方程得到观测预测值Z(k+1|k)
状态更新
X(k+1|k+1)=X(k+1|k)+K(k+1)[Z(k+1-HX(k+1|k))]误差协方差更新
结束
将X(k+1)代入
观测方程得
Z(k+1)
代入状态方程得X(k+1)
求增益K(k+1)
X(k+1|k)
图错误!文档中没有指定样式的文字。.1KF滤波算法流程框图
1.1.3算法仿真分析
假设目标在平面上作匀速直线运动，设声呐采样时间为T?,在采样时刻为kT?时，s(k)为目标在kT时刻的真实位置，y(k)为传感器在kT?时刻的观测值，则其观测模型为
y(k)=s(k)+v(k)