MATLAB 對卡爾曼濾波的仿真實現(xiàn)

1、計算機實習報告MATLAB對卡爾曼濾波的仿真實現(xiàn)MATLAB對卡爾曼濾波的仿真實現(xiàn)實驗目的基于卡爾曼濾波原理，利用matlab針對以下模型編制代碼進行仿真x(k)=0.5x(k-l)+25x(k+8cos1.2(k-1)+w(k-1)1+x(k-1)y(k)=x(k)2+v(k)201：仿真100次，每次50步。從100次仿真中任選一次，將其對應的狀態(tài)真值和濾波結果曲線繪制在同一副圖上，繪制時選用不同的線型，顏色并進行標注。2：用均方根誤差評價算法的精度，均方根誤差定義為MREMS(k)二G(x-x八)/M)1/2k,jk,jj=1M為仿真次數，k為離散時間點的索引值，x為第j次仿真時第Kk,

2、j拍的真值。請繪制出RMSE(k)隨k變化的曲線。程序設計程序代碼如下：A=zeros(50,100);B=zeros(50,100);C=zeros(50,100);x=0;fori=1:1:100fork=1:1:50 x=0.5*x+25*x/(1+xA2)+8*cos(1.2*(i-1)+randn;A(k,i)=x;endendxk=0;P=1;fori=1:1:100fork=1:1:50 xkk=0.5*xk+25*x/(l+xkA2)+8*cos(1.2*(k-l);F=1/2+25/(1+xkA2)-50*xkA2/(1+xkA2)A2;P=1+F*P*F;H=xk/10;S

3、=H*P*H+10;K=P*H/S;xk=xkk+K*(xkA2/20+sqrt(10)*randn-xkkA2/20-sqrt(10)*randn);Pk=P-K*S*K;B(k,i)=xk;C(k,i)=Pk;endendD=A-B;Q=zeros(50,1);fork=1:1:50E=D(k,:);sum=0;forj=1:1:100sum=sum+E(j)A2;endRMSE=sqrt(sum/100);Q(k)=RMSE;endm=round(rand*100);subplot(1,2,1);plot(A(:,m),rx-);holdonplot(B(:,m),b*:);legend

4、(真值,濾波);title(第一次仿真);subplot(1,2,2);plot(Q);title(RMSE隨k變化曲線);輸出結果截屏顯示為程序運行中觀測截屏A二Colunms1through197.873913.81771.3848-15.6697-9.29031.753312.94924.3323-0.1389-12.8158-0.816016.41588.1692-1.0942-15.2039-4.258716.36009.520114.3S3511.49947.7025-17.0096-7.329021.322712.23732.9S55-11.2407-10.5305-5.4527

5、14.58803.9232-22.511S-13.9365-3.489116.38156.035118.186510.56260.S259-17.5893-6.699619.7S5013.30343.0225-15.3531-9.4997-0.789214.58227.5152-21.4557-11.6619-4.237318.87438.402618.811910.70627.1127-17.8926-6.875018.765712.62223.7597-17.6440-10.4827-5.166715.87023.1837-21.0258-12.1845-1.900018.73825.36

6、7519.14469.25861.6619-IS.61S6-6.128719.333214.09751.9729-17.245S-8.6178-0.179815.31346.9S61-22.0571-10.3076-5.887919.14387.225118.50019.26995.1016-15.0626-6.997316.951213.12018.6107-17.7095-9.99780.921116.04035.5961-19.4276-10.2273-0.320019.49807.396217.703210.38441.0133-18.6538-6.027017.366513.5536

7、3.2584-17.6575-8.682518.440915.34515.2021-19.9021-12.1796-3.326119.88255.521718.539610.15309.0465-17.5613-7.203915.794012.87965.0896-18.2563-9.352818.500315.94374.1922-18.8604-11.4436-2.973921.28228.151818.17618.34441.4928-17.3680-5.716517.584713.87023.3267-18.0407-9.712017.806515.79425.9133-18.5604

8、-10.8492-3.553620.07937.455419.85199.24787.3235-18.5874-6.568017.920211.94945.5245-18.8891-10.805916.735516.31454.7765-19.2443-11.2006-3.421619.50347.003318.814710.46112.4028-17.9561-8.364118.362414.56684.5094-17.0171-9.548615.561214.46435.5259-17.6022-12.0886-4.413518.91306.647717.936911.57524.7539

9、-17.1461-6.237017.790813.63892.9784-18.1189-7.995416.390116.71674.7501-17.8587-10.7437-1.277118.88526.868418.139510.91931.6408-16.1633-5.375716.106513.37103.9122-17.5931-6.944316.711116.09725.8499-19.8094-8.2556-5.461118.25647.805219.525810.77507.3271-16.6826-5.211218.279011.98084.9090-20.0339-8.006

10、414.744916.42724.4971-19.2042-10.7270-0.993018.01595.334118.472910.1036-0.1584-16.4212-6.302017.528412.92785.4433-18.1308-9.896816.754018.03614.1679-18.5572-10.8378-7.042518.14997.819018.490310.6123-8.6210-15.1321-4.833918.047713.18252.7903-18.6739-9.213216.675816.85796.5868-19.0773-9.7745-2.926815.

11、68635.786217.115311.0857-15.1992-16.4950-5.981219.041212.13394.4195-17.9161-7.733316.917017.50015.9015-18.4641-11.5606-3.220318.15397.1443IS.13559.7700-16.4140-17.5102-4.240319.086813.578S3.7711-18.6425-8.0S9215.361116.29325.8293-17.2622-12.4623-1.907618.6636S.2S1118.059011.3759-16.0559-18.7955-6.73

12、8117.701515.50692.9809-17.8404-9.258717.699217.14843.3999-17.5843-11.7531-5.075718.49498.317117.96079.0952-15.5645-17.0405-5.817317.679515.42064.0946-18.7176-8.616817.844416.93956.8309-17.0592-13.8449-1.847318.93106.3935IS.512510.1162-16.9297-17.6705-5.944717.714114.12844.3334-18.98S3-7.S16816.24361

13、6.39623.8346-16.0222-13.2541-6.395717.75037.4S3318.629810.1715-15.0504-15.9830-5.801718.940514.25143.2699-19.6414-8.987916.201815.88006.8363-15.8558-13.0927-1.146416.82615.908518.650512.0769-15.1552-16.6508-5.549919.759513.95315.0009-20.4630-9.538615.772816.50905.8657-17.1004-11.1123-8.690718.01017.

14、1595程序運行中經過卡爾曼遞推后估計值截屏Coli-uniLS1through18651.25001.00330.96730.96860.97040.96481.01221.00160.97140.98000.95331.08630.97980.96060.97280.98520.96591.05690.00480.75680.87390.79370.88380.88290.7528Ll.75780.77430.T8ULILl.84330.71610.77630.79990.78270.7680Ll.79550.7213D.00620.86800.94800.90940.9611D.9524

15、D.88090.9182D.90190.92060.9408D.84650.92060.8942D.89580.88820.9047D.86580.00830.95621.02321.00181.90620.98350.9970Ll.95700.98151.02331.58800.95471.46390.95291.01711.00581.10980.95360.01111.88440.96340.95850.84680.9545D.95571.0028D.95380.96270.89901.84920.92131.1103D.95920.95890.97111.20120.0144Ll.64

16、240.96660.9653Ll.91860.96320.9637li.yrnri0.96030.96610.95710.65010.97930.84250.96330.9655Ll.97040.8110D.01830.71460.91470.88070.89320.9130D.88080.9159D.90630.89540.8691D.69300.90750.83310.91400.89600.9165D.85540.02340.80530.94040.9059Ll.92040.93820.90460.94020.94280.93280.92580.75780.92630.89900.931

17、60.94410.95420.89200.03160.97941.00680.95670.9531D.9530D.95301.16271.01250.95340.9553D.97940.96510.9597D.96710.96831.0251D.95300.04424.92970.95411.89521.51411.33431.4836Ll.97120.95371.46441.11778.38571.01093.45520.98320.9747Ll.96101.25570.05940.15330.9627D.71380.7531D.80580.76510.9691D.96210.77450.8

18、8161.20680.93220.5334D.94740.95020.9547D.83940.07490.17990.92290.8018Ll.80330.84370.80470.92290.90440.8464Ll.89730.89680.9150Ll.57580.89870.91880.88650.84870.09470.21930.91590.86040.86630.88820.83820.90500.91740.8666Ll.88150.87590.91510.66030.89240.92080.92360.89220.13030.31910.97180.97860.96850.970

19、50.97700.96970.96610.97700.96530.97500.9656Ll.90130.97410.97270.97220.97340.20440.54387.86207.09342.93975.50863.30026.91695.55638.48627.89297.40226.61201.59725.66816.62123.95147.2941n.30210.78990.83110.75230.60630.5742Ll.62910.67910.57421.24500.88840.74400.59580.83280.55050.6462Ll.57130.73920.38700.

20、85450.74310.68510.66350.58640.67750.66780.59481.0117Ll.87160.68820.62150.85310.57980.62700.64940.75260.46210.84600.75550.74190.7110D.6372D.72620.68690.67490.90110.80810.71410.61910.8554D.62790.65150.6953D.77940.62500.96210.91440.90570.90320.84120.89510.88280.87480.97600.95600.92610.78820.96990.82770

21、.82110.88310.9270D.94865.26811.54311.08241.0695D.95341.16891.0039D.96703.02931.37731.42730.96453.9619D.95560.95651.07541.81001.18260.61810.87490.96590.96570.98550.95200.96590.95510.66220.90820.89731.0919Ll.59760.97040.99130.96630.8253D.76800.69350.9126D.95880.9639D.86390.95700.9561D.93270.72810.9409

22、D.96170.79290.6533D.87270.85610.9645D.90120.82780.71780.88440.92640.90320.82470.89600.89790.90810.77070.91140.90000.8205Ll.73170.86010.85930.89110.8875B=1.0e+003*Colunms1tlirough180.45550.02220.01730.02060.01690.01680.02240.02220.02140.02120.01850.02420.02140.02030.02110.02170.02050.02390.40020.0182

23、0.01410.01630.01310.01370.01770.01590.01670.0157Ll.U1450.01920.01570.0170o.riitiy0.01730.LI1G50.01840.3472Ll.ULI790.00560.00580.00380.00600.00590.00670.00610.00560.00410.00770.LILI430.LILI720.00570.00580.00510.LIO750.30020.00370.00920.00860.02150.00800.00820.00570.00790.00910.01850.00370.01710.00500

24、.00860.00860.01140.00470.26260.02930.01050.01010.01670.01110.0110Ll.U1590.01160.01070.01420.02890.LH15o.riiyu0.LH130.0102o.runs0.02070.23260.02510.01540.01710.01720.01560.01710.01580.01600.01640.01810.02600.LH6U0.01970.01560.01640.01540.LH8U0.20480.02110.01450.01640.01580.01460.01650.01460.01440.015

25、00.01550.02300.01530.01690.01500.01430.01360.01720.17580.01140.00580.00800.00740.00740.00720.00490.00570.00700.00680.01170.00640.00830.00640.00630.00560.0071Ll.14790.00130.00790.00370.00410.00440.00410.01210.00780.00420.00500.00180.00570.00290.00600.00620.00880.00460.12680.07790.00940.02640.02390.02

26、130.02340.00960.00930.02300.01740.02150.01420.03640.01300.01270.00800.01970.11230.07010.01480.02160.02130.01950.02120.01490.01580.01950.01690.02160.01500.03210.01650.01540.01650.01920.09910.06190.01580.01880.01850.0174Ll.01900.01630.01570.0184li.Line0.01910.01590.02740.LH7U0.01560.01540.01720.08320.

27、04880.00970.01110.00900.00930.01170.00930.0089Ll.U1150.00880.LH260.00890.01710.00990.00980.00980.00970.06430.03270.i：ii：il60.00210.00310.00240.00300.00210.00240.00170.00180.00210.00220.00410.00240.00220.00280.00200.05050.02170.02900.03080.03250.03620.03200.03300.03620.02080.02770.03130.03590.02110.03730.LI34U0.03510.03130.0427li.niyn0.02690.02880.02790.03240.02750.0294Ll.U32IJ0.01770.02250.02880.03120.01930.03280.LI31U0.02900.02640.03740.01920.02400.02440.02510.02870.02460.0267li.02700.01780.02180.02560.0296o.iiiyy0.02910.02810.