1、第九讲：动态线性系统的卡尔曼滤波n工程实践中的估计问题有两类：工程实践中的估计问题有两类：1 1、系统的参数部分或全部未知、系统的参数部分或全部未知-有待确定；有待确定；2 2、实施最优控制时，需要了解系统的状态，而系统、实施最优控制时，需要了解系统的状态，而系统中部分或全部状态变量不能直接测得。中部分或全部状态变量不能直接测得。这样就：包含了两类估计问题：这样就：包含了两类估计问题：参数估计参数估计 状态估计状态估计。u不同估计准则会导致不同的估计方法；同样不同的不同估计准则会导致不同的估计方法；同样不同的观测信息观测信息，也会导致不同的估计方法。，也会导致不同的估计方法。n估计问题都是由三
2、个部分构成：估计问题都是由三个部分构成：1 1、函数模型（估计约束条件）、函数模型（估计约束条件）2 2、随机模型（估计验前信息）、随机模型（估计验前信息）3 3、估计准则。、估计准则。p前面讨论的各种最小二乘分解法，都是针对前面讨论的各种最小二乘分解法，都是针对静态函数模型静态函数模型的。即通过观测的。即通过观测向量向量L L所估计的参数向量是不依赖于时间所估计的参数向量是不依赖于时间t t的。的。p但是，在许多实际的问题中，如：但是，在许多实际的问题中，如：应用计算技术进行适时控制的需要；应用计算技术进行适时控制的需要；大坝的变形监测、大坝的变形监测、GPSGPS导航等等，导航等等，以上被
3、估计参数都是随着时间的变化而不断变化的，以上被估计参数都是随着时间的变化而不断变化的，必需在观测必需在观测过程中不断的对系统的过程中不断的对系统的状态状态进行估计，并且随着新观测值的获得进行估计，并且随着新观测值的获得不不断修正断修正这种估计。这种估计。卡尔曼滤波理论便是适应适时控制的需要，对系统的状卡尔曼滤波理论便是适应适时控制的需要，对系统的状态参数进行线性估计的一种递推算法。态参数进行线性估计的一种递推算法。Kalman滤波是卡尔曼（滤波是卡尔曼（kalman）于）于1960年提出的一年提出的一种滤波方法。种滤波方法。特点：特点：是对是对状态空间状态空间进行估计。状态空间估计一般进行估计
4、。状态空间估计一般是动态估计。是动态估计。估计过程利用：估计过程利用：系统状态方程系统状态方程、观测方程观测方程、系统过系统过程噪声程噪声以及以及观测噪声观测噪声的统计特性构成滤波算法。的统计特性构成滤波算法。Kalman采用递推算法。采用递推算法。n 何谓何谓kalman滤波？滤波？如：如：要研究的对象是一个房间的温度，根据经验，房间温度恒定的。1 1）根据上一时刻的温度值来推算下一时刻的温度）根据上一时刻的温度值来推算下一时刻的温度 （称：预测值或估计值，有偏差）（称：预测值或估计值，有偏差）；2 2）温度计测量（）温度计测量（称：观测值，有偏差称：观测值，有偏差）。）。p根据经验根据经验
5、预测估计值预测估计值和当前和当前状态的观测值状态的观测值，并结合其，并结合其各自噪声各自噪声来估算出房间的实际温度值。来估算出房间的实际温度值。p基本概念：基本概念：1、状态向量随时间不断变化的随机变量称为动态系统在时刻随时间不断变化的随机变量称为动态系统在时刻的的“状态状态”向量。向量。如：如：GPSGPS导航、变形监测中是以点的导航、变形监测中是以点的位置位置、运动速度运动速度为状态向为状态向量的。量的。状态向量：状态向量：点的位置、运动速度、加速度点的位置、运动速度、加速度。2 2、状态方程、状态方程（描述随机过程的状态变化）描述随机过程的状态变化）看一简单例子：匀加速直线运动看一简单例
6、子：匀加速直线运动K点点K+1点点K K点：位置点：位置 x xk k K+1 K+1点：位置点：位置x xk+1k+1 速度速度 v vk k 速度速度v vk+1k+1 加速度加速度k k 加速度加速度k+1k+1 t状态向量状态向量n则则K K点与点与K+1K+1点点3 3个量之间关系为：个量之间关系为：匀加速，故匀加速，故 x xk+1 k+1=x=xk k+v+vk kt+1/2t+1/2t t2 2 v vk+1k+1=v=vk k+t t k k=k+1 k+1=状态用矩阵表示（状态方程）状态用矩阵表示（状态方程）11,kkkkXX211111201001kkkkkkttxxvt
7、vaa其中：其中：为系统矩阵，表示位置变化的转移为系统矩阵，表示位置变化的转移；为状态（过程）噪声。为状态（过程）噪声。考虑系统噪声，状态方程为：虑系统噪声，状态方程为：11,kkkkXX1,kk3 3、观测方程、观测方程（描述随机过程与观测量的关系）描述随机过程与观测量的关系）在在K+1处对点的处对点的位置（状态的一项）位置（状态的一项）进行了观进行了观测，得观测向量测，得观测向量k+1，则观测方程：，则观测方程：111111100kKkkkkLxxvaBX其中：为观测噪声。其中：为观测噪声。线性系统线性系统：状态方程为线性的，称线性系统。状态方程为线性的，称线性系统。观测方程观测方程：观测
8、向量观测向量与与状态向量状态向量之间存在的某之间存在的某种函数关系，也称为种函数关系，也称为输出方程输出方程。白噪声白噪声：协方差为零的噪声。协方差为零的噪声。有色噪声有色噪声：不同不同时时刻的刻的动态动态噪声或噪声或观测观测噪声是噪声是相关的。相关的。()()()L tX ttp下面将针对以下问题进行学习：下面将针对以下问题进行学习：1、随机线性系统的数学模型（离散线性系统）；、随机线性系统的数学模型（离散线性系统）；2、随机线性离散系统的、随机线性离散系统的kalman滤波方程；滤波方程；3、kalman滤波的应用。滤波的应用。一、一、离散线性系统的数学模型离散线性系统的数学模型n离散时间
9、系统：离散时间系统：如仅在确定的如仅在确定的瞬间瞬间来研究系统的性能，则把这来研究系统的性能，则把这样的系统叫做离散时间系统。样的系统叫做离散时间系统。包括两种：包括两种：1 1）系统本身就是一个离散系统；）系统本身就是一个离散系统；2 2）本身是连续系统，为研究方便，仅在离散）本身是连续系统，为研究方便，仅在离散时间内研究其性能。时间内研究其性能。1 1、函数模型：、函数模型：状态方程状态方程和和观测方程观测方程称为动态系统的函数模型。称为动态系统的函数模型。u离散系统的状态方程和观测方程为：离散系统的状态方程和观测方程为：11,1,1,11,11,11kkkkkkkkkkkkkKkkkkX
10、XULBXGU 1KKKU，是状态外加的控制（输入）向量，是常量（非随机）即：是状态外加的控制（输入）向量，是常量（非随机）即：非随机控制项。非随机控制项。一般忽略不计。一般忽略不计。称系数矩阵。称系数矩阵。u对于连续线性系统的状态方程和观测方程：对于连续线性系统的状态方程和观测方程：设位置、速度为状态向量，加速度为系统噪声；设位置、速度为状态向量，加速度为系统噪声；按照运动状态知它们之间关系（是一个微分方程）：按照运动状态知它们之间关系（是一个微分方程）：观测方程：观测方程：12(),(),()X tXtt11222()()()()()()dX tX tXtdtdXtXttdt()10()(
11、)L tX tt1122()()(),()()()010()()()001X tX tX tX tXtXtX tX tt 令：则：2 2、离散线性系统的随机模型：、离散线性系统的随机模型：0000()0()0cov(,)()cov(,)()cov(,)0 0()(0)()0var()(0)cov(,)0cov(,)0kkkjkjkjkjkjXXkkEEDkDkE XXXDXX Dirac函数函数二、二、离散线性系统的卡尔曼滤波离散线性系统的卡尔曼滤波1 1）状态方程、观测方程）状态方程、观测方程离散线性系统的估计离散线性系统的估计 即利用观测向量即利用观测向量L L1 1，L L2 2，L L
12、K K，根据其数学模型求定，根据其数学模型求定第第j j时刻状态向量时刻状态向量X X的最佳估值的最佳估值 。11,1,1,1111111111KKKKKKKKKKKKKKKKKKKKXXULBXGUBXZ()jXku估计量估计量 分为三种情况分为三种情况:=，称为滤波；，称为滤波；(用依据过去直至现在的观测值来估计现在的状态，多用于随机系统的实时控制。），称为预测或外推；，称为预测或外推；（依据过去直至现在的观测值来预测未来的状态，用于对系统未来状态的预测和实时控制），平滑或内插。，平滑或内插。（依据过去直至现在的观测值来估计过去历史状态，用于通过分析实验或实验数据对系统进行评估）p预测是滤
13、波的基础，滤波又是平滑的基础。预测是滤波的基础，滤波又是平滑的基础。()jXk2）随机模型：）随机模型：上述模型的卡尔曼虑波称为上述模型的卡尔曼虑波称为“完全不相关的白噪声完全不相关的白噪声作用下的卡尔曼虑波作用下的卡尔曼虑波”。u注意：注意：随机模型参数仅仅给出随机模型参数仅仅给出初始状态初始状态的统计特性。的统计特性。即其他参数均为非随机的。即其他参数均为非随机的。()0()0cov(,)()cov(,)()cov(,)0kkkjkjkjkjkjEEDkDk 0000 0()(0)()0var()(0)cov(,)0cov(,)0XXkkE XXXDXXu状态估计的方法：1 1）最小方差估
14、计或极大验后估计原理；）最小方差估计或极大验后估计原理；2 2）广义最小二乘原理。）广义最小二乘原理。p以下将按照广义最小二乘原理并采用逐次平差以下将按照广义最小二乘原理并采用逐次平差方法导出卡尔曼滤波方程。方法导出卡尔曼滤波方程。设有设有K K个观测向量，则状态方程和观测方程为：个观测向量，则状态方程和观测方程为：1111111111KKKKKKKKKKLBXGUBXZ11,1,1,KKKKKKKKKKXXU 1,1,11,1,1()skkkkkkkkkkkkkkkL kUXXXX s1,1,()()TkkkkD KDK 11,1,1,KKKKKKKKKKXXU 1,1()()skkkksV
15、 KXXL k 为方便起见（为方便起见（类似观测方程类似观测方程），将状态方程改写为：），将状态方程改写为：并有并有：相应观测方程：相应观测方程：的方差阵的方差阵：kn以上实质是：将动态噪声项看成是相应于某种观测向量的观测以上实质是：将动态噪声项看成是相应于某种观测向量的观测噪声。噪声。设有K个观测向量，则状态方程和观测方程为：11,001,001,001111122,112,112,1122222,11,11,11kk kkk kkk kkkkkkkXXULB XZXXULB XZXXULB XZ 1,001111112,112222221,1(0)(0)(1)(1)(1)(1)sssssk
16、kkkskkkkkVXXLVB XZLVXXLVB XZLV kXXL kVB XZL 因只有初始状态是因只有初始状态是随机参数随机参数，根据广义最小二乘原，根据广义最小二乘原理，应将随机参数理，应将随机参数X X0 0的先验期望看成是虚拟观测值。的先验期望看成是虚拟观测值。则可以写出误差方程：则可以写出误差方程：0 0(0)()0XVXX1,001111112,112222221,1(0)(0)(1)(1)(1)(1)ssssskkkkskkkkkVXXLVB XZLVXXLVB XZLV kXXL kVB XZL 这样，就将上述动态系统的估计问题变换为一个最小这样，就将上述动态系统的估计问
17、题变换为一个最小二乘问题。二乘问题。这时，式中这时，式中 均看作是非随机参数；均看作是非随机参数；而而 看作是方差阵为看作是方差阵为 的相应的相应“观测值观测值”，它们之间的协方差阵均为零。，它们之间的协方差阵均为零。01,kXXX12 0(),(0),(1),0sskXLL kL LL(0),(0),(1),(1),()XssDDD kDD kn进行进行“逐次平差逐次平差”：(1)(1)取第一、第二式进行最小二乘间接平差取第一、第二式进行最小二乘间接平差 法方程形如：法方程形如：01,001 0(0)().(0)0(0)(0).(0)XXssSVXXDVXXLD 01,01 010()(0)
18、01(0)(0)XSSXVXVXL2100TTB PBXB PLPD111111,01,01,01,01111,00 01(0)(0)()(0)()(0)()(0)(0)00000 1(0)()(0)()(0)(0)000TTTXSSSXSSSSSSSDDXDXDXDLDXDXDL0101;00XXXX其中:整理法方程，可求解整理法方程，可求解 利用协方差传播律可得解的误差方差阵、利用协方差传播律可得解的误差方差阵、协方差阵协方差阵1,01,01,00 01()()(0)00 0()0SXXLXU 1,01,01,01,01,01()(0)(0)0 01cov(),()(0)00TTXXXDD
19、DXXD (2)(2)以上解作为虚拟观测值，与以上解作为虚拟观测值，与L L1 1 的误差方程的误差方程一起作第二次平差一起作第二次平差:误差方程误差方程 组、解法方程并整理，得：组、解法方程并整理，得：1111111111()()()()0100 1()(1)1XXVXXDVB XZLD.11111111111111111()()()()(1)()1000011111()()()()(1)()10000TTXXTTXXXXXXXDBB DBDLZB XDDDBB DBDB D(3)3)再以上解作为虚拟观测值，与再以上解作为虚拟观测值，与L LS S（1 1）一起作）一起作第三次平差：第三次平
20、差：误差方程误差方程 解为：解为：2 1111111121111XSSSSVXXVXXL，（）（）（）（）（）（）（）2 112 12 12 12 12 11111211121111211cov111STTXXXXXXXUDDDXXD ，2，1，（）=（）（）=（）+（）（）（）（）,（）（）（4 4）依此类推，就可得到递推计算公式（）依此类推，就可得到递推计算公式（5 5个），个），即即“卡尔曼滤波方程卡尔曼滤波方程”。（可分可分3 3步进行）步进行）()()()11()()()1kkkkXkkXkkkXXJLZB XkkkkkDEJ B Dkk,1,11,1,1,1,11()()111()
21、()(1)11k kk kkTTXk kXk kk kk kkkXXUkkkkDDDkkk 一步预测值：一步预测值：滤波值滤波值：1()()()11TTkXkkXkkkJDBB DBDkkk增益矩阵：增益矩阵：时间更新（预测值）时间更新（预测值）（1）计算先验状态估计值（2）计算先验误差估计值测量更新（修正估计值）测量更新（修正估计值）（1）计算修正矩阵（2）更新观测值（3）更新误差协方差,1,111()()11k kk kkkkXXUkk,1,1,1,11()()(1)11TTXk kXk kk kk kkkDDDkkk()()()1XkkXkkDEJ B Dkk()()()11kkkkkk
22、kXXJLZB Xkkk1()()()11TTkXkkXkkkJDBB DBDkkk11,1,1,11,111kkkkkkkkkkkkkKKkXXULBXZ u关于滤波公式关于滤波公式:（1）项称为项称为“预报残差预报残差”（新（新息）；息）；（2 2）J JK K滤波增益矩阵；滤波增益矩阵；n公式的物理意义是公式的物理意义是:滤波值等于滤波值等于预报值预报值加加一修一修正项正项,该修正项由预报残差乘增益矩阵构成。该修正项由预报残差乘增益矩阵构成。()()()11kkkkkkkXXJLZB Xkkk()1kkkkLZB Xk11111KKKKKLBXZp卡尔曼虑波的实质卡尔曼虑波的实质利用卡尔
23、曼滤波器进行滤波时利用卡尔曼滤波器进行滤波时,需要知道系统需要知道系统的状态方程和量测方程。的状态方程和量测方程。计算包括：计算包括：1 1）从一状态推（）从一状态推（K K）求另一状态（）求另一状态（K+1K+1）的预测值；）的预测值；2 2）从此状态本身有的观测）从此状态本身有的观测L LK+1K+1推求该状态估值；推求该状态估值；即：历史信息与观测信息的综合。即：历史信息与观测信息的综合。三、卡尔曼滤波的应用三、卡尔曼滤波的应用p以下通过几个实例来进一步理解卡尔曼滤以下通过几个实例来进一步理解卡尔曼滤波过程。波过程。例例1 设状态方程和观测方程为：设状态方程和观测方程为：随机模型是：随机
24、模型是：又已知两次观又已知两次观测数据L1=4，L2=2，求11110.5KKKKKKXXLX000 0()()()()00cov(,)cov(,)cov(,)0cov(,)cov(,)0()2,()1,(0)1KKkjkjkjkKXEEE XXXXDKDKD 22(),().22XXD 解：（1）计算一步预测值计算一步预测值（2）求增益距阵）求增益距阵J1，（3）计算）计算（4）以（）以（1）-（3）的公式计算）的公式计算（5）计算）计算 11(),().11XXD 22(),().22XXD 2 22(),(),11XXDJ 11(),().00XXD,11()()11k kkkXXkk
25、11110.5KKKKKKXXLX1,01,01,0 01()=()0.5 00001()(0)(0)0.5 1 0.5 11.250TXXXXDDD ,1,1,1,11()()(1)11TTXk kXk kk kk kkkDDDkkk 1()()()11TTkXkkXkkkJDBB DBDkkk()()()11kkkkkkkXXJLZB Xkkk()()()1XkkXkkDEJ B Dkk例例2 2 将一物体以初速度51米/秒从地面垂直上抛，在时刻tK观测物体离地面的距离SK，得观测值LK如下：设各观测值的观测是白噪声，其方差Di=1；初始状态（初始距离和速度）为其先验方差为设不考虑动态噪声
26、，求在各时刻tK物体离地面的距离和瞬时速度的估值 和它们的方差 。tK（秒）0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0LK（米）0.0 45.3 80.1 105.8 121.7 123.9 109.5 85.5 52.3000051SXS015001XDKKKSXS KXD（1）根据运动学，建立k-1时刻与k时刻物体离地面距离、瞬时速度的关系，即动态方程。21111111()()2()kkkkkkkkkkkSSSttg ttSSg tt2111111111()2011112011kkkkkkkkkkkkSSttttgSSttSgS 或写为：（非随机项）（时
27、间间隔1秒）（2）观测方程为kkkLS10kkkkSLS或写为1）计算预报值2）计算预报值的方差3）计算增益距阵J111046.095 1()9.81 20015141.1901X 11150111611()001010111TXD11161116110.9411010100100.059J 1().0XD 1()0X 4）计算估值5）计算估值方差6）按以上1）-5）重复计算求得各时刻估值及方差 1()1X 1().1XD0.94146.09545.3 1()45.31010.05941.19041.1X46.095=41.190100.9411610.910.061()101010.0591
28、10.060.94XD45.30.670.33 2(),)241.10.330.61106.00.660.32 3(),)320.80.320.6352.50.370.06 9(),)938.30.060.02XDXDXD2 (23 (39 (9模型（模型（CV)模型模型(CA)将加速度的变化率视为动态噪声将加速度的变化率视为动态噪声加速度视为动态噪声加速度视为动态噪声例例3在动态水准监测中常用的动态方程：在动态水准监测中常用的动态方程：思考：思考：在在GPSGPS变形监测中，将变形体视为一个动态系统，变形监测中，将变形体视为一个动态系统，将一组观测值作为系统输出，则卡尔曼滤波就可以用将一组观
29、测值作为系统输出，则卡尔曼滤波就可以用来描述这个变形体的运动情况。来描述这个变形体的运动情况。试写出试写出GPSGPS网的状态方程、观测方程。网的状态方程、观测方程。1 1）设）设GPSGPS监测网有监测网有n n个监测点组成，以个监测点组成，以GPSGPS点三维位置和三维点三维位置和三维速率为状态向量；速率为状态向量；2 2）设）设i i点在时刻点在时刻t t的：的：位置向量为位置向量为-瞬时速率为瞬时速率为-将瞬时加速度看成是随机干扰将瞬时加速度看成是随机干扰3 3）i i点的状态向量为：点的状态向量为：i()()()iittt6 13 13 1()()()()()()()()()iiii
30、iiiiiX tttx ty tz tx ty tz t4 4）全网的状态向量为：）全网的状态向量为：5)5)离散卡尔曼滤波的状态方程纯量形式为：离散卡尔曼滤波的状态方程纯量形式为：1261()()()()TnnX tX tXtXt2,1,1,1,1,112i ki kki kki ki ki kki kttt p卡尔曼滤波的优点：卡尔曼滤波的优点：1)1)滤波方程是一组递推计算公式滤波方程是一组递推计算公式,计算过程是一个不计算过程是一个不断预测、修正的过程；断预测、修正的过程；2)2)不需保留用过的观测值序列不需保留用过的观测值序列,并且当得到新的观测并且当得到新的观测数据时数据时,可随时
31、计算新的滤波值可随时计算新的滤波值,便于实时处理观便于实时处理观测成果测成果,把参数估计和预报有机地结合起来把参数估计和预报有机地结合起来.3)3)卡尔曼滤波特别适合变形监测、组合导航等动态卡尔曼滤波特别适合变形监测、组合导航等动态数据处理。数据处理。4 4）卡尔曼滤波值是最小方差无偏估值。）卡尔曼滤波值是最小方差无偏估值。卡尔曼滤波应用于动态观测系统中，可实时获卡尔曼滤波应用于动态观测系统中，可实时获得得系统当前状态系统当前状态；且滤波精度较高；且滤波精度较高；卡尔曼滤波除了可掌握系统的当前状态外，还卡尔曼滤波除了可掌握系统的当前状态外，还可可预测系统未来预测系统未来。（可以模拟动态目标系统
32、的。（可以模拟动态目标系统的变化规律等）变化规律等）（预测和平滑）（预测和平滑）n我们已介绍的近代平差方法：我们已介绍的近代平差方法：1 1）从函数模型的类型：）从函数模型的类型：静态、动态；静态、动态；2 2）从观测方程系数阵）从观测方程系数阵：满秩、列秩亏；：满秩、列秩亏；3 3）从估计参数）从估计参数：非随机变量、随机变量，或同时出现；：非随机变量、随机变量，或同时出现；4 4）从观测误差：）从观测误差：偶然误差、系统误差、偶然误差、系统误差、粗差；粗差；5 5）从估计模型）从估计模型：函数模型估计、随机模型估计。：函数模型估计、随机模型估计。另外还有：另外还有：6)6)观测值中含有粗差时，用观测值中含有粗差时，用“稳健估计稳健估计”（抗差估（抗差估计）来处理平差问题；计）来处理平差问题；7)7)附加系统参数方法处理系统误差时，因参数过度附加系统参数方法处理系统误差时，因参数过度化引入，导致法方程系数阵病态而提出的化引入，导致法方程系数阵病态而提出的“有偏估有偏估计计”。
