主题新回顾(发布时间:2010-12-14 14:47:56) |
---|
-- 作者:wangxinxin
-- vb语言实现的卡尔曼滤波源程序 (没有测试,输出部分要配合相应的过程)
Dim num1, num2, num3 Dim a(), a1(), e() As Single Dim alf, xgm(), p(), k1(), k2, k(), p1(), p2(), c(), c1, k3(), k4(), cc() As Single Private Sub Command1_Click() CommonDialog1.ShowOpen Open CommonDialog1.FileName For Input As #1 num3 = Val(Text1.Text) + 2 num2 = Val(Text2.Text) ReDim a(num3, num2) For j = 1 To num3 For i = 1 To num2 Input #1, a(j, i) Next i Next j Close #1 End Sub Private Sub Command2_Click() CommonDialog1.ShowSave Open CommonDialog1.FileName For Append As #2 For i = 1 To num2 Print #2, cc(i, 1), cc(i, 2), cc(i, 3) Next i Close #2 Shell "c:\windows\notepad " & CommonDialog1.FileName End Sub Private Sub Command3_Click() num1 = Val(Text1.Text) + 1 num2 = Val(Text2.Text) num3 = Val(Text1.Text) + 2 ReDim a1(num2), e(num1, num2) As Single ReDim 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 Single c(1) = c(2) = c(3) = 0 r = 0.000001 alf = 10 For j = 1 To num1 For i = 1 To num2 e(j, i) = a(j, i) a1(i) = a(num3, i) Next i Next j For x = 1 To num1 xgm(x) = alf * Sqr(r / e(x, 1)) p(x, x) = xgm(x) ^ 2 Next x For q = 1 To num2 For i = 1 To num1 k3(i) = k4(i) = 0 Next i k2 = 0 For i = 1 To num1 For j = 1 To num1 p1(i, j) = p2(i, j) = 0 Next j Next i c1 = 0 For m = 1 To num1 k3(m) = 0 For n = 1 To num1 k3(m) = k3(m) + e(n, q) * p(n, m) Next n Next m For m = 1 To num1 k4(m) = 0 k4(m) = k4(m) + 1000 * k3(m) * e(m, q) Next m For i = 1 To num1 k2 = k2 + k4(i) Next i For v = 1 To num1 k(v) = 0 k(v) = k3(v) * (k2 / 1000 + r) ^ -1 Next v For i = 1 To num1 For j = 1 To 3 p1(i, j) = k(i) * e(j, q) Next j Next i For l = 1 To num1 For i = 1 To num1 p2(l, i) = 0 For j = 1 To num1 p2(l, i) = 1000 * p1(l, j) * p(j, i) + p2(l, i) Next j Next i Next l For i = 1 To num1 For j = 1 To num1 p(i, j) = p(i, j) - p2(i, j) / 1000 Next j Next i For i = 1 To num1 c1 = c1 + e(i, q) * c(i) Next i For i = 1 To num1 c(i) = c(i) + k(i) * (a1(q) - c1) Next i For i = 1 To num1 cc(q, i) = c(i) Next i Next q End Sub Private Sub Command4_Click() End End Sub |