func main() { m := len(yGiven) n := degree + 1 y := matrix.MakeDenseMatrix(yGiven, m, 1) x := matrix.Zeros(m, n) for i := 0; i < m; i++ { ip := float64(1) for j := 0; j < n; j++ { x.Set(i, j, ip) ip *= xGiven[i] } } q, r := x.QR() qty, err := q.Transpose().Times(y) if err != nil { fmt.Println(err) return } c := make([]float64, n) for i := n - 1; i >= 0; i-- { c[i] = qty.Get(i, 0) for j := i + 1; j < n; j++ { c[i] -= c[j] * r.Get(i, j) } c[i] /= r.Get(i, i) } fmt.Println(c) }
func givens() (x, y *matrix.DenseMatrix) { height := []float64{1.47, 1.50, 1.52, 1.55, 1.57, 1.60, 1.63, 1.65, 1.68, 1.70, 1.73, 1.75, 1.78, 1.80, 1.83} weight := []float64{52.21, 53.12, 54.48, 55.84, 57.20, 58.57, 59.93, 61.29, 63.11, 64.47, 66.28, 68.10, 69.92, 72.19, 74.46} m := len(height) n := 3 y = matrix.MakeDenseMatrix(weight, m, 1) x = matrix.Zeros(m, n) for i := 0; i < m; i++ { ip := float64(1) for j := 0; j < n; j++ { x.Set(i, j, ip) ip *= height[i] } } return }
func main() { // Beacon positions var beaconA = newPos(0.0, 0.0) var beaconB = newPos(0.0, 2000.0) var beaconC = newPos(3000.0, 1000.0) // First state var x0 matrix.Matrix = matrix.MakeDenseMatrix([]float64{200, 0, 200, 0}, 4, 1) var P0 matrix.Matrix = matrix.Diagonal([]float64{10000, 100, 10000, 100}) fmt.Printf(">init:\n") fmt.Printf("x0:\n%v\n\n", x0) fmt.Printf("P0:\n%v\n\n", P0) // x(k+1) = Ax + Bu + Ww var W matrix.Matrix = matrix.MakeDenseMatrix([]float64{ 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}, 4, 2) fmt.Printf("> noise\n") fmt.Printf("W:\n%v\n\n", W) // Design var d = 0.01 * 0.01 var Q matrix.Matrix = matrix.Diagonal([]float64{d, d}) var R matrix.Matrix = matrix.Diagonal([]float64{d, d, d}) fmt.Printf(">Design\n") fmt.Printf("Q:\n%v\n\n", Q) fmt.Printf("R:\n%v\n\n", R) // Init filter var u matrix.Matrix = matrix.MakeDenseMatrix([]float64{0, 0}, 2, 1) var y matrix.Matrix = matrix.MakeDenseMatrix([]float64{1000, 1000, 3000}, 3, 1) // df(x,u)/dx dfdx := func(x, u matrix.Matrix) matrix.Matrix { // The process is independent of possition, df/dx = A T := 1.0 A := matrix.MakeDenseMatrix([]float64{ 1.0, T, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, T, 0.0, 0.0, 0.0, 1.0}, 4, 4) return A } // Estimated movement of robot given state x f := func(x, u matrix.Matrix) matrix.Matrix { // x(k+1) = Ax + Bu var A = dfdx(x, u) return matrix.Product(A, x) // Commented out: take motor gain in to account: /* var B matrix.Matrix = matrix.MakeDenseMatrix([]float64{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 4, 2) return matrix.Sum( matrix.Product(A, x), matrix.Product(B, u)) */ } // dh(x,u)/dx dhdx := func(x matrix.Matrix) matrix.Matrix { // Linearisation of H around x(k) robot := newPos(x.Get(0, 0), x.Get(2, 0)) denA := math.Sqrt(math.Pow(robot.X-beaconA.X, 2.0) + math.Pow(robot.Y-beaconA.Y, 2.0)) denB := math.Sqrt(math.Pow(robot.X-beaconB.X, 2.0) + math.Pow(robot.Y-beaconB.Y, 2.0)) denC := math.Sqrt(math.Pow(robot.X-beaconC.X, 2.0) + math.Pow(robot.Y-beaconC.Y, 2.0)) H := matrix.MakeDenseMatrix([]float64{ (robot.X - beaconA.X) / denA, 0.0, (robot.Y - beaconA.Y) / denA, 0.0, (robot.X - beaconB.X) / denB, 0.0, (robot.Y - beaconB.Y) / denB, 0.0, (robot.X - beaconC.X) / denC, 0.0, (robot.Y - beaconC.Y) / denC, 0.0}, 3, 4) return H } // Measure estimate given state x h := func(x matrix.Matrix) matrix.Matrix { var robot = newPos(x.Get(0, 0), x.Get(2, 0)) var y matrix.Matrix = matrix.MakeDenseMatrix([]float64{ math.Sqrt(math.Pow(robot.X-beaconA.X, 2.0) + math.Pow(robot.Y-beaconA.Y, 2.0)), math.Sqrt(math.Pow(robot.X-beaconB.X, 2.0) + math.Pow(robot.Y-beaconB.Y, 2.0)), math.Sqrt(math.Pow(robot.X-beaconC.X, 2.0) + math.Pow(robot.Y-beaconC.Y, 2.0))}, 3, 1) return y } // State variable var x matrix.Matrix // Initalize kalman filter var filter = extkalman.ExtendedKalman(W, R, Q, x0, P0, f, dfdx, h, dhdx) // Run filter fmt.Printf("Running:\n") for { x = filter.Step(y, u) fmt.Printf("x:\n%v\n\n", x) } }