Yes, this was our main worry as well. At first we switched to Rust only for the lower level code, the parts that interfaces with sensors and the motors, that do the data logging, our inter process communication library, etc. The part with the control algorithms (where all the interesting linear algebra code is) was still in C++ until recently.
For all the small vectors and matrices, things like Vector3 and Matrix3x3 from nalgebra work fine. A few generic things we had to change from statically sized to dynamically sized arrays/matrices, but since these were rather big already, the difference in performance was neglegible. The type safety was maintained by directly wrapping those in a generic struct where the template parameter describes the contents of the matrix or vector. Such template parameter is basically a struct where the members describe the meaning of all the columns/rows of the related vector/matrix. A custom derive proc macro generated the conversions to and from this type (which are all optimized away by the compiler). It looks something like this:
struct Foo {
matrix: DynanamicallyAllocatedMatrix,
}
impl Foo {
fn new() {
Foo { matrix: DynanamicallyAllocatedMatrix::zeros(T::N, T::N) }
}
fn get_diagonal(&self) -> T { ... }
...
}
We were already doing something like this in C++, but with some hacky preprocessor macros instead of Rust's procedural macros.
In the end, keeping track of the meaning of the entire vector/matrix in the type system provides us with even a lot more safety than only keeping the size of them in the type system. And it has the advantage that you don't need const generics, since you're keeping track of whole types, and not just numbers. The downside is that we have to maintain a few procedural macro implementations.