C# 实现卡尔曼滤波
最近做一个人流量计数项目，需要对视频流中的人进行目标识别并进行目标跟踪。在图片流中识别出人员目标后需要预测下一帧中目标的位置，查阅各种资料后决定采用卡尔曼滤波法对目标的一位置进行预测。
网上也有别人实现的方法，但都不能直接运行，总是缺出类库或组件，我在别人实现的基础上进行了修改，增加了个矩阵运算类，将两个类文件加入项目后可以直接使用。
类文件一（KalmanFace.cs）
public class KalmanFacade
{
#region inner class
class KalmanFilter
{
///
/// 测量向量维数
///
int MP;
///
/// 状态向量维数
///
int DP;
///
/// 控制向量维数
///
int CP;
///
/// 预测状态(x'(k)):x'(k)=A*x(k-1)+B*u(k)
///
public Matrix state_pre;
///
/// 修正状态 (x(k)):x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
///
public Matrix state_post;
///
/// 状态转移矩阵 (A)
///
public Matrix transition_matrix;
///
/// 控制矩阵 (B)
/// (如果没有控制，则不使用它)
///
public Matrix control_matrix;
///
/// 测量矩阵 (H)
///
public Matrix measurement_matrix;
///
/// 过程噪声协方差矩阵 (Q)
///
public Matrix process_noise_cov;
///
/// 测量噪声协方差矩阵 (R)
///
public Matrix measurement_noise_cov;
///
/// 先验误差估计协方差矩阵 (P'(k)):P'(k)=A*P(k-1)*At + Q)
///
public Matrix error_cov_pre;
///
/// 卡尔曼增益矩阵 (K(k)):K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
///
Matrix gain;
///
/// 后验误差估计协方差矩阵 (P(k)):P(k)=(I-K(k)*H)*P'(k)
///
Matrix error_cov_post;
///
/// 临时矩阵
///
Matrix tempMatrix1;
Matrix tempMatrix2;
Matrix tempMatrix3;
Matrix tempMatrix4;
Matrix tempMatrix5;
public KalmanFilter()
{
MP = 1;
DP = 2;
CP = 0;
state_pre = new Matrix(DP, 1);
state_post = new Matrix(DP, 1);
transition_matrix = new Matrix(DP, DP);
transition_matrix.SetIdentity(1.0);
transition_matrix[0, 1] = 1;
process_noise_cov = new Matrix(DP, DP);
process_noise_cov.SetIdentity(1.0);
measurement_matrix = new Matrix(MP, DP);
measurement_matrix.SetIdentity(1.0);
measurement_noise_cov = new Matrix(MP, MP);
measurement_noise_cov.SetIdentity(1.0);
error_cov_pre = new Matrix(DP, DP);
error_cov_post = new Matrix(DP, DP);
error_cov_post.SetIdentity(1);
gain = new Matrix(DP, MP);
if (CP > 0)
{
control_matrix = new Matrix(DP, CP);
control_matrix.Zero();
}
tempMatrix1 = new Matrix(DP, DP);
tempMatrix2 = new Matrix(MP, DP);
tempMatrix3 = new Matrix(MP, MP);
tempMatrix4 = new Matrix(MP, DP);
tempMatrix5 = new Matrix(MP, 1);
}
public Matrix Predict()
{
state_pre = transition_matrix * state_post;
tempMatrix1 = transition_matrix * error_cov_post;
Matrix at = transition_matrix.Transpose();
error_cov_pre = tempMatrix1 * at + process_noise_cov;
Matrix result = state_pre.DeepCopy();
return result;
}
public Matrix Correct(Matrix measurement)
{
tempMatrix2 = measurement_matrix * error_cov_pre;
Matrix ht = measurement_matrix.Transpose();
tempMatrix3 = tempMatrix2 * ht + measurement_noise_cov;
tempMatrix3 = tempMatrix3.Inverse();
tempMatrix4 = tempMatrix3 * tempMatrix2;
gain = tempMatrix4.Transpose();
tempMatrix5 = measurement - (measurement_matrix * state_pre);
state_post = gain * tempMatrix5 + state_pre;
error_cov_post = error_cov_pre - (gain * tempMatrix2);
Matrix result = state_post.DeepCopy();
return result;
}
public Matrix AutoPredict(Matrix measurement)
{
Matrix result = Predict();
Correct(measurement);
return result;
}
}
#endregion
///
/// 卡尔曼滤波器构造器
///
/// 状态值数组长度,预测下一位置状态值(x,y)数组长度为2
public KalmanFacade(int valueItem)
{
if (valueItem <= 0)
{
throw new Exception("not enough value items!");
}
kmfilter = new KalmanFilter[valueItem];
Random rand = new Random(1001);
for (int i = 0; i < valueItem; i++)
{
kmfilter[i] = new KalmanFilter();
kmfilter[i].state_post[0, 0] = rand.Next(10);
kmfilter[i].state_post[1, 0] = rand.Next(10);
//
kmfilter[i].process_noise_cov.SetIdentity(1e-5);
kmfilter[i].measurement_noise_cov.SetIdentity(1e-1);
}
}
private KalmanFilter[] kmfilter = null;
///
/// 根据现有状态值对下一组状态值进行预测
///
/// 现有的状态值数组
/// 预测的状态值结果数组
///
public bool GetValue(double[] inValue, ref double[] outValue)
{
if (inValue.Length != kmfilter.Length || outValue.Length != kmfilter.Length)
{
return false;
}
Matrix[] measures = new Matrix[kmfilter.Length];
for (int i = 0; i < kmfilter.Length; i++)
{
measures[i] = new Matrix(1,1);
measures[i][0, 0] = inValue[i];
outValue[i] = kmfilter[i].AutoPredict(measures[i])[0, 0];
}
return true;
}
}
类文件二（Matrix.cs）
///
/// 矩阵类
///
[Serializable]
public class Matrix
{
public double[] element;
private int rows = 0;
private int cols = 0;
///
/// 获取矩阵行数
///
public int Rows
{
get
{
return rows;
}
}
///
/// 获取矩阵列数
///
public int Cols
{
get
{
return cols;
}
}
///
/// 获取或设置第i行第j列的元素值
///
/// 第i行
/// 第j列
///
public double this[int i, int j]
{
get
{
if (i < Rows && j < Cols)
{
return element[i * cols + j];
}
else
{
throw new Exception("索引越界");
}
}
set
{
element[i * cols + j] = value;
}
}
public Matrix(int rows,int cols)
{
this.rows = rows;
this.cols = cols;
int count = 0;
this.element = new double[Rows * Cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
element[count++] = 0;
}
}
}
///
/// 用二维数组初始化Matrix
///
/// 二维数组
public Matrix(double[][] m)
{
this.rows = m.GetLength(0);
this.cols = m.GetLength(1);
int count = 0;
this.element = new double[Rows * Cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
element[count++] = m[i][j];
}
}
}
public Matrix(double[,] m)
{
this.rows = m.GetLength(0);
this.cols = m.GetLength(1);
this.element = new double[this.rows * this.cols];
int count = 0;
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
element[count++] = m[i, j];
}
}
}
public Matrix(List
{
this.rows = m.Count;
this.cols = m[0].Count;
this.element = new double[Rows * Cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
this[i, j] = m[i][j];
}
}
}
public void Zero()
{
int count = 0;
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
element[count++] = 0;
}
}
}
///
/// 设置单元矩阵
///
/// 对角线值
public void SetIdentity(double value)
{
for (int i = 0; i < rows; i++)
{
this[i, i] = value;
}
}
#region 矩阵数学运算
public static Matrix MAbs(Matrix a)
{
Matrix _thisCopy = a.DeepCopy();
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
_thisCopy[i, j] = Math.Abs(a[i, j]);
}
}
return _thisCopy;
}
///
/// 矩阵相加
///
/// 第一个矩阵,和b矩阵必须同等大小
/// 第二个矩阵
///
public static Matrix operator +(Matrix a, Matrix b)
{
if (a.cols == b.cols && a.rows == b.rows)
{
double[,] res = new double[a.rows, a.cols];
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
res[i, j] = a[i, j] + b[i, j];
}
}
return new Matrix(res);
}
else
{
throw new Exception("两个矩阵行列不相等");
}
}
///
/// 矩阵相减
///
/// 第一个矩阵,和b矩阵必须同等大小
/// 第二个矩阵
///
public static Matrix operator -(Matrix a, Matrix b)
{
if (a.cols == b.cols && a.rows == b.rows)
{
double[,] res = new double[a.rows, a.cols];
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
res[i, j] = a[i, j] - b[i, j];
}
}
return new Matrix(res);
}
else
{
throw new Exception("两个矩阵行列不相等");
}
}
///
/// 对矩阵每个元素取相反数
///
/// 二维矩阵
///
public static Matrix operator -(Matrix a)
{
Matrix res = a;
for (int i = 0; i < a.rows; i++)
{
for (int j = 0; j < a.cols; j++)
{
res.element[i * a.cols + j] = -res.element[i * a.cols + j];
}
}
return res;
}
///
/// 矩阵相乘
///
/// 第一个矩阵
/// 第二个矩阵,这个矩阵的行要与第一个矩阵的列相等
///
public static Matrix operator *(Matrix a, Matrix b)
{
if (a.cols == b.rows)
{
double[,] res = new double[a.rows, b.cols];
for (int i = 0; i < a.rows; i++)
{
for (int j = 0; j < b.cols; j++)
{
for (int k = 0; k < a.cols; k++)
{
res[i, j] += a[i, k] * b[k, j];
}
}
}
return new Matrix(res);
}
else
{
throw new Exception("两个矩阵行和列不等");
}
}
///
/// 矩阵与数相乘
///
/// 第一个矩阵
/// 一个实数
///
public static Matrix operator *(Matrix a, double num)
{
Matrix res = a;
for (int i = 0; i < a.rows; i++)
{
for (int j = 0; j < a.cols; j++)
{
res.element[i * a.cols + j] *= num;
}
}
return res;
}
///
/// 矩阵转置
///
///
public Matrix Transpose()
{
double[,] res = new double[cols, rows];
for (int i = 0; i < cols; i++)
{
for (int j = 0; j < rows; j++)
{
res[i, j] = this[j, i];
}
}
return new Matrix(res);
}
///
/// 矩阵求逆
///
///
public Matrix Inverse()
{
//最后原始矩阵并不变，所以需要深拷贝一份
Matrix _thisCopy = this.DeepCopy();
if (cols == rows && this.Determinant() != 0)
{
//初始化一个同等大小的单位阵
Matrix res = _thisCopy.EMatrix();
for (int i = 0; i < rows; i++)
{
//首先找到第i列的绝对值最大的数，并将该行和第i行互换
int rowMax = i;
double max = Math.Abs(_thisCopy[i, i]);
for (int j = i; j < rows; j++)
{
if (Math.Abs(_thisCopy[j, i]) > max)
{
rowMax = j;
max = Math.Abs(_thisCopy[j, i]);
}
}
//将第i行和找到最大数那一行rowMax交换
if (rowMax != i)
{
_thisCopy.Exchange(i, rowMax);
res.Exchange(i, rowMax);
}
//将第i行做初等行变换，将第一个非0元素化为1
double r = 1.0 / _thisCopy[i, i];
_thisCopy.Exchange(i, -1, r);
res.Exchange(i, -1, r);
//消元
for (int j = 0; j < rows; j++)
{
//到本行后跳过
if (j == i)
continue;
else
{
r = -_thisCopy[j, i];
_thisCopy.Exchange(i, j, r);
res.Exchange(i, j, r);
}
}
}
return res;
}
else
{
throw new Exception("矩阵不是方阵无法求逆");
}
}
#region 重载比较运算符
public static bool operator <(Matrix a, Matrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] >= b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator >(Matrix a, Matrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] <= b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator <=(Matrix a, Matrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] > b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator >=(Matrix a, Matrix b)
{
bool issmall = true;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] < b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator !=(Matrix a, Matrix b)
{
bool issmall = true;
issmall = ReferenceEquals(a, b);
if (issmall) return issmall;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] == b[i, j]) issmall = false;
}
}
return issmall;
}
public static bool operator ==(Matrix a, Matrix b)
{
bool issmall = true;
issmall = ReferenceEquals(a, b);
if (issmall) return issmall;
for (int i = 0; i < a.Rows; i++)
{
for (int j = 0; j < a.Cols; j++)
{
if (a[i, j] != b[i, j]) issmall = false;
}
}
return issmall;
}
public override bool Equals(object obj)
{
Matrix b = obj as Matrix;
return this == b;
}
public override int GetHashCode()
{
return base.GetHashCode();
}
#endregion
public double Determinant()
{
if (cols == rows)
{
Matrix _thisCopy = this.DeepCopy();
//行列式每次交换行，都需要乘以-1
double res = 1;
for (int i = 0; i < rows; i++)
{
//首先找到第i列的绝对值最大的数
int rowMax = i;
double max = Math.Abs(_thisCopy[i, i]);
for (int j = i; j < rows; j++)
{
if (Math.Abs(_thisCopy[j, i]) > max)
{
rowMax = j;
max = Math.Abs(_thisCopy[j, i]);
}
}
//将第i行和找到最大数那一行rowMax交换,同时将单位阵做相同初等变换
if (rowMax != i)
{
_thisCopy.Exchange(i, rowMax);
res *= -1;
}
//消元
for (int j = i + 1; j < rows; j++)
{
double r = -_thisCopy[j, i] / _thisCopy[i, i];
_thisCopy.Exchange(i, j, r);
}
}
//计算对角线乘积
for (int i = 0; i < rows; i++)
{
res *= _thisCopy[i, i];
}
return res;
}
else
{
throw new Exception("不是行列式");
}
}
#endregion
#region 初等变换
///
/// 初等变换：交换第r1和第r2行
///
/// 第r1行
/// 第r2行
///
public Matrix Exchange(int r1, int r2)
{
if (Math.Min(r2, r1) >= 0 && Math.Max(r1, r2) < rows)
{
for (int j = 0; j < cols; j++)
{
double temp = this[r1, j];
this[r1, j] = this[r2, j];
this[r2, j] = temp;
}
return this;
}
else
{
throw new Exception("超出索引");
}
}
///
/// 初等变换：将r1行乘以某个数加到r2行
///
/// 第r1行乘以num
/// 加到第r2行，若第r2行为负，则直接将r1乘以num并返回
/// 某行放大的倍数
///
public Matrix Exchange(int r1, int r2, double num)
{
if (Math.Min(r2, r1) >= 0 && Math.Max(r1, r2) < rows)
{
for (int j = 0; j < cols; j++)
{
this[r2, j] += this[r1, j] * num;
}
return this;
}
else if (r2 < 0)
{
for (int j = 0; j < cols; j++)
{
this[r1, j] *= num;
}
return this;
}
else
{
throw new Exception("超出索引");
}
}
///
/// 得到一个同等大小的单位矩阵
///
///
public Matrix EMatrix()
{
if (rows == cols)
{
double[,] res = new double[rows, cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
if (i == j)
res[i, j] = 1;
else
res[i, j] = 0;
}
}
return new Matrix(res);
}
else
throw new Exception("不是方阵，无法得到单位矩阵");
}
#endregion
///
/// 深拷贝，仅仅将值拷贝给一个新的对象
///
///
public Matrix DeepCopy()
{
double[,] ele = new double[rows, cols];
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
ele[i, j] = this[i, j];
}
}
return new Matrix(ele);
}
public override string ToString()
{
string str = "";
for (int i = 0; i < Rows; i++)
{
for (int j = 0; j < Cols; j++)
{
str += this[i, j].ToString();
if (j != Cols - 1)
str += " ";
else if (i != Rows - 1)
str += Environment.NewLine;
}
}
return str;
}
}