質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

新規登録して質問してみよう
ただいま回答率
85.50%
C#

C#はマルチパラダイムプログラミング言語の1つで、命令形・宣言型・関数型・ジェネリック型・コンポーネント指向・オブジェクティブ指向のプログラミング開発すべてに対応しています。

Q&A

解決済

2回答

1638閲覧

楕円体フィッティングによるセンサキャリブレーションについて

退会済みユーザー

退会済みユーザー

総合スコア0

C#

C#はマルチパラダイムプログラミング言語の1つで、命令形・宣言型・関数型・ジェネリック型・コンポーネント指向・オブジェクティブ指向のプログラミング開発すべてに対応しています。

0グッド

0クリップ

投稿2017/10/27 12:30

現在,加速度センサ・地磁気センサの計測値を楕円体へのフィッティングを行う事で,オフセットと感度補正を行おうとしています.回転を含まない楕円体の式を以下のように定義して,最小二乗法を使って解こうとしています.

(x - p)^2 / a^2 + (y - q)^2 / b^2 + (z - r)^2 / c^2 = 1

上記の式を,展開し,一般形(?)に変形しました.

Ay^2 + Bz^2 + Cx + Dy + Ez + F = 0

この左辺を二乗したものを評価関数としてAFで偏微分し,以下のような式が続く連立方程式としてA
Fを求められるプログラムを書きました.
Σ(Ay^4 + By^2z^2 + Cxy^2 + Dy^3 + Ey^2z + Fy^2)= -Σx^2y^2

ソースが以下です.

C#

1 public class Fitting { 2 private Matrix matrix = new Matrix(6, 6); 3 private double[] right = new double[6]; 4 5 /// <summary> 6 /// 与えられた計測値セットの数を取得する 7 /// </summary> 8 public int Samples { get; private set; } = 0; 9 10 /// <summary> 11 /// 与えられた計測値を元に方程式のパラメータを更新する 12 /// </summary> 13 /// <param name="args">3軸のセンサ計測値</param> 14 public void Update(double[] args) { 15 // 有効な引数かチェック 16 if (args.Length != 3) { 17 throw new ArgumentException(); 18 } 19 20 // 計測値セット数の加算 21 this.Samples++; 22 23 // 方程式のパラメータを更新する 24 var common = new double[6]; 25 common[0] = Math.Pow(args[1], 2); 26 common[1] = Math.Pow(args[2], 2); 27 common[2] = args[0]; 28 common[3] = args[1]; 29 common[4] = args[2]; 30 common[5] = 1; 31 32 for (int i = 0; i < 6; i++){ 33 for (int j = 0; j < 6; j++){ 34 this.matrix[i, j] += common[i] * common[j]; 35 } 36 37 this.right[i] -= common[i] * Math.Pow(args[1], 2); 38 } 39 } 40 41 /// <summary> 42 /// これまでの入力から楕円体のパラメータを計算して返す 43 /// </summary> 44 public double[] ComputeCoefficient(){ 45 // 引数が有効かをチェック 46 if(this.matrix.Rows != 6 || this.right.Length != 6){ 47 throw new ArgumentException(); 48 } 49 50 // 逆行列を計算し,逆行列がなければ例外を投げる 51 if(!this.matrix.Inverse(out Matrix inv)){ 52 throw new ArithmeticException(); 53 } 54 55 // 連立方程式の解を求める 56 var result = new double[6]; 57 for (int i = 0; i < 6; i++){ 58 for (int j = 0; j < 6; j++){ 59 result[i] += inv[i, j] * this.right[j]; 60 } 61 } 62 63 var A = result[0]; 64 var B = result[1]; 65 var C = result[2]; 66 var D = result[3]; 67 var E = result[4]; 68 var F = result[5]; 69 70 var p = -C / 2; 71 var q = -D / (2 * A); 72 var r = -E / (2 * B); 73 74 var a = Math.Sqrt(p * p + A * q * q + B * r * r - F); 75 var b = Math.Sqrt(a * a / A); 76 var c = Math.Sqrt(a * a / B); 77 78 return new double[] { p, q, r, a, b, c }; 79 } 80}

Main関数では,7点以下のデータセットをそれぞれUpdateに放り込み,
ComputeCoefficientでパラメータ計算を行っております.

xyz
101.572792206
013.033130074
003.269848481
0.20.73.189204043
10.41.572792206
0.20.23.284962311
0.90.12.093739111

パラメータとしては,もともと
a = 1, b = 2, C = 3, p = 0.1, q = 0.2, r = 0.3
として,与えており,そこから導出されるデータセットなので,誤差もほぼ無く求められるのではないかと思っているのですが,全く異なるパラメータが計算される状況となっております.

考え方やソースコードでどこか誤りはありますでしょうか.
一応,連立方程式は検算を行い,正しい計算結果が得られることを確認しました.

また,データセットについても入力間違いが存在していないことを確認しております.

Matrixクラスについては,自前で実装しており,余因子展開をつかって逆行列を求めるように実装しております.

または,もっと簡単な方法がございましたら,お教え頂けると幸いです.
よろしくお願いいたします.

気になる質問をクリップする

クリップした質問は、後からいつでもMYページで確認できます。

またクリップした質問に回答があった際、通知やメールを受け取ることができます。

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

guest

回答2

0

ベストアンサー

原因は解明しきれませんでしたが,自己解決いたしました.

http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/tech91.html

こちらのURLで解説されている式を用いたところ,それらしき値を得られ,特に,回転成分(xy, yz, xzの項)を0としてプログラムしてみたところ,うまく行きました.展開式としては,質問欄のA~Fを置き換え,以下のようにしたものと成っております.

Ax^2 + By^2 + Cz^2 + Dx + Ey + Fz = 1

投稿2017/10/29 11:04

退会済みユーザー

退会済みユーザー

総合スコア0

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

0

(x - p)^2 / a^2 + (y - q)^2 / b^2 + (z - r)^2 / c^2 = 1

上記の式を,展開し,一般形(?)に変形しました.
Ay^2 + Bz^2 + Cx + Dy + Ez + F = 0

x^2項どっか行っちゃってます。


行列計算は自分で実装するよりライブラリ使ったほうが楽です。
.NETならMath.NETなど

投稿2017/10/28 00:21

編集2017/10/28 00:24
ozwk

総合スコア13512

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

あなたの回答

tips

太字

斜体

打ち消し線

見出し

引用テキストの挿入

コードの挿入

リンクの挿入

リストの挿入

番号リストの挿入

表の挿入

水平線の挿入

プレビュー

15分調べてもわからないことは
teratailで質問しよう!

ただいまの回答率
85.50%

質問をまとめることで
思考を整理して素早く解決

テンプレート機能で
簡単に質問をまとめる

質問する

関連した質問