現設線性時變系統的離散狀態防城和觀測方程為：
X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)
Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分別是k時刻的狀態矢量和觀測矢量
F(k,k-1)為狀態轉移矩陣
U(k)為k時刻動態噪聲
T(k,k-1)為系統控制矩陣
H(k)為k時刻觀測矩陣
N(k)為k時刻觀測噪聲
則卡爾曼濾波的算法流程為：

預估計X(k)^= F(k,k-1)·X(k-1)

計算預估計協方差矩陣
C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
Q(k) = U(k)×U(k)'

計算卡爾曼增益矩陣
K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
R(k) = N(k)×N(k)' 

更新估計
X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^] 

計算更新后估計協防差矩陣
C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)' 
X(k+1) = X(k)~
C(k+1) = C(k)~
重復以上步驟

（沒有測試，輸出部分要配合相應的過程）


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


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

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
X0 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