卡尔曼滤波两例题含matlab程序

1、一. 已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表：时间s1234567高度km1.99451.97941.95541.92141.87771.82501.7598时间s891011121314高度km1.68671.60361.50921.40761.29441.17241.0399时间s151617181920高度km0.89800.74550.58500.41250.23180.0399设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，该物体的初始高度和速度也是高斯分布的随机变量，且。试求该物体高度和速度随时间变化的最优估计。（）解： 1. 令 t=1 R（k

2、）=1 Q(k=0 根据离散时间卡尔曼滤波公式，则有： 滤波初值：一步预测：滤波增益：滤波计算：2. 实验结果高度随时间变化估计速度随时间变化的最优估计高度协方差速度协方差从以上的结果，可以得到高度和速度的估计值，再通过所得到的高度协方差和速度协方差，可见用卡尔曼滤波法，虽然刚开始的初始高度协方差很大为100，但通过2步之后减小到不超过1，逐渐接近于0， 同样的速度协方差刚开始的时候也比较大，为2，但是通过5步之后迅速减小，到10步之后接近于0。3. 有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）；1）初始条件改变时，改变初始高度值，和速度值 由实验结果分析可得度滤波值和速度滤

3、波值在开始几步接近初始值，协方差值基本不变。2）当初始协方差值改变时，改为实验结果分析高度和速度滤波值基本不变，速度协方差和高度协方差开始要接近速度协方差和高度协方差的初始值。但是经过几步之后，都趋于0。二 同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所示。如果，且雷达测距和测角的测量噪声是高斯白噪声随机序列，均值为零、方差阵，试根据下列测量数据确定物体的高度和速度随时间变化的估计值。时间s*1000 斜距km 俯仰角rad*10000.00050000000000 2.82741643781891 0.000758504358760.0025000000000

4、0 2.80671786536244 0.000729007684520.00300000000000 2.79725268974089 0.000800724818190.00350000000000 2.78664273475039 0.000750955762130.00400000000000 2.77320365026313 0.000657627253790.00500000000000 2.74331288628195 0.000797837270340.00550000000000 2.72538888482812 0.000730607129860.0060000000000

5、0 2.70664967712312 0.000632420065300.00650000000000 2.68632403406473 0.000636565244950.00700000000000 2.66386533852220 0.000806598456390.00750000000000 2.64093529707333 0.000677047400690.00850000000000 2.59038109850785 0.000549557590810.00900000000000 2.56298794272843 0.000584879139710.0095000000000

6、0 2.53498317950797 0.000556027473680.02000000000000 2.00058143251913 0.00003246284068题2 示意图解： 1. 令 t=0.5 Q(k=0 根据离散时间扩展卡尔曼滤波公式，则有：状态方程： 测量方程： 辅助方程：一步预测：滤波增益：滤波计算：滤波初值： 2. 实验结果高度随时间变化估计速度随时间变化的估计高度协方差：速度协方差估计实验结果分析：根据图，可得高度和速度的估计值，通过扩展卡尔曼滤波法，高度协方差和速度协方差，刚开始的值比较大，但是迅速减小，在几步之后逐渐趋近于0。3. 有关参数的影响（例如初始条件、噪

7、声统计特性对滤波结果的影响等）；1）初始条件发生变化，改变高度和速度的初始值为分析：高度和速度在刚开始的时候波动比较大，不过经过10步之后，逐渐趋于平稳，高度协方差收敛变快，速度协方差基本不变2）初始噪声改变，分析：高度和速度滤波值基本不变，速度协方差也基本不变，高度协方差刚开始的时候有波动，10步之后趋于稳定。附源程序：第一题t=1;A=1 -t;0 1;g=9.8;U=-0.5*g*t2;g*t;C=1 0;R=1;I=1 0;0 1;X=zeros(2,1;K=zeros(2,1;P=zeros(2,2;P=150 0;0 10;X=1900;10y=1900 1994.5 1979.4

8、 1955.4 1921.4 1877.7 1825.0 1759.8 1686.7 1603.6 1509.2 1407.6 1294.4 1172.4 1039.9 898.0 745.5 585.0 412.5 231.8 399;for i=2:21Pi=A*P*A'%一步预测Xi=A*X+U;K=Pi*C'*(C*Pi*C'+R(-1;%增益X=Xi+K*(y(i-C*Xi;%滤波he(i=X(1,1;ve(i=X(2,1;P=(I-K*C*Pi;ph(i=P(1,1;pv(i=P(2,2;endhe(1=1900;ve(1=10;ph(1=150;pv(1=

9、10;figure;t=1:21;plot(t,he(t,'r'title('高度滤波值'figure;t=1:21;plot(t,ve(t;title('速度滤波值'figure;t=1:21;plot(t,ph(t;title('高度协方差'figure;t=1:21;plot(t,pv(t;title('速度度协方差'夷陵区水利普查对象清查数据审核报告根据宜昌市第一次水利普查领导小组办公室关于做好水利普查对象清查数据汇总、审核、上报的通知和转发第一次全国水利普查对象清查数据上报和验收管理办法的通知精神，夷陵区

10、水利普查办经过几个月的紧张工作，通过清查登记、审核、对比、查缺补漏、数据处理等工作流程，已初步完成对象清查数据审核，现汇报如下：一、清查数据审核工作开展情况一是精心组织，整体推进全省水利普查对象清查工作。我区水利普查办领导十分重视对象清查、数据录入工作，成立了水利普查数据质量控制工作组。在清查底册编制工作中，区普查办参考我区第二次全国经济普查成果和水利部门、统计部门、城建部门、农业部门有关资料整理出初步清查对象，然后根据乡镇水利服务中心和工商、税务部门资料对清查对象进行增减，按照清查分区编制各类清查对象的基础名录，作为清查工作的基础。在划分普查区时，我们主要是按普查专业对基础名录进行确定，各普

11、查专业又通过乡镇获取清查数据。对于河流湖泊、堤防等线状清查对象，以乡镇为单元，不再划分普查区，由乡镇普查员清查填表；对于水闸、泵站和农村饮水安全工程等水利部门掌握的点状水利工程的清查由乡镇普查员负责清查填报；对于取水口、入河排污口按河流流域水系划分普查区，按水利部下发的工作底图中河流边界划分普查区；对于农村供水工程、公共供水企业、工业企业、建筑业和第三产业单位等点状清查对象，以乡镇为普查区，以行政村为清查单元，由乡镇组织逐村清查；灌区清查分乡镇对应的各乡镇清查，交回区普查办核实和填表上报。对于大老岭林场，按“在地原则”划在邓村乡普查小区，由邓村乡负责清查；对于三峡坝区，由区普查办联系相关部门负

12、责协调并清查填表。为做好清查工作，区普查办召开乡镇指导员参加的清查阶段工作会议，按照清查分乡镇部署具体工作，由区普查办各专业的业务组长对清查中的注意事项和填表方法等进行了详细讲解，对所提问题进行一一解答，并印发了清查阶段工作任务及普查表填报说明。为确保清查进度和质量，区普查办派出各专业业务骨干，分别到乡镇进行督导。对进度慢的通报批评，并实行警告制、帮扶制，确保不拖整体工作的后腿。同时抽取部分清查乡镇，结合清查工作底图，采取内业分析和外业调查相结合的方式复核清查表，发现问题及时修正。二是及时抓好台帐建立。清查底册编制完成后，普查办及时安排部署台帐建立工作。按照工业企业、建筑业和第三产业用水量的大

13、小进行排序，初步选出规模以上的企业和规模以下的典型企业。所有规模以上清查对象逐一建立台帐，规模以下按照国家普查办要求选择个数，工业企业每个大类不少于3个，建筑业和第三产业每个门类不少于5个，分别由企业建立2011年1-5份用水台帐，即经济社会用水调查中，Q302，Q305，Q306与建立台帐保持一致。城镇和乡村生活用水典型户的选取，严格按照选取标准，结合用水户居住地点、住房类型、生活方式、用水器具等由乡镇推荐，然后由乡镇审核把关，最终由区普查办将不符合选取标准的及时进行更换，保证典型户具有代表性。对于灌区取用水台帐建立，根据台帐名录对全县11个灌区对象发放了台帐表，并建立用水台帐。三是严格审查

14、清查数据。清查质量是清查的生命，直接影响到水利普查结果的科学性、准确性和真实性，甚至影响整个后续普查工作。在清查工作中，我们采取全程质量控制、全员质量控制、建立责任制等多种方式加强质量管理。在全程质量控制中，我们从方案设计、宣传培训、清查表填报、审核、录入、汇总等各个环节都加强了质量控制。首先是普查办根据政府职能部门及有关单位的已有信息资料编制清查对象基础名录，与上级提供的基础名录进行对比分析，增加遗漏对象，删除重复，形成县级清查基础名录；其次是审查清查是否合理，界限是否清晰；然后是普查指导员负责对清查表进行人工审核，与基础名录进行对比，重点校对与基础名录不同的清查对象和清查跨县边界附近的清查

15、对象。最后是清查期间，区普查办派出得力人员对各乡镇的清查进行经常性的指导、检查和抽查。清查表上报后，由区普查办负责人对各乡镇上报的清查表进行审查。四是抓好了清查数据录入。为确保按时完成输入任务，区普查办成立了水利普查数据录入工作组，购置了3台电脑、1台打印机（传真机）、1台扫描仪、14部手持GPS等设备，抽调4名熟练电脑操作人员进行录入。分专业组把数据录入任务落实到人，所有录入人员集体办公集中就餐。并采取倒排日期，加班加点等方法，赶时间，赶进度，做到人停机不停。采取了机器审核、对象关系分析等方式进行数据质量检查。发现数据逻辑错误、名录重名、对象关系不一致等问题时，及时进行核实和更正。五是严格组

16、织水利普查清查阶段区级验收。为检验水利清查和普查成果的科学性、准确性和真实性，更好地发现问题，解决问题，促进水利普查工作的顺利开展，区普查领导组成立了水利普查验收工作组。六是严格普查登记，确保普查质量。区普查办会审和质量抽查审核，层层审核，严格把关。普查表首先由填表人自检，然后由管理单位部门负责人对填报的普查表进行内部审核，审核数据的真实性、合理性和逻辑性，确保真实有效。单位负责人对普查数据负责，对普查表进行整体审核，并加盖单位公章。然后由乡镇普查员和普查指导员审核，审核后交区普查办审核。普查表上报后，区普查办组织有关专业组，对重点普查数据进行会审，如取水口、河湖（河段）、用水户的取用水量、入

17、河废污水排放量、灌区灌溉面积等，特别是跨县项目的数据，分析普查数据的合理性和可信度。同时组织人员对上报的普查表进行抽查审核，从各项基础数据的来源、依据、填报的准确性和合理性、逻辑关系、数据的有效性、质量控制等方面详细审核，提出书面审核意见，普查对象根据区普查办会审的审核意见，予以解释或更改并重新填报。区普查办对普查表的审核、更改都应填写清查表审核情况反馈表，并将原始表、更正表和审核记录一并存档。二、清查数据的总体质量情况（一）各专项清查数据数量、分布及审核情况1、水利工程专项。清查我水库工程70处，总库容39.66亿立方米；水电站工程65处，总装机容量225.83千瓦；水闸工程38处，总过闸流

18、量298.30立方米/秒；泵站工程68处，总装机流量10.04立方米/秒，总装机功率3358.50千瓦；堤防工程14处，总长度89.70千米；规模以上农村供水工程15处，设计供水规模3.55万立方米/天，设计供水人口11.92人；全区123个村（居）委会有塘坝工程，110个村（居）委会有窑（池）工程。水利工程专项中各项清查表填写符合填表要求，表内表间数据未见明显错误，所抽取样本的漏报、错报率等符合省市级上报要求，审核程序完善严密、无疏漏，完全按照水利普查质量控制要求进行，结合我区区情及基础资料所提供的数据进行比较，验收工作组一致认为水利工程专项数据真实性高，通过区级验收。2、灌区专项。灌区清查

19、总数53个，总灌溉面积27.11万亩，其中跨县灌区1个。灌区专项中清查表填写符合填表要求，表内表间数据未见明显错误，所抽取样本的漏报、错报率等符合省市级上报要求，水利普查所得灌面面积与我区往年灌面面积基本持平，验收工作组一致认为灌区专项数据真实性高，通过区级验收。3、经济社会用水专项。全区共抽取居民生活典型用水户调查对象100户（城镇20户，农村80户）；规模化畜禽养殖场14个，2010年底大牲畜、小牲畜、家禽存栏数分别为945、52009、875000；符合“千吨万人”的公共供水企业6个，城镇5个，农村1个，日供水量62000吨，供水人口220376人；工业企业94个，其中用水大户24个，典

20、型用水户70个；建筑业与第三产业90个，其中建筑业典型用水户5个，第三产业用水大户10个，第三产业典型用水户75个。在听取了经济社会用水专业组工作人员汇报和仔细查阅汇总表和清查表后，验收组一致认为经济用水专项各项清查表填写符合填表要求，表内表间数据未见明显错误，审核程序完善严密、无疏漏，所抽取样本的漏报、错报率等符合省市级上报要求，数据真实性高，通过区级验收。4、河湖开发治理专项。清查河湖取水口116个，取水流量34.80立方米/秒，最大年取水总量20668.00万立方米；地表水源地5个；治理保护河流（河段）总数13处，长度604.47千米；入河排污口70个。河湖开发治理专项中各项清查表填写符

21、合填表要求，表内表间数据未见明显错误，审核程序完善严密、无疏漏，所抽取样本的漏报、错报率等符合省市级上报要求，再结合我区区情及基础资料所提供的数据进行比较，验收工作组一致认为河湖开发治理专项数据真实性高，通过区级验收。5、行业能力建设专项。共清查单位128个，其中从事水利活动的单位有127个。本专项清查表填写符合填表要求，表内表间数据未见明显错误，审核程序完善严密、无疏漏，所抽取样本的漏报、错报率等符合省市级上报要求，再结合我区区情及基础资料所提供的数据进行比较，验收工作组一致认为行业能力建设专项数据真实性高，通过区级验收。（二）清查数据复核情况夷陵区水利普查办对14个普查区清查数据进行了复核

22、工作，共复核8镇、3乡、1个街道办事处、1个新区和1个省级开发区，涉及195个村（居）委会。具体复核名录见表二。通过计算机软件审核、分专业详审和跨专业联审，提出分市、分专业初审意见和整改要求，各市限期完成对象调整、补填和补报后，全省清查数据总体填报完整、规范，成果一致、合理表一 湖北省对象清查数据汇总分布一览表雾渡河镇2.78664273475039;2.77320365026313;2.74331288628195;2.72538888482812;2.70664967712312;2.68632403406473;2.66386533852220;2.64093529707333;2.59

23、038109850785;y=y*1000; yy=0.000787898158189;0.00075850435876;0.00083282260478;0.00067808241639;0.00085279036802;0.00072900768452;0.00080072481819;0.00075095576213;0.00065762725379;0.00081186148545;0.00079783727034;0.00073060712986;0.00063242006530;0.00063656524495;0.00080659845639;0.00067704740069;0

24、.00076573767706;0.00054955759081;0.00058487913971;0.00055602747368;0.00033550412588;0.00056012688452;0.00056694491978;0.00059380631025;0.00053681916544;0.00065871960781;0.00068598344328;0.00060922471348;0.00057086018918;0.00041308535708;0.00047302026281;0.00030949309972;0.00040552624986;0.0003754503

25、3142;0.00020758327980;0.00037186464579;0.00023323830160;-0.00004536186964;0.00003246284068;yy=1000*yy;Pi=zeros(3,3;Xi=zeros(3,1;for i=2:41Pi=A*P*A'%一步预测协方差Xi=A*X+U;%一步预测状态h=sqrt(Xi(2,12+d2;atan(Xi(2,1/d;H=Xi(2,1/sqrt(Xi(2,12+d2 d/sqrt(Xi(2,12+d2 0;d/(Xi(2,12+d2 -Xi(2,1/(Xi(2,12+d2 0;K=Pi*H'*

26、inv(H*Pi*H'+R;%滤波增益%滤波计算X=Xi+K*(y(i;yy(i-h;he(i=X(2,1;ve(i=X(3,1;P=(I-K*H*Pi;ph(i=P(2,2;pv(i=P(3,3;endhe(1=2005;ve(1=1;ph(1=5;pv(1=2;figure;t=1:41;plot(t,he(t;title('高度滤波值'figure;t=1:41;plot(t,ve(t;title('速度滤波值'figure;t=1:41;plot(t,ph(t;title('高度协方差'figure;t=1:41;plot(t,pv(t;title('速度协方差'td 行政区划水利工程