《vb语言实现的卡尔曼滤波源程序》由会员分享,可在线阅读,更多相关《vb语言实现的卡尔曼滤波源程序(4页珍藏版)》请在金锄头文库上搜索。
1、vb 语言实现的卡尔曼滤波源程序 (没有测试,输出部分要配合相应的过程)Dim num1, num2, num3Dim a(), a1(), e() As SingleDim alf, xgm(), p(), k1(), k2, k(), p1(), p2(), c(), c1, k3(), k4(), cc() As SinglePrivate Sub Command1_Click()CommonDialog1.ShowOpenOpen CommonDialog1.FileName For Input As #1num3 = Val(Text1.Text) + 2num2 = Val(Tex
2、t2.Text)ReDim a(num3, num2)For j = 1 To num3For i = 1 To num2Input #1, a(j, i)Next iNext jClose #1End SubPrivate Sub Command2_Click()CommonDialog1.ShowSaveOpen CommonDialog1.FileName For Append As #2For i = 1 To num2Print #2, cc(i, 1), cc(i, 2), cc(i, 3)Next iClose #2Shell “c:windowsnotepad “ & Comm
3、onDialog1.FileNameEnd SubPrivate Sub Command3_Click()num1 = Val(Text1.Text) + 1num2 = Val(Text2.Text)num3 = Val(Text1.Text) + 2ReDim a1(num2), e(num1, num2) As SingleReDim xgm(num1), p(num1, num1), k1(num1), k(num1), p1(num1, num1), p2(num1, num1), c(num1), k3(num1), k4(num1), cc(num2, num1) As Sing
4、lec(1) = c(2) = c(3) = 0r = 0.000001alf = 10For j = 1 To num1For i = 1 To num2e(j, i) = a(j, i)a1(i) = a(num3, i)Next iNext jFor x = 1 To num1xgm(x) = alf * Sqr(r / e(x, 1)p(x, x) = xgm(x) 2Next xFor q = 1 To num2For i = 1 To num1k3(i) = k4(i) = 0Next ik2 = 0For i = 1 To num1For j = 1 To num1p1(i, j
5、) = p2(i, j) = 0Next jNext ic1 = 0For m = 1 To num1k3(m) = 0For n = 1 To num1k3(m) = k3(m) + e(n, q) * p(n, m)Next nNext mFor m = 1 To num1k4(m) = 0k4(m) = k4(m) + 1000 * k3(m) * e(m, q)Next mFor i = 1 To num1k2 = k2 + k4(i)Next iFor v = 1 To num1k(v) = 0k(v) = k3(v) * (k2 / 1000 + r) -1Next vFor i
6、= 1 To num1For j = 1 To 3p1(i, j) = k(i) * e(j, q)Next jNext iFor l = 1 To num1For i = 1 To num1p2(l, i) = 0For j = 1 To num1p2(l, i) = 1000 * p1(l, j) * p(j, i) + p2(l, i)Next jNext iNext lFor i = 1 To num1For j = 1 To num1p(i, j) = p(i, j) - p2(i, j) / 1000Next jNext iFor i = 1 To num1c1 = c1 + e(i, q) * c(i)Next iFor i = 1 To num1c(i) = c(i) + k(i) * (a1(q) - c1)Next iFor i = 1 To num1cc(q, i) = c(i)Next iNext qEnd SubPrivate Sub Command4_Click()EndEnd Sub