文档介绍：Kalman滤波算法
Kalman滤波算法
Kalman滤波算法
Ｋalman滤波算法
姓名：刘金强
专业:控制理论与控制工程
学号：２0０７25５
◆实验目的:
（1）、掌握kｌmaｎ滤波实现的原理和方法
(2)、掌握状态向量预测公式的实现过程
（３)、了解Rｉccati差分方程实现的过程和新息的基本性质和过程的计算
◆实验要求:
问题：
F=［ａ１，a2，ａ3］,其中a1=［ 0 0］的转置，ａ２=[0．3 1．０ 0］的转置,a３＝［0。１ ０。２ 0。４]的转置,x（０)=［３,—１，２]的转置;C=［ｂ１,b2,b3]，其中ｂ1=［ 0。5］的转置，b2=[１，０。４]的转置,ｂ３＝［０．８ —0。7]的转置；V１（n)=［0 ０ n1（n）ｓｉn（0．1ｎ)］的转置,V2（n)=[ｎ2（ｎ) n３（n)］；n1(ｎ）为均值为零，方差为1的均匀分布白噪声;n２(n),n3（n)为均值为0,，n1（n），ｎ2（n)，n3（n）相互独立,试用卡尔曼滤波器算法估计x^（ｎ）。
◆实验原理：
初始条件：
=E｛x（１)｝
Ｋ(１,0)=E｛［x(１）- ］［x(1)— ］｝，其中＝E｛x(1）｝
输入观测向量过程:
观测向量序列＝{ｙ（1）,…………y（n）｝
已知参数：
状态转移矩阵Ｆ（n+1，n)
观测矩阵C（n）
过程噪声向量的相关矩阵
观测噪声向量的相关矩阵
计算：n＝1，2，3，………………。
G(ｎ)=F(ｎ+1，ｎ)Ｋ(ｎ,ｎ＋１）
Kalman滤波算法
Kalman滤波算法
Kalman滤波算法
Kaｌｍan滤波器是一种线性的离散时间有限维系统。Ｋalmａn滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵Ｐ(n)的迹最小化。这意味着,kａlmaｎ滤波器是状态向量x（n）的线性最小方差估计。
◆实验结果：
◆程序代码:
（１)主程序
/*＊**＊＊＊＊＊***＊＊*＊＊＊*＊＊＊*＊**＊＊＊＊＊*＊＊＊＊＊＊＊*＊**＊＊＊****＊*＊*＊＊*＊＊**＊＊*＊*＊*
问题：
Ｆ＝[a１,a2，a3]，其中a１＝[ 0 0］的转置，a2=[0。3 0]的转置,
a3=[０。１ 0．2 0．4］的转置，x(０）＝[3，—1，2］的转置；C＝[ｂ1，b2，ｂ3],其中b１=［0。3 ］的转置,b2=［1,］的转置,ｂ3=[0。8 —］的转置;V1(ｎ）=［０ 0 n１（n)siｎ(0．１n）］的转置,Ｖ2（n）=[n2(n) n３（ｎ）]；n1（n）为均值为零,方差为１的均匀分布白噪声;n2(n）,n3(n）为均值为0，方差为0。1的均匀分布白噪声,n１(n），n2(n）,ｎ3(n）相互独立,试用卡尔曼滤波器算法估计x^(n)。
*＊*＊*＊＊*＊**＊*＊＊*＊***＊＊＊**＊＊*＊＊**＊＊＊＊＊＊＊*＊*＊＊＊***＊＊＊＊****＊＊＊＊**＊*＊＊＊** 设计作者：刘金强
设计时间：2００８年1１月２2日
作者专业:控制理论与控制工程
作者学号：２007255
*＊*＊*＊*＊*＊**＊***＊＊＊＊＊***＊＊＊＊*＊*＊＊**＊＊**＊＊＊*＊****＊＊＊**＊＊＊**＊＊*＊＊**＊＊*／
＃inｃlude 〈iｏstｒeａm。ｈ〉
#inclｕｄｅ ＂MathMaｔriｘ。ｈ”
Kalman滤波算法
Kalman滤波算法
Kalman滤波算法
＃iｎｃlude 〈math。h〉
＃incｌude <strinｇ．h＞
voｉd ＧauｓsIｎｖｅrse（doｕｂlｅ *＊a, inｔ n)；
＃defiｎe NUM ８0 //最大序列数
ｖoiｄ maｉn(）
｛
CMathMatrix MM;
ﻩint ｉ;
ﻩinｔ j， ｗ；
ﻩｄoｕbｌe F[3]［3] = ｛1．0,0。3，0．１， 0， 0。１,0。2,0, 0, 0。4}； //状态转移矩阵
doubｌe F_1[3]［3］ = {1．0，，0。1，0， ,，０， 0， ｝； //状态转移矩阵的逆
ﻩｄｏubｌｅ ＊*FN ＝ ｎew doubｌｅ * [3］； //申请内存空间
ﻩfｏr (i = 0； i ＜ 3； i++）
{
ﻩﻩFN[i］ = ｎeｗ do