卡尔曼滤波收敛

广义测量平差

广义测量平差

课程作业

题 目

卡尔曼滤波简述

学生姓名 王冠雅

学 号 165011040

任课教师 左廷英

学 院 地球科学与信息物理学院

专业班级 研究生测绘16级

广义测量平差

卡尔曼滤波简述

摘要:

卡尔曼滤波作为一种数值估计优化方法，从系统状态的视角出发，依据预估和修正的核

心思想，已成为目前应用最为广泛的滤波方法，并在诸多领域取得了广泛的扩展和应用。本

文通过对基本卡尔曼滤波算法（KF）的介绍进行了简单的程序实现。在卡尔曼滤波的基础

上对扩展卡尔曼滤波算法（EKF）进行了概述，并讨论了该方法在合成孔径雷达干涉测量

（InSAR）领域的相关应用。

关键词：卡尔曼滤波 扩展卡尔曼滤波 InSAR

I．引言

自1960年卡尔曼等人首次从系统状态出发，通过不断地预估和修正观测数

据进行动态滤波的方法之后，其在雷达数据处理、动态导航定位、动态测量等领

域得到了广泛地应用。卡尔曼滤波是以最小均方误差为估计准则的一种高效递归

滤波器，它对非平稳随机过程等动态观测系统具有最优的适用性。但是最初提出

的卡尔曼滤波只针对线性系统，为了扩展该方法的应用，后人又对方法进行了改

进和扩展。本文将在原始卡尔曼滤波的基础上进一步归纳其在雷达数据处理领域

的应用。

II．基本卡尔曼滤波算法（KF）

2.1背景

卡尔曼滤波是在维纳滤波的基础上提出的。二战期间，维纳等人提出了基于

最小均方误差准则，针对平稳随机过程的滤波方法。维纳滤波方法中的维纳-霍

夫积分方程有效解决了随机信号的预估以及从随机噪声中提取随机信号这两个

统计学问题；其中的谱分解方法解决了在静态统计和有理频谱情况下的积分问题。

但是，维纳滤波只对平稳的随机过程适用，而且很难获取半无限时间区间内的全

部观察数据。尽管后人陆续对维纳滤波进行了相应的改进，但是仍存在提取滤波

数据困难，最优脉冲响应的数值确定复杂不便计算，非专业人士难以掌握以及数 广义测量平差

学推导不透明等缺陷。针对上述问题，卡尔曼滤波的优点主要体现在以下几个方

#include "stdio.h"

#include "stdlib.h"

#include "math.h"

double frand()

{

return 2*((rand()/(double)RAND_MAX) - 0.5); //随机噪声

}

void main()

{

float x_last=0;

float p_last=0.02;

float Q=0.018;

float R=0.542;

float kg;

float x_mid;

float x_now;

float p_mid;

float p_now;

float z_real=0.56;//0.56

float z_measure;

float sumerror_kalman=0;

float sumerror_measure=0;

int i;

x_last=z_real+frand()*0.03;

x_mid=x_last;

for(i=0;i<20;i++)

{ x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)

p_mid=sqrt(pow(p_last,2)+pow(Q,2)); //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

kg=sqrt(pow(p_mid,2)/(pow(p_mid,2)+pow(R,2))); //kg为kalman filter，R为噪声

z_measure=z_real+frand()*0.03;//测量值

x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值

p_now=sqrt((1-kg)*pow(p_mid,2));//最优值对应的covariance

//p_now=(1-kg)*p_mid;

分布式卡尔曼滤波

分布式卡尔曼滤波（Distributed Kalman Filter）是一种基于分布式计算的滤波算法，其目的在于对一个由多个传感器组成的系统进行状态估计，其中每个传感器只能观测到系统的一部分状态。

传统的卡尔曼滤波算法是基于单一中心控制器的，该控制器负责整个系统的状态估计和控制。然而，在实际应用中，系统通常由多个不同地点的传感器组成，因此中心控制器的方式会带来许多问题，比如传感器之间的通信延迟、网络传输带宽限制等，影响了系统的实时性和稳定性。

分布式卡尔曼滤波通过将卡尔曼滤波算法分解成多个局部滤波器，每个局部滤波器只负责对本地观测得到的状态进行估计，在滤波器之间通过局部观测值和相关信息进行信息交互和更新，最终完成全局状态估计。相比于传统的卡尔曼滤波算法，分布式卡尔曼滤波具有计算资源分布、通信开销小、实时性好等优点，因此在无人机、传感器网络、智能交通等领域得到了广泛应用。

分布式卡尔曼滤波的基本框架如下： -系统模型：系统状态方程和观测方程；

-局部估计器：每个局部估计器利用本地观测值对局部状态进行估计，并预测下一时刻的状态；

-信息交互：每个局部估计器根据局部观测值和估计结果，与周围局部估计器交换信息，并更新自己的估计值；

-全局估计器：全局估计器收集所有局部估计器的消息，整合后获得全局状态估计值。

具体地，分布式卡尔曼滤波可以通过以下步骤进行实现：

1.确定系统模型：系统状态方程和观测方程是分布式卡尔曼滤波的关键。最常用的是线性的系统状态方程和观测方程，可以用矩阵形式表示。

2.选择一个节点作为全局估计器：在分布式卡尔曼滤波中，需要一个节点负责整合所有局部估计值，得到全局状态估计值。一般选择一个中心节点或者根据特定参数选择最优节点。

3.为每个局部估计器指定观测变量集合：由于每个局部估计器只能观测到系统的局部状态，因此需要在每个局部估计器处预先指定该局部观测变量集合。这里需要注意，不同局部观测变量之间应当是相互独立的。

卡尔曼滤波器原理

卡尔曼的基本思想是：采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，求出现在时刻的估计值。它适合于实时处理和计算机运算。其实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态，或根据系统的量测值从被污染的系统中恢复系统的本来面目。

我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：

111kkkkxAxBuw (1)

再加上系统的测量值：

kkkzHxv (2)

随机信号wk和vk分别表示过程激励噪声和观测噪声，并且假设它们是相互独立并满足正态分布的白噪声。即有：

~(0,)kwNQ

~(0,)kvNR

在此，状态转移矩阵A，输入矩阵B和测量方程中的H,过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R,都假设为常数。

定义ˆkx为根据上一次迭代计算结果而产生的估计值，称为先验估计。先验估计误差为：ˆkkkexx。对应的先验估计误差的协方差为kP。

定义ˆkx为根据当前计算结果而产生的估计值，称为后验估计。后验估计误差为：ˆkkkexx。对应的后验估计误差的协方差为kP。

我们的目的是在系统结构已知的情况下，给定k时刻的状态观测向量kz，求k时刻的系统状态向量的最优估计ˆkx，使得kP最小。基本思路如下：

1.根据1ˆkx计算出k时刻的先验估计ˆkx

11ˆˆkkkxAxBu (3)