1use nalgebra::{DMatrix, DVector};
4
5use rust_robotics_core::RoboticsError;
6
7#[derive(Debug, Clone, Copy)]
9pub struct GaussianProcessConfig {
10 pub length_scale: f64,
12 pub signal_variance: f64,
14 pub noise_variance: f64,
16}
17
18impl Default for GaussianProcessConfig {
19 fn default() -> Self {
20 Self {
21 length_scale: 1.0,
22 signal_variance: 1.0,
23 noise_variance: 0.01,
24 }
25 }
26}
27
28#[derive(Debug, Clone)]
30pub struct GaussianProcessRegressor {
31 config: GaussianProcessConfig,
32 train_xy: Vec<(f64, f64)>,
33 alpha: Option<DVector<f64>>,
34 k_inv: Option<DMatrix<f64>>,
35}
36
37impl GaussianProcessRegressor {
38 pub fn new(config: GaussianProcessConfig) -> Self {
39 Self {
40 config,
41 train_xy: Vec::new(),
42 alpha: None,
43 k_inv: None,
44 }
45 }
46
47 pub fn fit(&mut self, samples: &[(f64, f64, f64)]) -> Result<(), RoboticsError> {
49 if samples.is_empty() {
50 return Err(RoboticsError::InvalidParameter(
51 "GaussianProcess: at least one training sample is required".to_string(),
52 ));
53 }
54 if self.config.length_scale <= 0.0 || self.config.signal_variance <= 0.0 {
55 return Err(RoboticsError::InvalidParameter(
56 "GaussianProcess: length_scale and signal_variance must be positive".to_string(),
57 ));
58 }
59 if self.config.noise_variance <= 0.0 {
60 return Err(RoboticsError::InvalidParameter(
61 "GaussianProcess: noise_variance must be positive".to_string(),
62 ));
63 }
64
65 let n = samples.len();
66 self.train_xy = samples.iter().map(|(x, y, _)| (*x, *y)).collect();
67 let z = DVector::from_iterator(n, samples.iter().map(|(_, _, z)| *z));
68
69 let mut k = self.kernel_matrix(&self.train_xy, &self.train_xy);
70 for i in 0..n {
71 k[(i, i)] += self.config.noise_variance;
72 }
73
74 let Some(cholesky) = k.clone().cholesky() else {
75 return Err(RoboticsError::NumericalError(
76 "GaussianProcess: kernel matrix is not positive definite".to_string(),
77 ));
78 };
79
80 let alpha = cholesky.solve(&z);
81 let identity = DMatrix::<f64>::identity(n, n);
82 let k_inv = cholesky.solve(&identity);
83
84 self.alpha = Some(alpha);
85 self.k_inv = Some(k_inv);
86 Ok(())
87 }
88
89 pub fn predict(
91 &self,
92 query_points: &[(f64, f64)],
93 ) -> Result<(Vec<f64>, Vec<f64>), RoboticsError> {
94 if query_points.is_empty() {
95 return Ok((Vec::new(), Vec::new()));
96 }
97 let Some(alpha) = self.alpha.as_ref() else {
98 return Err(RoboticsError::InvalidParameter(
99 "GaussianProcess: model must be fitted before predict".to_string(),
100 ));
101 };
102 let Some(k_inv) = self.k_inv.as_ref() else {
103 return Err(RoboticsError::InvalidParameter(
104 "GaussianProcess: inverse kernel state missing; call fit first".to_string(),
105 ));
106 };
107
108 let k_star = self.kernel_matrix(query_points, &self.train_xy); let mean_vec = &k_star * alpha; let mut means = Vec::with_capacity(query_points.len());
112 let mut variances = Vec::with_capacity(query_points.len());
113
114 for i in 0..query_points.len() {
115 let query = query_points[i];
116 let k_ss = self.kernel(query, query);
117 let k_row = k_star.row(i).transpose();
118 let variance = (k_ss - (k_row.transpose() * k_inv * k_row)[(0, 0)]).max(0.0);
119 means.push(mean_vec[i]);
120 variances.push(variance);
121 }
122
123 Ok((means, variances))
124 }
125
126 fn kernel(&self, a: (f64, f64), b: (f64, f64)) -> f64 {
127 let dx = a.0 - b.0;
128 let dy = a.1 - b.1;
129 let r2 = dx * dx + dy * dy;
130 self.config.signal_variance
131 * (-0.5 * r2 / (self.config.length_scale * self.config.length_scale)).exp()
132 }
133
134 fn kernel_matrix(&self, lhs: &[(f64, f64)], rhs: &[(f64, f64)]) -> DMatrix<f64> {
135 let mut matrix = DMatrix::zeros(lhs.len(), rhs.len());
136 for i in 0..lhs.len() {
137 for j in 0..rhs.len() {
138 matrix[(i, j)] = self.kernel(lhs[i], rhs[j]);
139 }
140 }
141 matrix
142 }
143}
144
145#[cfg(test)]
146mod tests {
147 use super::*;
148
149 #[test]
150 fn test_gaussian_process_config_defaults() {
151 let config = GaussianProcessConfig::default();
152 assert_eq!(config.length_scale, 1.0);
153 assert_eq!(config.signal_variance, 1.0);
154 assert_eq!(config.noise_variance, 0.01);
155 }
156
157 #[test]
158 fn test_gaussian_process_predicts_known_function() {
159 let mut gp = GaussianProcessRegressor::new(GaussianProcessConfig::default());
160 let train = vec![
161 (0.0, 0.0, 1.0),
162 (1.0, 0.0, 1.8414709848),
163 (0.0, 1.0, 0.5403023059),
164 (1.0, 1.0, 1.3817732907),
165 (0.5, 0.5, 1.3570081005),
166 ];
167 gp.fit(&train).expect("fit should succeed");
168
169 let query = vec![(0.5, 0.5), (0.8, 0.2)];
170 let (mean, _) = gp.predict(&query).expect("predict should succeed");
171
172 assert!((mean[0] - 1.3570081005).abs() < 0.1);
173 let expected = 0.8f64.sin() + 0.2f64.cos();
174 assert!((mean[1] - expected).abs() < 0.2);
175 }
176
177 #[test]
178 fn test_gaussian_process_uncertainty_increases_away_from_data() {
179 let mut gp = GaussianProcessRegressor::new(GaussianProcessConfig::default());
180 let train = vec![
181 (0.0, 0.0, 0.0),
182 (0.5, 0.0, 0.5),
183 (0.0, 0.5, 0.5),
184 (0.5, 0.5, 1.0),
185 ];
186 gp.fit(&train).expect("fit should succeed");
187
188 let (_, var_near) = gp.predict(&[(0.25, 0.25)]).expect("predict near");
189 let (_, var_far) = gp.predict(&[(3.0, 3.0)]).expect("predict far");
190
191 assert!(var_far[0] > var_near[0]);
192 }
193}