小弟现在急需卡尔曼滤波的源码,请各位大侠帮帮小弟!
我得, 先行谢过!
我得, 先行谢过!
解决方案 »
- 什么叫熟悉TCP/IP协议?麻烦大家说说自己的理解
- 谁帮忙把这个简单C控制台程序改成MFC的
- 急求助一个简单问题!
- 刚学完c++,有个问题想问大家
- 关于深入浅出中的命令绕行问题,OnCommand
- 谁有单文档多视图切换的例子?
- 巨难问题《《宽带网(10M)没有静态或动态IP,如何配置HTTP、FTP等服务。》》1000分求解决办法
- 为什么我INCLUDE了一个H文件,它还说找不到,明明有啊
- 作工控系统的弟兄,留个QQ咋样?
- select模型与模态对话框
- 求救:请问unhandled exception in XXX.exe(msvcrtd.dll)0X00000005:access vialation是什么错误该如何消除阿
- ADSL拨号,路由共享上网,如何取得路由器的外网IP?
* $Id: kalman.c,v 1.5 2002/02/15 18:11:57 tramm Exp $
*
* This is the core Kalman filtering algorithm.
*
* Author: Aaron Kahn, Suresh Kannan, Eric Johnson
* copyright 2001
*
* Merged into the Autopilot project by Trammell Hudson <[email protected]>
*/#include <math.h>
#include <string.h>#include "kalman.h"
/*
* This will perform a Kalman filter Gain, state, and coveriance
* matrix update. What is needed is the linear A matrix, state vector,
* C matrix, P coveriance matrix, measurement vector, and the R matrix.
*
* A(n,n) Linear model
* P(n,n) Coveriance matrix
* X(n,1) State Vector
* C(m,n) Measurement matrix; m=# of measurements, n=# of states
* R(m,n) Measurement weight matrix
* ys(m,1) Measurement vector
* K(n,m) Kalman Gain matrix
*/
void
kalmanUpdate(
Matrix A,
Matrix P,
Vector X,
Matrix C,
Matrix R,
Vector ys,
Matrix K,
int n,
int m
)
{
Matrix E;
Matrix T1;
Matrix T2;
Vector TV1;
Vector TV2;
/* perform E = C*P*C' + R */
MMmult(C,P,T1,m,n,n);
transpose(C,T2,m,n);
MMmult(T1,T2,E,m,n,m);
MMadd(E,R,E,m,m);
/* perform K = P*C'*inv(E). T2 still has C' */
MMmult(P,T2,T1,n,n,m);
inv(E,T2,m);
MMmult(T1,T2,K,n,m,m);
/* perform x = x + K*( ys - yp ) */
MVmult(C,X,TV1,m,n);
VVsub( ys, TV1, TV1, m );
MVmult(K,TV1,TV2,n,m);
VVadd(X,TV2,X,n);
/* perform P = P - K*C*P */
MMmult(K,C,T1,n,m,n);
MMmult(T1,P,T2,n,n,n);
MMsub( P, T2, P, n, n );
}/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
* $Id: kalman.h,v 1.2 2002/02/14 16:14:03 tramm Exp $
*
* Kalman Filtering routines
*
* Author: Aaron Kahn, Suresh Kannan, Eric Johnson
* copyright 2001
*
* Merged into the Autopilot project by Trammell Hudson <[email protected]>
*/#ifndef _KALMAN_H_
#define _KALMAN_H_#include "matrix.h"/*
* This will perform a Kalman filter Gain, state, and coveriance
* matrix update. What is needed is the linear A matrix, state vector,
* C matrix, P coveriance matrix, measurement vector, and the R matrix.
*
* A(n,n) Linear model
* P(n,n) Coveriance matrix
* X(n,1) State Vector
* C(m,n) Measurement matrix; m=# of measurements, n=# of states
* R(m,n) Measurement weight matrix
* ys(m,1) Measurement vector
* K(n,m) Kalman Gain matrix
*/
extern void
kalmanUpdate(
Matrix A,
Matrix P,
Vector X,
Matrix C,
Matrix R,
Vector ys,
Matrix K,
int n,
int m
);#endif
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 #1End SubPrivate Sub Command2_Click()
CommonDialog1.ShowSave
Open CommonDialog1.FileName For Append As #2For i = 1 To num2
Print #2, cc(i, 1), cc(i, 2), cc(i, 3)
Next iClose #2
Shell "c:\windows\notepad " & CommonDialog1.FileName
End SubPrivate 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 Singlec(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 xFor 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 = 0For 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 mFor m = 1 To num1
k4(m) = 0
k4(m) = k4(m) + 1000 * k3(m) * e(m, q)
Next mFor i = 1 To num1
k2 = k2 + k4(i)
Next iFor v = 1 To num1
k(v) = 0
k(v) = k3(v) * (k2 / 1000 + r) ^ -1
Next vFor i = 1 To num1
For j = 1 To 3
p1(i, j) = k(i) * e(j, q)
Next j
Next iFor l = 1 To num1
For i = 1 To num1
p2(l, i) = 0
For j = 1 To num1p2(l, i) = 1000 * p1(l, j) * p(j, i) + p2(l, i)
Next j
Next i
Next lFor i = 1 To num1
For j = 1 To num1
p(i, j) = p(i, j) - p2(i, j) / 1000
Next j
Next iFor 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 num1cc(q, i) = c(i)
Next i
Next qEnd SubPrivate Sub Command4_Click()
End
End Sub
crystal521(水晶泪)的代码我好几个地方看不明白,比如 :
CommonDialog1.ShowOpen
Open CommonDialog1.FileName For Input As #1
还有:Shell "c:\windows\notepad " & CommonDialog1.FileName
能不能麻烦解释一下?
能不能拜托二位把源码发到我的邮箱里?万分感谢!
[email protected]