110,567
社区成员
发帖
与我相关
我的任务
分享
// MyMatrix.cs
//
using System;
public class MyMatrix
{
private int cols, rows;
protected double[,] data;
/// <summary>Construct a matrix</summary>
public MyMatrix(int rows, int cols)
{
this.rows = rows;
this.cols = cols;
this.data = new double[rows, cols];
}
/// <summary>Construct a matrix by copying the data</summary>
public MyMatrix(double[,] data) : this( data.GetLength(0), data.GetLength(1) )
{
System.Buffer.BlockCopy(data, 0, this.data, 0, sizeof(double) * this.rows * this.cols);
}
/// <summary> Gets the number of rows in the matrix</summary>
public int Rows { get { return this.rows; } }
/// <summary> Gets the number of columns in the matrix</summary>
public int Cols { get { return this.cols; } }
/// <summary> Gets the raw data of the matrix. Should be treated as readonly</summary>
public double[,] RawData { get { return this.data; } }
/// <summary> Gets the sets the matrix element</summary>
public double this[int row, int col]
{
get { return this.data[row, col]; }
set { this.data[row, col] = value; }
}
/// <summary> Multiply by a vector. The width of the matrix should equal the height of the vector</summary>
public double[] Multiply(double[] x)
{
if (x == null || x.Length != this.cols) throw new ArgumentException("Invalid vector");
double[] result = new double[this.rows];
for (int i = 0; i < result.Length; i++)
{
double sum = 0;
for (int j = 0; j < this.cols; j++)
{
sum += this.data[i, j] * x[j];
}
result[i] = sum;
}
return result;
}
/// <summary> Multiply by another matrix. The width of left matrix should equal the height of right matrix </summary>
public MyMatrix Multiply(MyMatrix m)
{
if (m == null || m.rows != this.cols) throw new ArgumentException("Invalid matrix to multiply");
MyMatrix result = new MyMatrix(this.rows, m.cols);
int inner = this.cols;
for (int row = 0; row < result.rows; row++)
{
for (int col = 0; col < result.cols; col++)
{
double sum = 0;
for (int i = 0; i < inner; i++) sum += this[row, i] * m[i, col];
result[row, col] = sum;
}
}
return result;
}
/// <summary> Create a transposed matrix </summary>
public MyMatrix Transpose()
{
MyMatrix result = new MyMatrix(this.cols, this.rows);
for (int row = 0; row < this.rows; row++)
{
for (int col = 0; col < this.cols; col++)
{
result[col, row] = this.data[row, col];
}
}
return result;
}
public static double[] operator *(MyMatrix m, double[] vector)
{
return m.Multiply(vector);
}
public static MyMatrix operator *(MyMatrix m1, MyMatrix m2)
{
return m1.Multiply(m2);
}
}
public class SquareMatrix : MyMatrix
{
int rank;
/// <summary>Construct an identity matrix</summary>
public SquareMatrix(int rank) : base(rank, rank)
{
this.rank = rank;
for (int i = 0; i < rank; i++) this.data[i, i] = 1;
}
/// <summary>Construct a square matrix by copying the data</summary>
public SquareMatrix(double[,] data) : base(data)
{
if ( data.GetLength(0) != data.GetLength(1))
{
throw new ArgumentException("Only square matrix supported");
}
this.rank = data.GetLength(0);
}
/// <summary>Use Laplace expansion to calculate the determinant of a square matrix</summary>
public double Determinant()
{
if (this.rank == 1)
{
return data[0, 0];
}
if (this.rank == 2)
{
return data[0, 0] * data[1, 1] - data[1, 0] * data[0, 1];
}
double det = 0;
for (int i = 0; i < this.rank; i++)
{
det += this.data[i, 0] * this.Cofactor(i, 0).Determinant() * (i % 2 == 0 ? 1 : -1);
}
return det;
}
/// <summary>Use Cramer's rule to recursively find the inverse of a square matrix</summary>
public SquareMatrix Inverse()
{
double det = this.Determinant();
if (Math.Abs(det) < 1e-8) throw new InvalidOperationException("Cannot inverse a singular matrix");
SquareMatrix result = new SquareMatrix(this.rank);
for (int i = 0; i < this.rank; i++)
{
for (int j = 0; j < this.rank; j++)
{
result.data[j, i] = this.Cofactor(i, j).Determinant() * ((i + j) % 2 == 0 ? 1 : -1) / det;
}
}
return result;
}
/// <summary>Get a Cofactor matrix </summary>
public SquareMatrix Cofactor(int i, int j)
{
if (this.rank < 2) throw new InvalidOperationException("Rank is less than two");
SquareMatrix result = new SquareMatrix(this.rank - 1);
double[,] buf = result.data;
for (int y = 0; y < this.rank; y++)
{
for (int x = 0; x < this.rank; x++)
{
if (y != i && x != j)
{
buf[y - (y > i ? 1 : 0), x - (x > j ? 1 : 0)] = this.data[y, x];
}
}
}
return result;
}
/// <summary> Create a square matrix from a general matrix</summary>
public static SquareMatrix FromMatrix(MyMatrix m)
{
if (m == null || m.Cols != m.Rows) throw new ArgumentException("Not a valid square matrix");
return new SquareMatrix(m.RawData);
}
}
// GaussNewton.cs
//
using System;
public class GaussNewton
{
public delegate double F(double[] coefficients, double x);
/// <summary> Construct a GaussNewton solver </summary>
public GaussNewton(int coefficientCount)
{
this.coefficientCount = coefficientCount;
this.coefficients = new double[coefficientCount];
}
/// <summary> Initialize the solver without a guess. Y = f(X) </summary>
public void Initialize(double[] Y, double[] X, F f)
{
Initialize(Y, X, f, null);
}
/// <summary> Initialize the solver: Y = f(X) with a guessed approximation</summary>
public void Initialize(double[] Y, double[] X, F f, double[] coefficientGuess)
{
if (X == null || Y == null || f == null) throw new ArgumentNullException();
if (X.Length != Y.Length) throw new ArgumentException("Y and X not in pairs");
this.xs = X;
this.ys = Y;
this.function = f;
this.solved = false;
if (coefficientGuess != null && coefficientGuess.Length == this.coefficientCount)
{
Array.Copy(coefficientGuess, this.coefficients, this.coefficientCount);
}
}
/// <summary> Get the solved coefficents </summary>
/// <exception cref="InvalidOperationException">Throw when not answer can be found</exception>
public double[] Coefficients
{
get { if (!this.solved) { Solve(); } return this.coefficients; }
}
/// <summary> Get the residual error (SSE) </summary>
public double SumOfSquredError
{
get { return this.sse; }
}
/// <summary> Iteratively solve the coefficients using Gauss-Newton method </summary>
/// <remarks> http://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm </remarks>
public double[] Solve()
{
if( this.ys == null) throw new InvalidOperationException("Not yet initialized");
double lastSSE = double.MaxValue;
double[] errors = new double[ this.ys.Length ];
// let C0 be the current coefficient guess, C1 be the better answer we are after
// let E0 be the error using current guess
// we have:
// JacT * Jac * (C1 - C0) = JacT * E0
//
MyMatrix jacobian = Jacobian();
MyMatrix jacobianT = jacobian.Transpose();
MyMatrix product = jacobianT * jacobian;
SquareMatrix inverse = SquareMatrix.FromMatrix(product).Inverse();
for (int iteration = 0; iteration < GaussNewton.MaxIteration; iteration++)
{
this.sse = 0;
for (int i = 0; i < this.ys.Length; i++)
{
double y = function(this.coefficients, this.xs[i]);
errors[i] = this.ys[i] - y;
sse += errors[i] * errors[i];
}
if (lastSSE - sse < GaussNewton.ConvergeThreshold)
{
this.solved = true;
return this.coefficients;
}
double[] shift = inverse * (jacobianT * errors);
for (int i = 0; i < this.coefficientCount; i++)
{
this.coefficients[i] += shift[i];
}
lastSSE = sse;
}
throw new InvalidOperationException("No answer can be found");
}
/// <summary> Calculate a Jacobian matrix. </summary>
/// <remarks> http://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant </remarks>
private MyMatrix Jacobian()
{
double[][] p = new double[this.coefficientCount][];
for (int i = 0; i < p.Length; i++)
{
p[i] = new double[this.coefficientCount];
p[i][i] = 1;
}
MyMatrix jacobian = new MyMatrix(this.ys.Length, this.coefficientCount);
for (int i = 0; i < this.ys.Length; i++)
{
for (int j = 0; j < this.coefficientCount; j++)
{
jacobian[i, j] = function(p[j], this.xs[i]);
}
}
return jacobian;
}
private bool solved;
private int coefficientCount;
private double[] coefficients;
private double[] xs;
private double[] ys;
private double sse;
private F function;
public static readonly int MaxIteration = 16;
public static readonly double Epsilon = 1e-10;
public static readonly double ConvergeThreshold = 1e-8;
}
// Test.cs
//
using System;
class Test
{
static void Main()
{
double[] Y =
{
9999992.348,
9999992.35,
9999992.354,
9999992.359,
9999992.361,
9999992.365,
9999992.366,
9999992.37,
9999992.371,
9999992.374,
9999992.376,
9999992.377,
9999992.379,
9999992.38,
9999992.382,
9999992.384,
9999992.386,
9999992.387,
9999992.389,
9999992.39,
9999992.39,
9999992.392,
9999992.392
};
double[] X = new double[Y.Length];
for (int i = 0; i < X.Length; i++) X[i] = i + 1;
// f(x) = A ln(x) + B
GaussNewton.F f = delegate(double[] coefficients, double x)
{
return coefficients[0] * Math.Log(x) + coefficients[1];
};
GaussNewton gaussNewton = new GaussNewton(2);
gaussNewton.Initialize(Y, X, f);
double[] answer = gaussNewton.Coefficients; //answer[0] = 0.016 answer[1]=9999992.3386
}
static void PolynomialTest()
{
// y = -x*x + 2*x + 3
double[] X = { 1, 2, 3, 8 };
double[] Y = { 4, 3, 0, -45 };
// f(x) = A*x*x + B*x + C
GaussNewton.F f = delegate(double[] coefficients, double x)
{
return coefficients[0] * x * x + coefficients[1] * x + coefficients[2];
};
GaussNewton gaussNewton = new GaussNewton(3);
gaussNewton.Initialize(Y, X, f);
double[] answer = gaussNewton.Coefficients; //A=-1 B=2 C=3
}
}